diff --git a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp new file mode 100644 index 0000000..477d914 --- /dev/null +++ b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp @@ -0,0 +1,796 @@ +// BQ_workpieceCornerExtract_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// + +#include +#include +#include +#include +#include +#include "direct.h" +#include +#include "BQ_workpieceCornerExtraction_Export.h" +#include +#include +#include + +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>& 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>& scanData, double _F, std::vector>& 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>& 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>& scanData, + std::vector>& 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>& 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>& 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> scanData; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData); + if (scanData.size() == 0) + continue; + + std::vector> 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> 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> 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> 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 文件 diff --git a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.vcxproj b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.vcxproj new file mode 100644 index 0000000..0096fa4 --- /dev/null +++ b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.vcxproj @@ -0,0 +1,159 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {cf563709-0402-447e-bfcc-7701cc90d0af} + BQworkpieceCornerExtracttest + 10.0 + + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + Level3 + true + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv320\build\include; + /D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions) + + + Console + true + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world320d.lib;BQ_workpieceCornerExtraction.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv320\build\include; + /D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions) + + + Console + true + true + true + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world320.lib;BQ_workpieceCornerExtraction.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/BQ_workpieceCornerExtraction/BQ_workpieceCornerExtraction.vcxproj b/BQ_workpieceCornerExtraction/BQ_workpieceCornerExtraction.vcxproj new file mode 100644 index 0000000..c9c8465 --- /dev/null +++ b/BQ_workpieceCornerExtraction/BQ_workpieceCornerExtraction.vcxproj @@ -0,0 +1,174 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {ad8415b7-a745-4184-87b8-95619e5066d6} + BQworkpieceCornerExtraction + 10.0 + + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;BQWORKPIECECORNEREXTRACTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;BQWORKPIECECORNEREXTRACTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + true + true + false + + + + + Level3 + true + _DEBUG;BQWORKPIECECORNEREXTRACTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories) + /D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions) + + + Windows + true + false + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world320d.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + NDEBUG;BQWORKPIECECORNEREXTRACTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories) + /D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions) + + + Windows + true + true + true + false + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world320.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + + + + + + + + \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index e57ba3b..2f034c1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) diff --git a/sourceCode/BQ_workpieceCornerExtraction.cpp b/sourceCode/BQ_workpieceCornerExtraction.cpp new file mode 100644 index 0000000..3e24558 --- /dev/null +++ b/sourceCode/BQ_workpieceCornerExtraction.cpp @@ -0,0 +1,673 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "BQ_workpieceCornerExtraction_Export.h" +#include +#include + +//һƽƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SSG_planeCalibPara sx_BQ_getBaseCalibPara( + std::vector< std::vector>& 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& contour, std::vector< std::vector>& 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 trees, bool isVscanTrees, SVzNL3DPoint seedPt, std::vector< std::vector>& 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& contour, std::vector< std::vector>& 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 edge; + SVzNL3DPoint edge_ends[2]; + std::vector edgeLink_1; + SVzNL3DPoint edge_link1_ends[2]; + std::vector edgeLink_2; + SVzNL3DPoint edge_link2_ends[2]; +}SSX_featureContour; + +SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( + std::vector< std::vector>& 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> jumpFeatures_v; + for (int line = 0; line < lineNum; line++) + { + if (line == 202) + int kkk = 1; + + std::vector& lineData = scanLines[line]; + if (linePtNum != (int)lineData.size()) + isGridData = false; + + //˲˳쳣 + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); + + std::vector line_features; + int dataSize = (int)lineData.size(); + sg_getLineCornerFeature_BQ( + &lineData[0], + dataSize, + line, + groundCalibPara.planeHeight, + cornerPara, //scaleͨȡbagH1/4 + line_features); + jumpFeatures_v.push_back(line_features); + } + + if (false == isGridData)//ݲʽ + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return workpieceCorners; + } + + //ˮƽɨ + std::vector> 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> jumpFeatures_h; + int lineNum_h = (int)hLines.size(); + for (int line = 0; line < lineNum_h; line++) + { + if (line == 416) + int kkk = 1; + std::vector& lineData = hLines[line]; + //˲˳쳣 + int ptNum = (int)lineData.size(); + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam); + + std::vector line_features; + int dataSize = (int)lineData.size(); + sg_getLineCornerFeature_BQ( + &hLines[line][0], + dataSize, + line, + groundCalibPara.planeHeight, + cornerPara, //scaleͨȡbagH1/4 + line_features); + jumpFeatures_h.push_back(line_features); + } + + // + //ֱ߷ + std::vector v_trees; + for (int line = 0; line < lineNum; line++) + { + bool isLastLine = false; + if (line == lineNum - 1) + isLastLine = true; + std::vector& 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 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& a_lineJumpFeature = jumpFeatures_h[line]; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineJumpFeature, + h_trees, + growParam); + } + + //treeϢ + std::vector 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 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 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ǽģҶӦROItopBottom + 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 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 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 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; +} \ No newline at end of file diff --git a/sourceCode/BQ_workpieceCornerExtraction_Export.h b/sourceCode/BQ_workpieceCornerExtraction_Export.h new file mode 100644 index 0000000..e5a6622 --- /dev/null +++ b/sourceCode/BQ_workpieceCornerExtraction_Export.h @@ -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 +#include + +#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; + +//һƽƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SG_WORKPIECESHARED_EXPORT SSG_planeCalibPara sx_BQ_getBaseCalibPara( + std::vector< std::vector>& 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>& 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); \ No newline at end of file