更新工件定位
This commit is contained in:
parent
4ab2793a33
commit
45448801a3
@ -0,0 +1,796 @@
|
||||
// BQ_workpieceCornerExtract_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
|
||||
//
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <vector>
|
||||
#include <stdio.h>
|
||||
#include <VZNL_Types.h>
|
||||
#include "direct.h"
|
||||
#include <string>
|
||||
#include "BQ_workpieceCornerExtraction_Export.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <Windows.h>
|
||||
#include <limits>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int r;
|
||||
int g;
|
||||
int b;
|
||||
}SG_color;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int nPointIdx;
|
||||
double x;
|
||||
double y;
|
||||
double z;
|
||||
float r;
|
||||
float g;
|
||||
float b;
|
||||
} SPointXYZRGB;
|
||||
|
||||
void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
if (inputFile.is_open() == false)
|
||||
return;
|
||||
|
||||
std::vector< SVzNL3DPosition> a_line;
|
||||
int ptIdx = 0;
|
||||
while (getline(inputFile, linedata))
|
||||
{
|
||||
if (0 == strncmp("Line_", linedata.c_str(), 5))
|
||||
{
|
||||
int ptSize = (int)a_line.size();
|
||||
if (ptSize > 0)
|
||||
{
|
||||
scanData.push_back(a_line);
|
||||
}
|
||||
a_line.clear();
|
||||
ptIdx = 0;
|
||||
}
|
||||
else if (0 == strncmp("{", linedata.c_str(), 1))
|
||||
{
|
||||
float X, Y, Z;
|
||||
int imageY = 0;
|
||||
float leftX, leftY;
|
||||
float rightX, rightY;
|
||||
sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
|
||||
SVzNL3DPosition a_pt;
|
||||
a_pt.pt3D.x = X;
|
||||
a_pt.pt3D.y = Y;
|
||||
a_pt.pt3D.z = Z;
|
||||
a_pt.nPointIdx = ptIdx;
|
||||
ptIdx++;
|
||||
a_line.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
//last line
|
||||
int ptSize = (int)a_line.size();
|
||||
if (ptSize > 0)
|
||||
{
|
||||
scanData.push_back(a_line);
|
||||
a_line.clear();
|
||||
}
|
||||
|
||||
inputFile.close();
|
||||
return;
|
||||
}
|
||||
|
||||
void _convertToGridData_XYZ_vector(std::vector<std::vector< SVzNL3DPosition>>& scanData, double _F, std::vector<std::vector< SVzNL3DPosition>>& scanData_grid)
|
||||
{
|
||||
int min_y = 100000000;
|
||||
int max_y = -10000000;
|
||||
int lineNum = scanData.size();
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector< SVzNL3DPosition>& a_line = scanData[line];
|
||||
int nPointCnt = a_line.size();
|
||||
for (int i = 0; i < nPointCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition* a_pt = &scanData[line][i];
|
||||
if (a_pt->pt3D.z > 1e-4)
|
||||
{
|
||||
double v = _F * a_pt->pt3D.y / a_pt->pt3D.z + 2000;
|
||||
a_pt->nPointIdx = (int)(v + 0.5);
|
||||
max_y = max_y < (int)a_pt->nPointIdx ? (int)a_pt->nPointIdx : max_y;
|
||||
min_y = min_y > (int)a_pt->nPointIdx ? (int)a_pt->nPointIdx : min_y;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (min_y == 100000000)
|
||||
return;
|
||||
|
||||
int pt_counter = max_y - min_y + 1;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector< SVzNL3DPosition> gridData;
|
||||
gridData.resize(pt_counter);
|
||||
for (int i = 0; i < pt_counter; i++)
|
||||
gridData[i] = { 0,{ 0.0, 0.0, 0.0} };
|
||||
|
||||
std::vector< SVzNL3DPosition>& a_line = scanData[line];
|
||||
int nPointCnt = a_line.size();
|
||||
for (int i = 0; i < nPointCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition a_pt = a_line[i];
|
||||
if (a_pt.pt3D.z > 1e-4)
|
||||
{
|
||||
int pt_id = a_pt.nPointIdx - min_y;
|
||||
gridData[pt_id] = a_pt;
|
||||
}
|
||||
}
|
||||
scanData_grid.push_back(gridData);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void _outputScanDataFile_XYZ_vector(char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
int lineNum = scanData.size();
|
||||
sw << "LineNum:" << lineNum << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp: 0_0" << std::endl;
|
||||
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int nPositionCnt = scanData[line].size();
|
||||
sw << "Line_" << line << "_0_" << nPositionCnt << std::endl;
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanData[line][i];
|
||||
float x = (float)pt3D->pt3D.x;
|
||||
float y = (float)pt3D->pt3D.y;
|
||||
float z = (float)pt3D->pt3D.z;
|
||||
char str[250];
|
||||
sprintf_s(str, "{ %f, %f, %f } - { 0, 0 } - { 0, 0 }", x, y, z);
|
||||
|
||||
sw << str << std::endl;
|
||||
}
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _getRoiData_XYZ_vector(
|
||||
std::vector<std::vector< SVzNL3DPosition>>& scanData,
|
||||
std::vector<std::vector< SVzNL3DPosition>>& roiData,
|
||||
SVzNL3DRangeD roi)
|
||||
{
|
||||
int lineNum = scanData.size();
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int nPositionCnt = scanData[line].size();
|
||||
std::vector< SVzNL3DPosition> linePts;
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition pt3D = scanData[line][i];
|
||||
if ((pt3D.pt3D.z >= roi.zRange.min) &&
|
||||
(pt3D.pt3D.z <= roi.zRange.max) &&
|
||||
(pt3D.pt3D.y >= roi.yRange.min) &&
|
||||
(pt3D.pt3D.y <= roi.yRange.max))
|
||||
{
|
||||
linePts.push_back(pt3D);
|
||||
}
|
||||
}
|
||||
roiData.push_back(linePts);
|
||||
}
|
||||
}
|
||||
|
||||
void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
char dataStr[250];
|
||||
//调平矩阵
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[0], calibPara.planeCalib[1], calibPara.planeCalib[2]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[3], calibPara.planeCalib[4], calibPara.planeCalib[5]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[6], calibPara.planeCalib[7], calibPara.planeCalib[8]);
|
||||
sw << dataStr << std::endl;
|
||||
//地面高度
|
||||
sprintf_s(dataStr, 250, "%g", calibPara.planeHeight);
|
||||
sw << dataStr << std::endl;
|
||||
//反向旋转矩阵
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[0], calibPara.invRMatrix[1], calibPara.invRMatrix[2]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[3], calibPara.invRMatrix[4], calibPara.invRMatrix[5]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[6], calibPara.invRMatrix[7], calibPara.invRMatrix[8]);
|
||||
sw << dataStr << std::endl;
|
||||
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _outputCornerInfo(char* fileName, SSX_BQworkpieceResult workpieceCorner)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
char dataStr[250];
|
||||
sw << "L:" << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_0: (%g, %g, %g)", workpieceCorner.corner_L[0].x, workpieceCorner.corner_L[0].y, workpieceCorner.corner_L[0].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_1: (%g, %g, %g)", workpieceCorner.corner_L[1].x, workpieceCorner.corner_L[1].y, workpieceCorner.corner_L[1].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_2: (%g, %g, %g)", workpieceCorner.corner_L[2].x, workpieceCorner.corner_L[2].y, workpieceCorner.corner_L[2].z);
|
||||
sw << dataStr << std::endl;
|
||||
double dist = sqrt(pow(workpieceCorner.corner_L[0].x - workpieceCorner.corner_L[2].x, 2) +
|
||||
pow(workpieceCorner.corner_L[0].y - workpieceCorner.corner_L[2].y, 2) +
|
||||
pow(workpieceCorner.corner_L[0].z - workpieceCorner.corner_L[2].z, 2));
|
||||
sprintf_s(dataStr, 250, " Len: %g", dist);
|
||||
sw << dataStr << std::endl;
|
||||
|
||||
sw << "T:" << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_0: (%g, %g, %g)", workpieceCorner.corner_T[0].x, workpieceCorner.corner_T[0].y, workpieceCorner.corner_T[0].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_1: (%g, %g, %g)", workpieceCorner.corner_T[1].x, workpieceCorner.corner_T[1].y, workpieceCorner.corner_T[1].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_2: (%g, %g, %g)", workpieceCorner.corner_T[2].x, workpieceCorner.corner_T[2].y, workpieceCorner.corner_T[2].z);
|
||||
sw << dataStr << std::endl;
|
||||
dist = sqrt(pow(workpieceCorner.corner_T[0].x - workpieceCorner.corner_T[2].x, 2) +
|
||||
pow(workpieceCorner.corner_T[0].y - workpieceCorner.corner_T[2].y, 2) +
|
||||
pow(workpieceCorner.corner_T[0].z - workpieceCorner.corner_T[2].z, 2));
|
||||
sprintf_s(dataStr, 250, " Len: %g", dist);
|
||||
sw << dataStr << std::endl;
|
||||
|
||||
sw << "R:" << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_0: (%g, %g, %g)", workpieceCorner.corner_R[0].x, workpieceCorner.corner_R[0].y, workpieceCorner.corner_R[0].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_1: (%g, %g, %g)", workpieceCorner.corner_R[1].x, workpieceCorner.corner_R[1].y, workpieceCorner.corner_R[1].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_2: (%g, %g, %g)", workpieceCorner.corner_R[2].x, workpieceCorner.corner_R[2].y, workpieceCorner.corner_R[2].z);
|
||||
sw << dataStr << std::endl;
|
||||
dist = sqrt(pow(workpieceCorner.corner_R[0].x - workpieceCorner.corner_R[2].x, 2) +
|
||||
pow(workpieceCorner.corner_R[0].y - workpieceCorner.corner_R[2].y, 2) +
|
||||
pow(workpieceCorner.corner_R[0].z - workpieceCorner.corner_R[2].z, 2));
|
||||
sprintf_s(dataStr, 250, " Len: %g", dist);
|
||||
sw << dataStr << std::endl;
|
||||
|
||||
sw << "B:" << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_0: (%g, %g, %g)", workpieceCorner.corner_B[0].x, workpieceCorner.corner_B[0].y, workpieceCorner.corner_B[0].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_1: (%g, %g, %g)", workpieceCorner.corner_B[1].x, workpieceCorner.corner_B[1].y, workpieceCorner.corner_B[1].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_2: (%g, %g, %g)", workpieceCorner.corner_B[2].x, workpieceCorner.corner_B[2].y, workpieceCorner.corner_B[2].z);
|
||||
sw << dataStr << std::endl;
|
||||
dist = sqrt(pow(workpieceCorner.corner_B[0].x - workpieceCorner.corner_B[2].x, 2) +
|
||||
pow(workpieceCorner.corner_B[0].y - workpieceCorner.corner_B[2].y, 2) +
|
||||
pow(workpieceCorner.corner_B[0].z - workpieceCorner.corner_B[2].z, 2));
|
||||
sprintf_s(dataStr, 250, " Len: %g", dist);
|
||||
sw << dataStr << std::endl;
|
||||
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _outputScanDataFile_vector(char* fileName, std::vector<std::vector<SVzNL3DPosition>>& scanLines, bool removeZeros, int* headNullLines)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
int lineNum = (int)scanLines.size();
|
||||
if (lineNum == 0)
|
||||
return;
|
||||
|
||||
sw << "LineNum:" << lineNum << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp: 0_0" << std::endl;
|
||||
|
||||
int lineIdx = 0;
|
||||
int null_lines = 0;
|
||||
bool counterNull = true;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int linePtNum = (int)scanLines[line].size();
|
||||
if (linePtNum == 0)
|
||||
continue;
|
||||
|
||||
if (true == removeZeros)
|
||||
{
|
||||
int vldPtNum = 0;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
if (scanLines[line][i].pt3D.z > 1e-4)
|
||||
vldPtNum++;
|
||||
}
|
||||
linePtNum = vldPtNum;
|
||||
}
|
||||
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
|
||||
lineIdx++;
|
||||
bool isNull = true;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D;
|
||||
if ((pt3D->z > 1e-4) && (isNull == true))
|
||||
isNull = false;
|
||||
if ((true == removeZeros) && (pt3D->z < 1e-4))
|
||||
continue;
|
||||
float x = (float)pt3D->x;
|
||||
float y = (float)pt3D->y;
|
||||
float z = (float)pt3D->z;
|
||||
sw << "{ " << x << "," << y << "," << z << " }-";
|
||||
sw << "{0,0}-{0,0}" << std::endl;
|
||||
}
|
||||
if (true == counterNull)
|
||||
{
|
||||
if (true == isNull)
|
||||
null_lines++;
|
||||
else
|
||||
counterNull = false;
|
||||
}
|
||||
}
|
||||
*headNullLines = null_lines;
|
||||
sw.close();
|
||||
}
|
||||
|
||||
SSG_planeCalibPara _readCalibPara(char* fileName)
|
||||
{
|
||||
//设置初始结果
|
||||
double initCalib[9] = {
|
||||
1.0, 0.0, 0.0,
|
||||
0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 1.0 };
|
||||
SSG_planeCalibPara planePara;
|
||||
for (int i = 0; i < 9; i++)
|
||||
planePara.planeCalib[i] = initCalib[i];
|
||||
planePara.planeHeight = -1.0;
|
||||
for (int i = 0; i < 9; i++)
|
||||
planePara.invRMatrix[i] = initCalib[i];
|
||||
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
if (inputFile.is_open() == false)
|
||||
return planePara;
|
||||
|
||||
//调平矩阵
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]);
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]);
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]);
|
||||
//地面高度
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight);
|
||||
//反向旋转矩阵
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]);
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]);
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]);
|
||||
|
||||
inputFile.close();
|
||||
return planePara;
|
||||
}
|
||||
|
||||
void _outputRGBDScanLapWeld_RGBD(
|
||||
char* fileName,
|
||||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||||
SSX_BQworkpieceResult workpieceCorner,
|
||||
bool outDebugInfo,
|
||||
SSX_debugInfo* debugData)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
std::ofstream sw(fileName);
|
||||
int realLines = lineNum;
|
||||
if (workpieceCorner.workpieceType > 0)
|
||||
realLines++;
|
||||
if(debugData)
|
||||
realLines++;
|
||||
sw << "LineNum:" << realLines << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp: 0_0" << std::endl;
|
||||
|
||||
int maxLineIndex = 0;
|
||||
int max_stamp = 0;
|
||||
|
||||
SG_color rgb = { 0, 0, 0 };
|
||||
|
||||
SG_color objColor[8] = {
|
||||
{245,222,179},//淡黄色
|
||||
{210,105, 30},//巧克力色
|
||||
{240,230,140},//黄褐色
|
||||
{135,206,235},//天蓝色
|
||||
{250,235,215},//古董白
|
||||
{189,252,201},//薄荷色
|
||||
{221,160,221},//梅红色
|
||||
{188,143,143},//玫瑰红色
|
||||
};
|
||||
int size = 1;
|
||||
int lineIdx = 0;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int linePtNum = (int)scanLines[line].size();
|
||||
if (linePtNum == 0)
|
||||
continue;
|
||||
|
||||
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
|
||||
lineIdx++;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
if (pt3D->nPointIdx > 0)
|
||||
int kkk = 1;
|
||||
int featureType_v = pt3D->nPointIdx & 0xff;
|
||||
int featureType_h = featureType_v >> 4;
|
||||
featureType_v &= 0x0f;
|
||||
if (true == outDebugInfo)
|
||||
{
|
||||
if (LINE_FEATURE_L_JUMP_H2L == featureType_v)
|
||||
{
|
||||
rgb = { 255, 97, 0 };
|
||||
size = 5;
|
||||
}
|
||||
else if (LINE_FEATURE_L_JUMP_L2H == featureType_v)
|
||||
{
|
||||
rgb = objColor[7];
|
||||
size = 5;
|
||||
}
|
||||
else if (LINE_FEATURE_L_JUMP_H2L == featureType_h)
|
||||
{
|
||||
rgb = objColor[6];
|
||||
size = 5;
|
||||
}
|
||||
else if (LINE_FEATURE_L_JUMP_L2H == featureType_h)
|
||||
{
|
||||
rgb = { 97, 255, 0 };
|
||||
size = 5;
|
||||
}
|
||||
else
|
||||
{
|
||||
rgb = { 200, 200, 200 };
|
||||
size = 1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
rgb = { 200, 200, 200 };
|
||||
size = 1;
|
||||
}
|
||||
float x = (float)pt3D->pt3D.x;
|
||||
float y = (float)pt3D->pt3D.y;
|
||||
float z = (float)pt3D->pt3D.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
if (workpieceCorner.workpieceType > 0)
|
||||
{
|
||||
int linePtNum = 12;
|
||||
sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl;
|
||||
lineNum++;
|
||||
|
||||
SVzNL3DPoint ptBuffer[12];
|
||||
int idx = 0;
|
||||
for (int i = 0; i < 3; i++)
|
||||
ptBuffer[idx++] = workpieceCorner.corner_L[i];
|
||||
for (int i = 0; i < 3; i++)
|
||||
ptBuffer[idx++] = workpieceCorner.corner_R[i];
|
||||
for (int i = 0; i < 3; i++)
|
||||
ptBuffer[idx++] = workpieceCorner.corner_T[i];
|
||||
for (int i = 0; i < 3; i++)
|
||||
ptBuffer[idx++] = workpieceCorner.corner_B[i];
|
||||
|
||||
rgb = { 255, 0, 0 };
|
||||
size = 15;
|
||||
for (int j = 0; j < 12; j++)
|
||||
{
|
||||
float x = (float)ptBuffer[j].x;
|
||||
float y = (float)ptBuffer[j].y;
|
||||
float z = (float)ptBuffer[j].z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
//加一个点，用于跳过显示工具bug
|
||||
float x = (float)ptBuffer[0].x;
|
||||
float y = (float)ptBuffer[0].y;
|
||||
float z = (float)ptBuffer[0].z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
|
||||
if (debugData)
|
||||
{
|
||||
int linePtNum = debugData[0].edge_size + debugData[0].edgeLink1_size + debugData[0].edgeLink2_size;
|
||||
linePtNum += debugData[1].edge_size + debugData[1].edgeLink1_size + debugData[1].edgeLink2_size;
|
||||
linePtNum += debugData[2].edge_size + debugData[2].edgeLink1_size + debugData[2].edgeLink2_size;
|
||||
linePtNum += debugData[3].edge_size + debugData[3].edgeLink1_size + debugData[3].edgeLink2_size;
|
||||
sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl;
|
||||
lineNum++;
|
||||
|
||||
rgb = { 255, 0, 0 };
|
||||
size = 3;
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
for (int j = 0; j < debugData[i].edge_size; j++)
|
||||
{
|
||||
float x = (float)debugData[i].edge[j].x;
|
||||
float y = (float)debugData[i].edge[j].y;
|
||||
float z = (float)debugData[i].edge[j].z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
for (int j = 0; j < debugData[i].edgeLink1_size; j++)
|
||||
{
|
||||
float x = (float)debugData[i].edgeLink_1[j].x;
|
||||
float y = (float)debugData[i].edgeLink_1[j].y;
|
||||
float z = (float)debugData[i].edgeLink_1[j].z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
for (int j = 0; j < debugData[i].edgeLink2_size; j++)
|
||||
{
|
||||
float x = (float)debugData[i].edgeLink_2[j].x;
|
||||
float y = (float)debugData[i].edgeLink_2[j].y;
|
||||
float z = (float)debugData[i].edgeLink_2[j].z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
}
|
||||
//加一个点，用于跳过显示工具bug
|
||||
float x = (float)debugData[0].edge[0].x;
|
||||
float y = (float)debugData[0].edge[0].y;
|
||||
float z = (float)debugData[0].edge[0].z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
|
||||
//显示拟合直线
|
||||
rgb = { 255, 0, 0 };
|
||||
size = 3;
|
||||
int lineIdx = 0;
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
SVzNL3DPoint pt0 = debugData[i].edge_ends[0];
|
||||
SVzNL3DPoint pt1 = debugData[i].edge_ends[1];
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
|
||||
pt0 = debugData[i].edge_link1_ends[0];
|
||||
pt1 = debugData[i].edge_link1_ends[1];
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
|
||||
pt0 = debugData[i].edge_link2_ends[0];
|
||||
pt1 = debugData[i].edge_link2_ends[1];
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
}
|
||||
//加一个直线，用于跳过显示工具bug
|
||||
SVzNL3DPoint pt0 = debugData[0].edge_ends[0];
|
||||
SVzNL3DPoint pt1 = debugData[0].edge_ends[1];
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
#define CONVERT_TO_GRID 0
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
#define TEST_COMPUTE_CORNER 1
|
||||
#define TEST_GROUP 1
|
||||
int main()
|
||||
{
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\" //0
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{1,3}
|
||||
};
|
||||
|
||||
#if CONVERT_TO_GRID
|
||||
char _scan_dir[256];
|
||||
sprintf_s(_scan_dir, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\");
|
||||
char _scan_file[256];
|
||||
double _F = 1243.34;
|
||||
for (int fidx = 1; fidx <= 3; fidx++)
|
||||
{
|
||||
sprintf_s(_scan_file, "%sscanData_%d.txt", _scan_dir, fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanData;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData);
|
||||
if (scanData.size() == 0)
|
||||
continue;
|
||||
|
||||
std::vector<std::vector< SVzNL3DPosition>> gridData;
|
||||
_convertToGridData_XYZ_vector(scanData, _F, gridData);
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "%sscanData_%d_grid.txt", _scan_dir, fidx);
|
||||
_outputScanDataFile_XYZ_vector(_out_file, gridData);
|
||||
printf("%s: convert done!\n", _scan_file);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if TEST_COMPUTE_CALIB_PARA
|
||||
char _calib_datafile[256];
|
||||
sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\scanData_1.txt");
|
||||
int lineNum = 0;
|
||||
float lineV = 0.0f;
|
||||
int dataCalib = 0;
|
||||
int maxTimeStamp = 0;
|
||||
int clockPerSecond = 0;
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanData;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData);
|
||||
|
||||
|
||||
SVzNL3DRangeD roi;
|
||||
roi.xRange.min = -DBL_MAX;
|
||||
roi.xRange.max = DBL_MAX;
|
||||
roi.yRange.min = -DBL_MAX;
|
||||
roi.yRange.max = 580.0;
|
||||
roi.zRange.min = 2380.0;
|
||||
roi.zRange.max = 2460.0;
|
||||
|
||||
std::vector<std::vector< SVzNL3DPosition>> roiData;
|
||||
_getRoiData_XYZ_vector(scanData, roiData, roi);
|
||||
|
||||
lineNum = (int)scanData.size();
|
||||
if (scanData.size() > 0)
|
||||
{
|
||||
SSG_planeCalibPara calibPara = sx_BQ_getBaseCalibPara(scanData);
|
||||
//结果进行验证
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
if (i == 14)
|
||||
int kkk = 1;
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
sx_BQ_lineDataR(scanData[i], calibPara.planeCalib, -1); // calibPara.planeHeight);
|
||||
}
|
||||
//
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\ground_calib_para.txt");
|
||||
_outputCalibPara(calibFile, calibPara);
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\scanData_ground_1_calib.txt");
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_out_file, scanData, false, &headNullLines);
|
||||
printf("%s: calib done!\n", _calib_datafile);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if TEST_COMPUTE_CORNER
|
||||
for (int grp = 0; grp <= 0; grp++)
|
||||
{
|
||||
SSG_planeCalibPara poseCalibPara;
|
||||
//初始化成单位阵
|
||||
poseCalibPara.planeCalib[0] = 1.0;
|
||||
poseCalibPara.planeCalib[1] = 0.0;
|
||||
poseCalibPara.planeCalib[2] = 0.0;
|
||||
poseCalibPara.planeCalib[3] = 0.0;
|
||||
poseCalibPara.planeCalib[4] = 1.0;
|
||||
poseCalibPara.planeCalib[5] = 0.0;
|
||||
poseCalibPara.planeCalib[6] = 0.0;
|
||||
poseCalibPara.planeCalib[7] = 0.0;
|
||||
poseCalibPara.planeCalib[8] = 1.0;
|
||||
poseCalibPara.planeHeight = -1.0;
|
||||
for (int i = 0; i < 9; i++)
|
||||
poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
|
||||
char calibFile[250];
|
||||
if (grp == 0)
|
||||
{
|
||||
sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\ground_calib_para.txt");
|
||||
poseCalibPara = _readCalibPara(calibFile);
|
||||
}
|
||||
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =1;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||
|
||||
long t1 = (long)GetTickCount64();//统计时间
|
||||
|
||||
for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++)
|
||||
{
|
||||
if (i == 14)
|
||||
int kkk = 1;
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
sx_BQ_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1);
|
||||
}
|
||||
#if 0
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "%sscanData_%d_calib.txt", dataPath[grp], fidx);
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_out_file, scanLines, false, &headNullLines);
|
||||
#endif
|
||||
|
||||
SSG_cornerParam cornerParam;
|
||||
cornerParam.cornerTh = 60; //45度角
|
||||
cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4;
|
||||
cornerParam.minEndingGap_z = 20;
|
||||
cornerParam.jumpCornerTh_1 = 10; //水平角度，小于此角度视为水平
|
||||
cornerParam.jumpCornerTh_2 = 60;
|
||||
SSG_outlierFilterParam filterParam;
|
||||
filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
|
||||
filterParam.outlierTh = 5;
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.maxLineSkipNum = 10;
|
||||
growParam.yDeviation_max = 10.0;
|
||||
growParam.maxSkipDistance = 10.0;
|
||||
growParam.zDeviation_max = 10.0;// algoParam.bagParam.bagH / 2; //袋子高度1/2
|
||||
growParam.minLTypeTreeLen = 100; //mm
|
||||
growParam.minVTypeTreeLen = 100; //mm
|
||||
SSX_BQworkpiecePara workpieceParam;
|
||||
workpieceParam.lineLen = 180.0; //直线段长度
|
||||
int errCode = 0;
|
||||
SSX_debugInfo debug_conturs[4];
|
||||
SSX_BQworkpieceResult workpieceCorner = sx_BQ_getWorkpieceCorners(
|
||||
scanLines,
|
||||
cornerParam,
|
||||
filterParam,
|
||||
growParam,
|
||||
poseCalibPara,
|
||||
workpieceParam,
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
debug_conturs,
|
||||
#endif
|
||||
&errCode);
|
||||
long t2 = (long)GetTickCount64();
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
//输出测试结果
|
||||
sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
_outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, false, debug_conturs);
|
||||
#else
|
||||
_outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, true, NULL);
|
||||
#endif
|
||||
sprintf_s(calibFile, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx);
|
||||
_outputCornerInfo(calibFile, workpieceCorner);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
|
||||
// 调试程序: F5 或调试 >“开始调试”菜单
|
||||
|
||||
// 入门使用技巧:
|
||||
// 1. 使用解决方案资源管理器窗口添加/管理文件
|
||||
// 2. 使用团队资源管理器窗口连接到源代码管理
|
||||
// 3. 使用输出窗口查看生成输出和其他消息
|
||||
// 4. 使用错误列表窗口查看错误
|
||||
// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
|
||||
// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件
|
||||
@ -0,0 +1,159 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup Label="ProjectConfigurations">
|
||||
<ProjectConfiguration Include="Debug|Win32">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|Win32">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Debug|x64">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|x64">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
</ItemGroup>
|
||||
<PropertyGroup Label="Globals">
|
||||
<VCProjectVersion>16.0</VCProjectVersion>
|
||||
<Keyword>Win32Proj</Keyword>
|
||||
<ProjectGuid>{cf563709-0402-447e-bfcc-7701cc90d0af}</ProjectGuid>
|
||||
<RootNamespace>BQworkpieceCornerExtracttest</RootNamespace>
|
||||
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||
<ImportGroup Label="ExtensionSettings">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="Shared">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<PropertyGroup Label="UserMacros" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;</AdditionalIncludeDirectories>
|
||||
<AdditionalOptions>/D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions)</AdditionalOptions>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world320d.lib;BQ_workpieceCornerExtraction.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;</AdditionalIncludeDirectories>
|
||||
<AdditionalOptions>/D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions)</AdditionalOptions>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world320.lib;BQ_workpieceCornerExtraction.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="BQ_workpieceCornerExtract_test.cpp" />
|
||||
</ItemGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||
<ImportGroup Label="ExtensionTargets">
|
||||
</ImportGroup>
|
||||
</Project>
|
||||
@ -0,0 +1,174 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup Label="ProjectConfigurations">
|
||||
<ProjectConfiguration Include="Debug|Win32">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|Win32">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Debug|x64">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|x64">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
</ItemGroup>
|
||||
<PropertyGroup Label="Globals">
|
||||
<VCProjectVersion>16.0</VCProjectVersion>
|
||||
<Keyword>Win32Proj</Keyword>
|
||||
<ProjectGuid>{ad8415b7-a745-4184-87b8-95619e5066d6}</ProjectGuid>
|
||||
<RootNamespace>BQworkpieceCornerExtraction</RootNamespace>
|
||||
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||
<ImportGroup Label="ExtensionSettings">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="Shared">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<PropertyGroup Label="UserMacros" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>WIN32;_DEBUG;BQWORKPIECECORNEREXTRACTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>Use</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Windows</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableUAC>false</EnableUAC>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>WIN32;NDEBUG;BQWORKPIECECORNEREXTRACTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>Use</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Windows</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableUAC>false</EnableUAC>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>_DEBUG;BQWORKPIECECORNEREXTRACTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||
<AdditionalOptions>/D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions)</AdditionalOptions>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Windows</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableUAC>false</EnableUAC>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world320d.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>NDEBUG;BQWORKPIECECORNEREXTRACTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||
<AdditionalOptions>/D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions)</AdditionalOptions>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Windows</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableUAC>false</EnableUAC>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world320.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\sourceCode\BQ_workpieceCornerExtraction.cpp" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClInclude Include="..\sourceCode\BQ_workpieceCornerExtraction_Export.h" />
|
||||
</ItemGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||
<ImportGroup Label="ExtensionTargets">
|
||||
</ImportGroup>
|
||||
</Project>
|
||||
@ -36,9 +36,10 @@ ADD_LIBRARY(bagPositioning SHARED sourceCode/SG_bagPositioning.cpp)
|
||||
|
||||
ADD_LIBRARY(lapWeldDetection SHARED sourceCode/SX_lapWeldDetection.cpp)
|
||||
|
||||
|
||||
ADD_LIBRARY(beltTearingDetection SHARED sourceCode/beltTearingDetection.cpp)
|
||||
|
||||
ADD_LIBRARY(workpieceCornerExtraction SHARED sourceCode/BQ_workpieceCornerExtraction.cpp)
|
||||
|
||||
#add executable file
|
||||
# ADD_EXECUTABLE(bagPositioning_test bagPositioning_test/bagPositioning_test.cpp)
|
||||
|
||||
|
||||
673
sourceCode/BQ_workpieceCornerExtraction.cpp
Normal file
673
sourceCode/BQ_workpieceCornerExtraction.cpp
Normal file
@ -0,0 +1,673 @@
|
||||
#include <vector>
|
||||
#include "SG_baseDataType.h"
|
||||
#include "SG_baseAlgo_Export.h"
|
||||
#include "BQ_workpieceCornerExtraction_Export.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <limits>
|
||||
|
||||
//计算一个平面调平参数。
|
||||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
SSG_planeCalibPara sx_BQ_getBaseCalibPara(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||||
{
|
||||
return sg_getPlaneCalibPara2(scanLines);
|
||||
}
|
||||
|
||||
//相机姿态调平，并去除地面
|
||||
void sx_BQ_lineDataR(
|
||||
std::vector< SVzNL3DPosition>& a_line,
|
||||
const double* camPoseR,
|
||||
double groundH)
|
||||
{
|
||||
lineDataRT_vector(a_line, camPoseR, groundH);
|
||||
}
|
||||
|
||||
SVzNL3DPoint _translatePoint(SVzNL3DPoint point, double rMatrix[9])
|
||||
{
|
||||
SVzNL3DPoint result;
|
||||
double x = point.x * rMatrix[0] + point.y * rMatrix[1] + point.z * rMatrix[2];
|
||||
double y = point.x * rMatrix[3] + point.y * rMatrix[4] + point.z * rMatrix[5];
|
||||
double z = point.x * rMatrix[6] + point.y * rMatrix[7] + point.z * rMatrix[8];
|
||||
result.x = x;
|
||||
result.y = y;
|
||||
result.z = z;
|
||||
return result;
|
||||
}
|
||||
|
||||
//获取生长树的ROI
|
||||
void sg_getTreeROI(SSG_featureTree* a_tree)
|
||||
{
|
||||
if (a_tree->treeNodes.size() == 0)
|
||||
{
|
||||
a_tree->roi.left = 0;
|
||||
a_tree->roi.right = 0;
|
||||
a_tree->roi.top = 0;
|
||||
a_tree->roi.bottom = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
a_tree->roi.left = a_tree->treeNodes[0].jumpPos.x;
|
||||
a_tree->roi.right = a_tree->treeNodes[0].jumpPos.x;
|
||||
a_tree->roi.top = a_tree->treeNodes[0].jumpPos.y;
|
||||
a_tree->roi.bottom = a_tree->treeNodes[0].jumpPos.y;
|
||||
for (int i = 1, i_max = a_tree->treeNodes.size(); i < i_max; i++)
|
||||
{
|
||||
if (a_tree->roi.left > a_tree->treeNodes[i].jumpPos.x)
|
||||
a_tree->roi.left = a_tree->treeNodes[i].jumpPos.x;
|
||||
if (a_tree->roi.right < a_tree->treeNodes[i].jumpPos.x)
|
||||
a_tree->roi.right = a_tree->treeNodes[i].jumpPos.x;
|
||||
if (a_tree->roi.top > a_tree->treeNodes[i].jumpPos.y)
|
||||
a_tree->roi.top = a_tree->treeNodes[i].jumpPos.y;
|
||||
if (a_tree->roi.bottom < a_tree->treeNodes[i].jumpPos.y)
|
||||
a_tree->roi.bottom = a_tree->treeNodes[i].jumpPos.y;
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void _getEdgeContour(SSG_featureTree* a_tree, std::vector<SVzNL3DPoint>& contour, std::vector< std::vector<SVzNL3DPosition>>& scanLines, bool isVScan)
|
||||
{
|
||||
for (int j = 0, j_max = (int)a_tree->treeNodes.size(); j < j_max; j++)
|
||||
{
|
||||
SSG_basicFeature1D* a_feature = &a_tree->treeNodes[j];
|
||||
SVzNL3DPoint a_pt;
|
||||
if (true == isVScan)
|
||||
a_pt = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D;
|
||||
else
|
||||
a_pt = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D;
|
||||
if (a_pt.z > 1e-4)//虚假目标过滤后点会置0
|
||||
{
|
||||
contour.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int _getPointClosestContour(std::vector<SSG_featureTree> trees, bool isVscanTrees, SVzNL3DPoint seedPt, std::vector< std::vector<SVzNL3DPosition>>& scanLines, bool fromHead)
|
||||
{
|
||||
double minDist = -1.0;
|
||||
int idx = -1;
|
||||
for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
|
||||
{
|
||||
SSG_basicFeature1D a_feature;
|
||||
if (true == fromHead)
|
||||
a_feature = trees[i].treeNodes[0];
|
||||
else
|
||||
a_feature = trees[i].treeNodes.back();
|
||||
|
||||
SVzNL3DPoint a_pt;
|
||||
if (true == isVscanTrees)
|
||||
a_pt = scanLines[a_feature.jumpPos2D.x][a_feature.jumpPos2D.y].pt3D;
|
||||
else
|
||||
a_pt = scanLines[a_feature.jumpPos2D.y][a_feature.jumpPos2D.x].pt3D;
|
||||
|
||||
double dist = sqrt(pow(a_pt.x - seedPt.x, 2) + pow(a_pt.y - seedPt.y, 2));
|
||||
if (minDist < 0)
|
||||
{
|
||||
minDist = dist;
|
||||
idx = i;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(dist < minDist)
|
||||
{
|
||||
minDist = dist;
|
||||
idx = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
return idx;
|
||||
}
|
||||
|
||||
void _getEdgeLinkingContour(SSG_featureTree* a_tree, bool isVScanTree, SVzNL3DPoint seedPt, std::vector<SVzNL3DPoint>& contour, std::vector< std::vector<SVzNL3DPosition>>& scanLines, bool fromHead, double lineLen)
|
||||
{
|
||||
for (int i = 0, i_max = (int)a_tree->treeNodes.size(); i < i_max; i++)
|
||||
{
|
||||
int idx = i;
|
||||
if (false == fromHead)
|
||||
idx = i_max - 1 - i;
|
||||
SSG_basicFeature1D* a_feature = &a_tree->treeNodes[idx];
|
||||
|
||||
SVzNL3DPoint a_pt;
|
||||
if (true == isVScanTree)
|
||||
a_pt = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D;
|
||||
else
|
||||
a_pt = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D;
|
||||
if (a_pt.z > 1e-4)//虚假目标过滤后点会置0
|
||||
{
|
||||
double dist = sqrt(pow(a_pt.x - seedPt.x, 2) + pow(a_pt.y - seedPt.y, 2));
|
||||
if (dist > lineLen)
|
||||
break;
|
||||
contour.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int rgnIdx;
|
||||
std::vector<SVzNL3DPoint> edge;
|
||||
SVzNL3DPoint edge_ends[2];
|
||||
std::vector<SVzNL3DPoint> edgeLink_1;
|
||||
SVzNL3DPoint edge_link1_ends[2];
|
||||
std::vector<SVzNL3DPoint> edgeLink_2;
|
||||
SVzNL3DPoint edge_link2_ends[2];
|
||||
}SSX_featureContour;
|
||||
|
||||
SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_cornerParam cornerPara,
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
SSG_treeGrowParam growParam,
|
||||
SSG_planeCalibPara groundCalibPara,
|
||||
SSX_BQworkpiecePara workpieceParam,
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
SSX_debugInfo* debug_conturs,
|
||||
#endif
|
||||
int* errCode)
|
||||
{
|
||||
*errCode = 0;
|
||||
SSX_BQworkpieceResult workpieceCorners;
|
||||
memset(&workpieceCorners, 0, sizeof(SSX_BQworkpieceResult));
|
||||
int lineNum = (int)scanLines.size();
|
||||
if (lineNum == 0)
|
||||
{
|
||||
*errCode = SG_ERR_3D_DATA_NULL;
|
||||
return workpieceCorners;
|
||||
}
|
||||
|
||||
int linePtNum = (int)scanLines[0].size();
|
||||
bool isGridData = true;
|
||||
|
||||
//垂直跳变特征提取
|
||||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (line == 202)
|
||||
int kkk = 1;
|
||||
|
||||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||||
if (linePtNum != (int)lineData.size())
|
||||
isGridData = false;
|
||||
|
||||
//滤波，滤除异常点
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||||
|
||||
std::vector<SSG_basicFeature1D> line_features;
|
||||
int dataSize = (int)lineData.size();
|
||||
sg_getLineCornerFeature_BQ(
|
||||
&lineData[0],
|
||||
dataSize,
|
||||
line,
|
||||
groundCalibPara.planeHeight,
|
||||
cornerPara, //scale通常取bagH的1/4
|
||||
line_features);
|
||||
jumpFeatures_v.push_back(line_features);
|
||||
}
|
||||
|
||||
if (false == isGridData)//数据不是网格格式
|
||||
{
|
||||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||||
return workpieceCorners;
|
||||
}
|
||||
|
||||
//生成水平扫描
|
||||
std::vector<std::vector<SVzNL3DPosition>> hLines;
|
||||
hLines.resize(linePtNum);
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
hLines[i].resize(lineNum);
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||||
hLines[j][line] = scanLines[line][j];
|
||||
hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||||
hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||||
}
|
||||
}
|
||||
//水平arc特征提取
|
||||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h;
|
||||
int lineNum_h = (int)hLines.size();
|
||||
for (int line = 0; line < lineNum_h; line++)
|
||||
{
|
||||
if (line == 416)
|
||||
int kkk = 1;
|
||||
std::vector<SVzNL3DPosition>& lineData = hLines[line];
|
||||
//滤波，滤除异常点
|
||||
int ptNum = (int)lineData.size();
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);
|
||||
|
||||
std::vector<SSG_basicFeature1D> line_features;
|
||||
int dataSize = (int)lineData.size();
|
||||
sg_getLineCornerFeature_BQ(
|
||||
&hLines[line][0],
|
||||
dataSize,
|
||||
line,
|
||||
groundCalibPara.planeHeight,
|
||||
cornerPara, //scale通常取bagH的1/4
|
||||
line_features);
|
||||
jumpFeatures_h.push_back(line_features);
|
||||
}
|
||||
|
||||
//特征生长
|
||||
//垂直方向特征生长（激光线方向）
|
||||
std::vector<SSG_featureTree> v_trees;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
bool isLastLine = false;
|
||||
if (line == lineNum - 1)
|
||||
isLastLine = true;
|
||||
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_v[line];
|
||||
if (a_lineJumpFeature.size() > 0)
|
||||
int kkk = 1;
|
||||
if (line == 202)
|
||||
int kkk = 1;
|
||||
sg_lineFeaturesGrowing(
|
||||
line,
|
||||
isLastLine,
|
||||
a_lineJumpFeature,
|
||||
v_trees,
|
||||
growParam);
|
||||
}
|
||||
|
||||
//水平方向特征生长（扫描运动方向）
|
||||
std::vector<SSG_featureTree> h_trees;
|
||||
for (int line = 0; line < lineNum_h; line++)
|
||||
{
|
||||
if (line == 650)
|
||||
int kkk = 1;
|
||||
bool isLastLine = false;
|
||||
if (line == lineNum_h - 1)
|
||||
isLastLine = true;
|
||||
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h[line];
|
||||
sg_lineFeaturesGrowing(
|
||||
line,
|
||||
isLastLine,
|
||||
a_lineJumpFeature,
|
||||
h_trees,
|
||||
growParam);
|
||||
}
|
||||
|
||||
//tree信息
|
||||
std::vector<SSG_treeInfo> allTreesInfo; //不包含边界
|
||||
SSG_treeInfo a_nullTree;
|
||||
memset(&a_nullTree, 0, sizeof(SSG_treeInfo));
|
||||
allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引
|
||||
//标记，根据起点的生长树进行标注
|
||||
int hvTreeIdx = 1;
|
||||
for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
|
||||
{
|
||||
SSG_featureTree* a_vTree = &v_trees[i];
|
||||
sg_getTreeROI(a_vTree);
|
||||
//记录Tree的信息
|
||||
SSG_treeInfo a_treeInfo;
|
||||
a_treeInfo.vTreeFlag = 1;
|
||||
a_treeInfo.treeIdx = hvTreeIdx;
|
||||
a_treeInfo.treeType = a_vTree->treeType;
|
||||
a_treeInfo.sLineIdx = a_vTree->sLineIdx;
|
||||
a_treeInfo.eLineIdx = a_vTree->eLineIdx;
|
||||
a_treeInfo.roi = a_vTree->roi;
|
||||
allTreesInfo.push_back(a_treeInfo);
|
||||
|
||||
std::vector<SVzNL3DPoint> a_weld_contour;
|
||||
//在原始点云上标记，同时有Mask上标记
|
||||
for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
|
||||
{
|
||||
SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j];
|
||||
if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置0
|
||||
{
|
||||
int existEdgeId = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx >> 16;
|
||||
if (existEdgeId == 0)
|
||||
{
|
||||
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
|
||||
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
|
||||
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16;
|
||||
}
|
||||
a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D);
|
||||
}
|
||||
}
|
||||
hvTreeIdx++;
|
||||
}
|
||||
int hTreeStart = hvTreeIdx;
|
||||
////标注:水平特征
|
||||
for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
|
||||
{
|
||||
SSG_featureTree* a_hTree = &h_trees[i];
|
||||
sg_getTreeROI(a_hTree);
|
||||
//记录Tree的信息
|
||||
SSG_treeInfo a_treeInfo;
|
||||
a_treeInfo.vTreeFlag = 0;
|
||||
a_treeInfo.treeIdx = hvTreeIdx;
|
||||
a_treeInfo.treeType = a_hTree->treeType;
|
||||
a_treeInfo.sLineIdx = a_hTree->sLineIdx;
|
||||
a_treeInfo.eLineIdx = a_hTree->eLineIdx;
|
||||
a_treeInfo.roi.left = a_hTree->roi.top; //水平扫描xy是交换的
|
||||
a_treeInfo.roi.right = a_hTree->roi.bottom;
|
||||
a_treeInfo.roi.top = a_hTree->roi.left;
|
||||
a_treeInfo.roi.bottom = a_hTree->roi.right;
|
||||
allTreesInfo.push_back(a_treeInfo);
|
||||
|
||||
std::vector<SVzNL3DPoint> a_weld_contour;
|
||||
//在原始点云上标记，同时有Mask上标记
|
||||
for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
|
||||
{
|
||||
SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j];
|
||||
if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置0
|
||||
{
|
||||
int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16;
|
||||
if (existEdgeId == 0)
|
||||
{
|
||||
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
|
||||
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
|
||||
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16;
|
||||
}
|
||||
a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D);
|
||||
}
|
||||
}
|
||||
hvTreeIdx++;
|
||||
}
|
||||
int hvTreeSize = hvTreeIdx;
|
||||
|
||||
if(v_trees.size() < 2)
|
||||
{
|
||||
*errCode = SX_ERR_INVLD_VTREE_NUM;
|
||||
return workpieceCorners;
|
||||
}
|
||||
if (h_trees.size() < 2)
|
||||
{
|
||||
*errCode = SX_ERR_INVLD_HTREE_NUM;
|
||||
return workpieceCorners;
|
||||
}
|
||||
|
||||
//寻找vTree的最上和最下
|
||||
int vTree_T = 0;
|
||||
int vTree_B = 0;
|
||||
for (int i = 1, i_max = (int)v_trees.size(); i < i_max; i++)
|
||||
{
|
||||
if (v_trees[i].roi.top < v_trees[vTree_T].roi.top)
|
||||
vTree_T = i;
|
||||
if (v_trees[i].roi.bottom > v_trees[vTree_B].roi.bottom)
|
||||
vTree_B = i;
|
||||
}
|
||||
|
||||
//寻找hTree的最左和最右
|
||||
int hTree_L = 0;
|
||||
int hTree_R = 0;
|
||||
for (int i = 1, i_max = (int)h_trees.size(); i < i_max; i++)
|
||||
{
|
||||
//水平扫描xy是交换的，左右对应ROI的topBottom
|
||||
if (h_trees[i].roi.top < h_trees[hTree_L].roi.top)
|
||||
hTree_L = i;
|
||||
if (h_trees[i].roi.bottom > h_trees[hTree_R].roi.bottom)
|
||||
hTree_R = i;
|
||||
}
|
||||
|
||||
SSX_featureContour region[4];
|
||||
region[0].rgnIdx = 0; //Left
|
||||
_getEdgeContour(&h_trees[hTree_L], region[0].edge, scanLines,false);
|
||||
//寻找对应的两边
|
||||
SVzNL3DPoint firstPt = region[0].edge[0];
|
||||
SVzNL3DPoint lastPt = region[0].edge.back();
|
||||
int idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, true);
|
||||
_getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[0].edgeLink_1, scanLines, true, workpieceParam.lineLen);
|
||||
int idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, true);
|
||||
_getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[0].edgeLink_2, scanLines, true, workpieceParam.lineLen);
|
||||
|
||||
region[1].rgnIdx = 1; //Top
|
||||
_getEdgeContour(&v_trees[vTree_T], region[1].edge, scanLines, true);
|
||||
//寻找对应的两边
|
||||
firstPt = region[1].edge[0];
|
||||
lastPt = region[1].edge.back();
|
||||
idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true);
|
||||
_getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[1].edgeLink_1, scanLines, true, workpieceParam.lineLen);
|
||||
idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true);
|
||||
_getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[1].edgeLink_2, scanLines, true, workpieceParam.lineLen);
|
||||
|
||||
region[2].rgnIdx = 2; //Right
|
||||
_getEdgeContour(&h_trees[hTree_R], region[2].edge, scanLines, false);
|
||||
//寻找对应的两边
|
||||
firstPt = region[2].edge[0];
|
||||
lastPt = region[2].edge.back();
|
||||
idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, false);
|
||||
_getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[2].edgeLink_1, scanLines, false, workpieceParam.lineLen);
|
||||
idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, false);
|
||||
_getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[2].edgeLink_2, scanLines, false, workpieceParam.lineLen);
|
||||
|
||||
region[3].rgnIdx = 3; //Bottom
|
||||
_getEdgeContour(&v_trees[vTree_B], region[3].edge, scanLines, true);
|
||||
//寻找对应的两边
|
||||
firstPt = region[3].edge[0];
|
||||
lastPt = region[3].edge.back();
|
||||
idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true);
|
||||
_getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[3].edgeLink_1, scanLines, false, workpieceParam.lineLen);
|
||||
idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true);
|
||||
_getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[3].edgeLink_2, scanLines, false, workpieceParam.lineLen);
|
||||
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
if ((i == 0) || (i == 2))
|
||||
{
|
||||
//Left:防止垂直直线，使用x=ky+b
|
||||
std::vector<SVzNL3DPoint> transPts;
|
||||
for (int m = 0, m_max = (int)region[i].edge.size(); m < m_max; m++)
|
||||
{
|
||||
SVzNL3DPoint a_pt;
|
||||
a_pt.x = region[i].edge[m].y;
|
||||
a_pt.y = region[i].edge[m].x;
|
||||
a_pt.z = region[i].edge[m].z;
|
||||
transPts.push_back(a_pt);
|
||||
}
|
||||
//拟合测量
|
||||
double edge_x_k, edge_x_b;
|
||||
lineFitting(transPts, &edge_x_k, &edge_x_b);
|
||||
//计算拟合直线端点
|
||||
SVzNL3DPoint end_0 = transPts[0];
|
||||
SVzNL3DPoint end_1 = transPts.back();
|
||||
SVzNL2DPointD foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_x_k, edge_x_b);
|
||||
SVzNL2DPointD foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_x_k, edge_x_b);
|
||||
region[i].edge_ends[0] = { foot_0.y, foot_0.x, groundCalibPara.planeHeight };
|
||||
region[i].edge_ends[1] = { foot_1.y, foot_1.x, groundCalibPara.planeHeight };
|
||||
//两侧Linking直线使用 y=kx+b
|
||||
double edge_link1_k, edge_link1_b;
|
||||
lineFitting(region[i].edgeLink_1, &edge_link1_k, &edge_link1_b);
|
||||
end_0 = region[i].edgeLink_1[0];
|
||||
end_1 = region[i].edgeLink_1.back();
|
||||
foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_link1_k, edge_link1_b);
|
||||
foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link1_k, edge_link1_b);
|
||||
region[i].edge_link1_ends[0] = { foot_0.x, foot_0.y, groundCalibPara.planeHeight };
|
||||
region[i].edge_link1_ends[1] = { foot_1.x, foot_1.y, groundCalibPara.planeHeight };
|
||||
|
||||
double edge_link2_k, edge_link2_b;
|
||||
lineFitting(region[i].edgeLink_2, &edge_link2_k, &edge_link2_b);
|
||||
end_0 = region[i].edgeLink_2[0];
|
||||
end_1 = region[i].edgeLink_2.back();
|
||||
foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_link2_k, edge_link2_b);
|
||||
foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link2_k, edge_link2_b);
|
||||
region[i].edge_link2_ends[0] = { foot_0.x, foot_0.y, groundCalibPara.planeHeight };
|
||||
region[i].edge_link2_ends[1] = { foot_1.x, foot_1.y, groundCalibPara.planeHeight };
|
||||
|
||||
//计算交点
|
||||
end_0 = region[i].edge[0];
|
||||
end_1 = region[i].edge.back();
|
||||
SVzNL3DPoint crossPt[3];
|
||||
crossPt[0].x = (edge_x_k * edge_link1_b + edge_x_b) / (1.0 - edge_x_k * edge_link1_k);
|
||||
crossPt[0].y = edge_link1_k * crossPt[0].x + edge_link1_b;
|
||||
crossPt[0].z = groundCalibPara.planeHeight;
|
||||
crossPt[2].x = (edge_x_k * edge_link2_b + edge_x_b) / (1.0 - edge_x_k * edge_link2_k);
|
||||
crossPt[2].y = edge_link2_k * crossPt[2].x + edge_link2_b;
|
||||
crossPt[2].z = groundCalibPara.planeHeight;
|
||||
crossPt[1].x = (crossPt[0].x + crossPt[2].x) / 2;
|
||||
crossPt[1].y = (crossPt[0].y + crossPt[2].y) / 2;
|
||||
crossPt[1].z = groundCalibPara.planeHeight;
|
||||
if (i == 0)
|
||||
{
|
||||
for (int m = 0; m < 3; m++)
|
||||
workpieceCorners.corner_L[m] = crossPt[m];
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int m = 0; m < 3; m++)
|
||||
workpieceCorners.corner_R[m] = crossPt[m];
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
//拟合测量
|
||||
double edge_k, edge_b;
|
||||
lineFitting(region[i].edge, &edge_k, &edge_b);
|
||||
SVzNL3DPoint end_0 = region[i].edge[0];
|
||||
SVzNL3DPoint end_1 = region[i].edge.back();
|
||||
SVzNL2DPointD foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_k, edge_b);
|
||||
SVzNL2DPointD foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_k, edge_b);
|
||||
region[i].edge_ends[0] = { foot_0.x, foot_0.y, groundCalibPara.planeHeight };
|
||||
region[i].edge_ends[1] = { foot_1.x, foot_1.y, groundCalibPara.planeHeight };
|
||||
//防止垂直直线，使用x=ky+b
|
||||
std::vector<SVzNL3DPoint> transPts_link1;
|
||||
for (int m = 0, m_max = (int)region[i].edgeLink_1.size(); m < m_max; m++)
|
||||
{
|
||||
SVzNL3DPoint a_pt;
|
||||
a_pt.x = region[i].edgeLink_1[m].y;
|
||||
a_pt.y = region[i].edgeLink_1[m].x;
|
||||
a_pt.z = region[i].edgeLink_1[m].z;
|
||||
transPts_link1.push_back(a_pt);
|
||||
}
|
||||
double edge_link1_kx, edge_link1_bx;
|
||||
lineFitting(transPts_link1, &edge_link1_kx, &edge_link1_bx);
|
||||
//计算拟合直线端点
|
||||
end_0 = transPts_link1[0];
|
||||
end_1 = transPts_link1.back();
|
||||
foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_link1_kx, edge_link1_bx);
|
||||
foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link1_kx, edge_link1_bx);
|
||||
region[i].edge_link1_ends[0] = { foot_0.y, foot_0.x, groundCalibPara.planeHeight };
|
||||
region[i].edge_link1_ends[1] = { foot_1.y, foot_1.x, groundCalibPara.planeHeight };
|
||||
//两侧Linking直线使用 y=kx+b
|
||||
std::vector<SVzNL3DPoint> transPts_link2;
|
||||
for (int m = 0, m_max = (int)region[i].edgeLink_2.size(); m < m_max; m++)
|
||||
{
|
||||
SVzNL3DPoint a_pt;
|
||||
a_pt.x = region[i].edgeLink_2[m].y;
|
||||
a_pt.y = region[i].edgeLink_2[m].x;
|
||||
a_pt.z = region[i].edgeLink_2[m].z;
|
||||
transPts_link2.push_back(a_pt);
|
||||
}
|
||||
double edge_link2_kx, edge_link2_bx;
|
||||
lineFitting(transPts_link2, &edge_link2_kx, &edge_link2_bx);
|
||||
end_0 = transPts_link2[0];
|
||||
end_1 = transPts_link2.back();
|
||||
foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_link2_kx, edge_link2_bx);
|
||||
foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link2_kx, edge_link2_bx);
|
||||
region[i].edge_link2_ends[0] = { foot_0.y, foot_0.x, groundCalibPara.planeHeight };
|
||||
region[i].edge_link2_ends[1] = { foot_1.y, foot_1.x, groundCalibPara.planeHeight };
|
||||
|
||||
//计算交点
|
||||
end_0 = region[i].edge[0];
|
||||
end_1 = region[i].edge.back();
|
||||
SVzNL3DPoint crossPt[3];
|
||||
crossPt[0].x = (edge_link1_kx * edge_b + edge_link1_bx) / (1.0 - edge_link1_kx * edge_k);
|
||||
crossPt[0].y = edge_k * crossPt[0].x + edge_b;
|
||||
crossPt[0].z = groundCalibPara.planeHeight;
|
||||
crossPt[2].x = (edge_link2_kx * edge_b + edge_link2_bx) / (1.0 - edge_link2_kx * edge_k);
|
||||
crossPt[2].y = edge_k * crossPt[2].x + edge_b;
|
||||
crossPt[2].z = groundCalibPara.planeHeight;
|
||||
crossPt[1].x = (crossPt[0].x + crossPt[2].x) / 2;
|
||||
crossPt[1].y = (crossPt[0].y + crossPt[2].y) / 2;
|
||||
crossPt[1].z = groundCalibPara.planeHeight;
|
||||
if (i == 1)
|
||||
{
|
||||
for (int m = 0; m < 3; m++)
|
||||
workpieceCorners.corner_T[m] = crossPt[m];
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int m = 0; m < 3; m++)
|
||||
workpieceCorners.corner_B[m] = crossPt[m];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
#if 1
|
||||
//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
sx_BQ_lineDataR(scanLines[i], groundCalibPara.invRMatrix, -1);
|
||||
//将检测结果重新投射回原来的坐标系
|
||||
SVzNL3DPoint rawObj;
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
rawObj = _translatePoint(workpieceCorners.corner_L[i], groundCalibPara.invRMatrix);
|
||||
workpieceCorners.corner_L[i] = rawObj;
|
||||
rawObj = _translatePoint(workpieceCorners.corner_R[i], groundCalibPara.invRMatrix);
|
||||
workpieceCorners.corner_R[i] = rawObj;
|
||||
rawObj = _translatePoint(workpieceCorners.corner_T[i], groundCalibPara.invRMatrix);
|
||||
workpieceCorners.corner_T[i] = rawObj;
|
||||
rawObj = _translatePoint(workpieceCorners.corner_B[i], groundCalibPara.invRMatrix);
|
||||
workpieceCorners.corner_B[i] = rawObj;
|
||||
}
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
rawObj = _translatePoint(region[i].edge_ends[0], groundCalibPara.invRMatrix);
|
||||
region[i].edge_ends[0] = rawObj;
|
||||
rawObj = _translatePoint(region[i].edge_ends[1], groundCalibPara.invRMatrix);
|
||||
region[i].edge_ends[1] = rawObj;
|
||||
rawObj = _translatePoint(region[i].edge_link1_ends[0], groundCalibPara.invRMatrix);
|
||||
region[i].edge_link1_ends[0] = rawObj;
|
||||
rawObj = _translatePoint(region[i].edge_link1_ends[1], groundCalibPara.invRMatrix);
|
||||
region[i].edge_link1_ends[1] = rawObj;
|
||||
rawObj = _translatePoint(region[i].edge_link2_ends[0], groundCalibPara.invRMatrix);
|
||||
region[i].edge_link2_ends[0] = rawObj;
|
||||
rawObj = _translatePoint(region[i].edge_link2_ends[1], groundCalibPara.invRMatrix);
|
||||
region[i].edge_link2_ends[1] = rawObj;
|
||||
for (int m = 0, m_max = (int)region[i].edge.size(); m < m_max; m++)
|
||||
{
|
||||
rawObj = _translatePoint(region[i].edge[m], groundCalibPara.invRMatrix);
|
||||
region[i].edge[m] = rawObj;
|
||||
}
|
||||
for (int m = 0, m_max = (int)region[i].edgeLink_1.size(); m < m_max; m++)
|
||||
{
|
||||
rawObj = _translatePoint(region[i].edgeLink_1[m], groundCalibPara.invRMatrix);
|
||||
region[i].edgeLink_1[m] = rawObj;;
|
||||
}
|
||||
for (int m = 0, m_max = (int)region[i].edgeLink_2.size(); m < m_max; m++)
|
||||
{
|
||||
rawObj = _translatePoint(region[i].edgeLink_2[m], groundCalibPara.invRMatrix);
|
||||
region[i].edgeLink_2[m] = rawObj;;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
if (debug_conturs)
|
||||
{
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
debug_conturs[i].rgnIdx = region[i].rgnIdx;
|
||||
debug_conturs[i].edge_ends[0] = region[i].edge_ends[0];
|
||||
debug_conturs[i].edge_ends[1] = region[i].edge_ends[1];
|
||||
debug_conturs[i].edge_link1_ends[0] = region[i].edge_link1_ends[0];
|
||||
debug_conturs[i].edge_link1_ends[1] = region[i].edge_link1_ends[1];
|
||||
debug_conturs[i].edge_link2_ends[0] = region[i].edge_link2_ends[0];
|
||||
debug_conturs[i].edge_link2_ends[1] = region[i].edge_link2_ends[1];
|
||||
debug_conturs[i].edge_size = (int)region[i].edge.size();
|
||||
debug_conturs[i].edge = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * (int)region[i].edge.size());
|
||||
for (int m = 0, m_max = (int)region[i].edge.size(); m < m_max; m++)
|
||||
debug_conturs[i].edge[m] = region[i].edge[m];
|
||||
debug_conturs[i].edgeLink1_size = (int)region[i].edgeLink_1.size();
|
||||
debug_conturs[i].edgeLink_1 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * (int)region[i].edgeLink_1.size());
|
||||
for (int m = 0, m_max = (int)region[i].edgeLink_1.size(); m < m_max; m++)
|
||||
debug_conturs[i].edgeLink_1[m] = region[i].edgeLink_1[m];
|
||||
debug_conturs[i].edgeLink2_size = (int)region[i].edgeLink_2.size();
|
||||
debug_conturs[i].edgeLink_2 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * (int)region[i].edgeLink_2.size());
|
||||
for (int m = 0, m_max = (int)region[i].edgeLink_2.size(); m < m_max; m++)
|
||||
debug_conturs[i].edgeLink_2[m] = region[i].edgeLink_2[m];
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
workpieceCorners.workpieceType = 1;
|
||||
return workpieceCorners;
|
||||
}
|
||||
74
sourceCode/BQ_workpieceCornerExtraction_Export.h
Normal file
74
sourceCode/BQ_workpieceCornerExtraction_Export.h
Normal file
@ -0,0 +1,74 @@
|
||||
#pragma once
|
||||
|
||||
#if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
|
||||
# define Q_DECL_EXPORT __declspec(dllexport)
|
||||
# define Q_DECL_IMPORT __declspec(dllimport)
|
||||
#else
|
||||
# define Q_DECL_EXPORT __attribute__((visibility("default")))
|
||||
# define Q_DECL_IMPORT __attribute__((visibility("default")))
|
||||
#endif
|
||||
|
||||
#if defined(SG_API_LIBRARY)
|
||||
# define SG_WORKPIECESHARED_EXPORT Q_DECL_EXPORT
|
||||
#else
|
||||
# define SG_WORKPIECESHARED_EXPORT Q_DECL_IMPORT
|
||||
#endif
|
||||
|
||||
#include "SG_baseDataType.h"
|
||||
#include <vector>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
#define _OUTPUT_DEBUG_DATA 1
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double lineLen; //直线段长度
|
||||
}SSX_BQworkpiecePara;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int workpieceType;
|
||||
SVzNL3DPoint corner_L[3];
|
||||
SVzNL3DPoint corner_R[3];
|
||||
SVzNL3DPoint corner_T[3];
|
||||
SVzNL3DPoint corner_B[3];
|
||||
}SSX_BQworkpieceResult;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int rgnIdx;
|
||||
SVzNL3DPoint* edge;
|
||||
int edge_size;
|
||||
SVzNL3DPoint edge_ends[2];
|
||||
SVzNL3DPoint* edgeLink_1;
|
||||
int edgeLink1_size;
|
||||
SVzNL3DPoint edge_link1_ends[2];
|
||||
SVzNL3DPoint* edgeLink_2;
|
||||
int edgeLink2_size;
|
||||
SVzNL3DPoint edge_link2_ends[2];
|
||||
}SSX_debugInfo;
|
||||
|
||||
//计算一个平面调平参数。
|
||||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
SG_WORKPIECESHARED_EXPORT SSG_planeCalibPara sx_BQ_getBaseCalibPara(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
|
||||
|
||||
//相机姿态调平，并去除地面
|
||||
SG_WORKPIECESHARED_EXPORT void sx_BQ_lineDataR(
|
||||
std::vector< SVzNL3DPosition>& a_line,
|
||||
const double* camPoseR,
|
||||
double groundH);
|
||||
|
||||
//提取搭接焊缝
|
||||
SG_WORKPIECESHARED_EXPORT SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_cornerParam cornerPara,
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
SSG_treeGrowParam growParam,
|
||||
SSG_planeCalibPara groundCalibPara,
|
||||
SSX_BQworkpiecePara workpieceParam,
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
SSX_debugInfo* debug_conturs,
|
||||
#endif
|
||||
int* errCode);
|
||||
Loading…
x
Reference in New Issue
Block a user