From ab632226fecf442f956c4f418a6d0ed17067d992 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Wed, 22 Oct 2025 21:26:30 +0800 Subject: [PATCH 01/12] =?UTF-8?q?=E5=88=9D=E5=A7=8B=E6=8F=90=E4=BA=A4?= =?UTF-8?q?=E5=8D=9A=E6=B8=85=E5=AE=9A=E4=BD=8D=E7=BA=A0=E5=81=8F=E7=AE=97?= =?UTF-8?q?=E6=B3=95=20BQ=5FworkpieceCornerExtract?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../BQ_workpieceCornerExtract_test.cpp | 796 ++++++++++++++++++ .../BQ_workpieceCornerExtract_test.vcxproj | 159 ++++ .../BQ_workpieceCornerExtraction.vcxproj | 174 ++++ sourceCode/BQ_workpieceCornerExtraction.cpp | 673 +++++++++++++++ .../BQ_workpieceCornerExtraction_Export.h | 66 ++ 5 files changed, 1868 insertions(+) create mode 100644 BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp create mode 100644 BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.vcxproj create mode 100644 BQ_workpieceCornerExtraction/BQ_workpieceCornerExtraction.vcxproj create mode 100644 sourceCode/BQ_workpieceCornerExtraction.cpp create mode 100644 sourceCode/BQ_workpieceCornerExtraction_Export.h 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/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..b61d217 --- /dev/null +++ b/sourceCode/BQ_workpieceCornerExtraction_Export.h @@ -0,0 +1,66 @@ +#pragma once + +#if defined(SG_API_LIBRARY) +# define SG_APISHARED_EXPORT __declspec(dllexport) +#else +# define SG_APISHARED_EXPORT __declspec(dllimport) +#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_APISHARED_EXPORT SSG_planeCalibPara sx_BQ_getBaseCalibPara( + std::vector< std::vector>& scanLines); + +//̬ƽȥ +SG_APISHARED_EXPORT void sx_BQ_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); + +//ȡӺ +SG_APISHARED_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 From 90840cb696b888b5a0ad1f81c213ff00a6cafb79 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Sat, 1 Nov 2025 23:32:20 +0800 Subject: [PATCH 02/12] =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E6=8B=86=E5=8C=85?= =?UTF-8?q?=E7=AE=97=E6=B3=95=E4=B8=AD=E4=B8=80=E5=A4=84bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/SG_bagPositioning.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/sourceCode/SG_bagPositioning.cpp b/sourceCode/SG_bagPositioning.cpp index d7ee30f..fd6a229 100644 --- a/sourceCode/SG_bagPositioning.cpp +++ b/sourceCode/SG_bagPositioning.cpp @@ -2879,8 +2879,8 @@ if (hLine == 14) featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vEdgeTree->treeType; featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1; //vscan - int px = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.x - x_range.min); - int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min); + int px = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.x - x_range.min) + x_skip; + int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min) + y_skip; distTranformMask.at(py, px) = 0; } } @@ -2939,8 +2939,8 @@ if (hLine == 14) featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vEdgeTree->treeType; featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1; //vscan - int px = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.x - x_range.min); - int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min); + int px = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.x - x_range.min)+ x_skip; + int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min) + y_skip; distTranformMask.at(py, px) = 0; } } @@ -3005,8 +3005,8 @@ if (hLine == 14) featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hEdgeTree->treeType << 4; featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag - int px = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.x - x_range.min); - int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min); + int px = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.x - x_range.min) + x_skip; + int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min) + y_skip; distTranformMask.at(py, px) = 0; } } @@ -3071,8 +3071,8 @@ if (hLine == 14) featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hEdgeTree->treeType << 4; featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag - int px = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.x - x_range.min); - int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min); + int px = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.x - x_range.min) + x_skip; + int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min) + y_skip; distTranformMask.at(py, px) = 0; } @@ -3113,8 +3113,8 @@ if (hLine == 14) featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vTree->treeType; featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1; //vscan - int px = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.x - x_range.min); - int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min); + int px = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.x - x_range.min) + x_skip; + int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min) + y_skip; distTranformMask.at(py, px) = 0; } } @@ -3154,8 +3154,8 @@ if (hLine == 14) featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hTree->treeType << 4; featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag - int px = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.x - x_range.min); - int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min); + int px = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.x - x_range.min) + x_skip; + int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min) + y_skip; distTranformMask.at(py, px) = 0; } } From ec210c520da840b710c772770c9fc3147b28f175 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Sun, 2 Nov 2025 08:57:28 +0800 Subject: [PATCH 03/12] =?UTF-8?q?=E6=B7=BB=E5=8A=A0vector=E7=A9=BA?= =?UTF-8?q?=E6=A3=80=E6=9F=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/BQ_workpieceCornerExtraction.cpp | 22 +++++++++++++++++++-- sourceCode/SG_errCode.h | 3 ++- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/sourceCode/BQ_workpieceCornerExtraction.cpp b/sourceCode/BQ_workpieceCornerExtraction.cpp index 3e24558..6aebc31 100644 --- a/sourceCode/BQ_workpieceCornerExtraction.cpp +++ b/sourceCode/BQ_workpieceCornerExtraction.cpp @@ -413,6 +413,11 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( _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); + if ((region[0].edgeLink_1.size() < 5) || (region[0].edgeLink_2.size() < 5)) + { + *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; + return workpieceCorners; + } region[1].rgnIdx = 1; //Top _getEdgeContour(&v_trees[vTree_T], region[1].edge, scanLines, true); @@ -423,7 +428,11 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( _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); - + if ((region[1].edgeLink_1.size() < 5) || (region[1].edgeLink_2.size() < 5)) + { + *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; + return workpieceCorners; + } region[2].rgnIdx = 2; //Right _getEdgeContour(&h_trees[hTree_R], region[2].edge, scanLines, false); //ѰҶӦ @@ -433,7 +442,11 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( _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); - + if ((region[2].edgeLink_1.size() < 5) || (region[2].edgeLink_2.size() < 5)) + { + *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; + return workpieceCorners; + } region[3].rgnIdx = 3; //Bottom _getEdgeContour(&v_trees[vTree_B], region[3].edge, scanLines, true); //ѰҶӦ @@ -443,6 +456,11 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( _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); + if ((region[3].edgeLink_1.size() < 5) || (region[3].edgeLink_2.size() < 5)) + { + *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; + return workpieceCorners; + } for (int i = 0; i < 4; i++) { diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index 111db32..e61d36e 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -9,4 +9,5 @@ //BQ_workpiece #define SX_ERR_INVLD_VTREE_NUM -2001 -#define SX_ERR_INVLD_HTREE_NUM -2002 \ No newline at end of file +#define SX_ERR_INVLD_HTREE_NUM -2002 +#define SX_ERR_INVLD_EDGE_LINK_NUM -2003 \ No newline at end of file From 3076a17c18178dac861b9252cd570e48ca4fc19b Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Sun, 2 Nov 2025 09:36:53 +0800 Subject: [PATCH 04/12] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E9=94=99=E8=AF=AF?= =?UTF-8?q?=E5=88=A4=E6=96=AD=EF=BC=9A=E8=BF=94=E5=9B=9E=E5=80=BC=E4=B8=BA?= =?UTF-8?q?-1=E6=97=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/BQ_workpieceCornerExtraction.cpp | 40 +++++++++++++++++++++ sourceCode/SG_errCode.h | 3 +- 2 files changed, 42 insertions(+), 1 deletion(-) diff --git a/sourceCode/BQ_workpieceCornerExtraction.cpp b/sourceCode/BQ_workpieceCornerExtraction.cpp index 6aebc31..5aaa912 100644 --- a/sourceCode/BQ_workpieceCornerExtraction.cpp +++ b/sourceCode/BQ_workpieceCornerExtraction.cpp @@ -410,8 +410,18 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( SVzNL3DPoint firstPt = region[0].edge[0]; SVzNL3DPoint lastPt = region[0].edge.back(); int idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, true); + if (idx0 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return; + } _getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[0].edgeLink_1, scanLines, true, workpieceParam.lineLen); int idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, true); + if (idx1 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return; + } _getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[0].edgeLink_2, scanLines, true, workpieceParam.lineLen); if ((region[0].edgeLink_1.size() < 5) || (region[0].edgeLink_2.size() < 5)) { @@ -425,8 +435,18 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( firstPt = region[1].edge[0]; lastPt = region[1].edge.back(); idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true); + if (idx0 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return; + } _getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[1].edgeLink_1, scanLines, true, workpieceParam.lineLen); idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true); + if (idx1 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return; + } _getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[1].edgeLink_2, scanLines, true, workpieceParam.lineLen); if ((region[1].edgeLink_1.size() < 5) || (region[1].edgeLink_2.size() < 5)) { @@ -439,8 +459,18 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( firstPt = region[2].edge[0]; lastPt = region[2].edge.back(); idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, false); + if (idx0 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return; + } _getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[2].edgeLink_1, scanLines, false, workpieceParam.lineLen); idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, false); + if (idx1 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return; + } _getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[2].edgeLink_2, scanLines, false, workpieceParam.lineLen); if ((region[2].edgeLink_1.size() < 5) || (region[2].edgeLink_2.size() < 5)) { @@ -453,8 +483,18 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( firstPt = region[3].edge[0]; lastPt = region[3].edge.back(); idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true); + if (idx0 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return; + } _getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[3].edgeLink_1, scanLines, false, workpieceParam.lineLen); idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true); + if (idx1 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return; + } _getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[3].edgeLink_2, scanLines, false, workpieceParam.lineLen); if ((region[3].edgeLink_1.size() < 5) || (region[3].edgeLink_2.size() < 5)) { diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index e61d36e..a35b02a 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -10,4 +10,5 @@ //BQ_workpiece #define SX_ERR_INVLD_VTREE_NUM -2001 #define SX_ERR_INVLD_HTREE_NUM -2002 -#define SX_ERR_INVLD_EDGE_LINK_NUM -2003 \ No newline at end of file +#define SX_ERR_INVLD_EDGE_LINK_NUM -2003 +#define SX_ERR_INVLD_CLOSES_PT -2004 \ No newline at end of file From 5b658bbef72f538167f888dbc5a768748d5b33c2 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Sun, 2 Nov 2025 09:46:49 +0800 Subject: [PATCH 05/12] =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E4=B8=80=E4=B8=AAbug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/BQ_workpieceCornerExtraction.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/sourceCode/BQ_workpieceCornerExtraction.cpp b/sourceCode/BQ_workpieceCornerExtraction.cpp index 5aaa912..4744e53 100644 --- a/sourceCode/BQ_workpieceCornerExtraction.cpp +++ b/sourceCode/BQ_workpieceCornerExtraction.cpp @@ -413,14 +413,14 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( if (idx0 < 0) { *errCode = SX_ERR_INVLD_CLOSES_PT; - return; + return workpieceCorners; } _getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[0].edgeLink_1, scanLines, true, workpieceParam.lineLen); int idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, true); if (idx1 < 0) { *errCode = SX_ERR_INVLD_CLOSES_PT; - return; + return workpieceCorners; } _getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[0].edgeLink_2, scanLines, true, workpieceParam.lineLen); if ((region[0].edgeLink_1.size() < 5) || (region[0].edgeLink_2.size() < 5)) @@ -438,14 +438,14 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( if (idx0 < 0) { *errCode = SX_ERR_INVLD_CLOSES_PT; - return; + return workpieceCorners; } _getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[1].edgeLink_1, scanLines, true, workpieceParam.lineLen); idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true); if (idx1 < 0) { *errCode = SX_ERR_INVLD_CLOSES_PT; - return; + return workpieceCorners; } _getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[1].edgeLink_2, scanLines, true, workpieceParam.lineLen); if ((region[1].edgeLink_1.size() < 5) || (region[1].edgeLink_2.size() < 5)) @@ -462,14 +462,14 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( if (idx0 < 0) { *errCode = SX_ERR_INVLD_CLOSES_PT; - return; + return workpieceCorners; } _getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[2].edgeLink_1, scanLines, false, workpieceParam.lineLen); idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, false); if (idx1 < 0) { *errCode = SX_ERR_INVLD_CLOSES_PT; - return; + return workpieceCorners; } _getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[2].edgeLink_2, scanLines, false, workpieceParam.lineLen); if ((region[2].edgeLink_1.size() < 5) || (region[2].edgeLink_2.size() < 5)) @@ -486,14 +486,14 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( if (idx0 < 0) { *errCode = SX_ERR_INVLD_CLOSES_PT; - return; + return workpieceCorners; } _getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[3].edgeLink_1, scanLines, false, workpieceParam.lineLen); idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true); if (idx1 < 0) { *errCode = SX_ERR_INVLD_CLOSES_PT; - return; + return workpieceCorners; } _getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[3].edgeLink_2, scanLines, false, workpieceParam.lineLen); if ((region[3].edgeLink_1.size() < 5) || (region[3].edgeLink_2.size() < 5)) From db2ad9337edd6c75a75883bbee7ecc9efa9501a8 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Sat, 8 Nov 2025 02:13:19 +0800 Subject: [PATCH 06/12] BQ_workpieceCornerExtract Version:1.1.0 --- .../BQ_workpieceCornerExtract_test.cpp | 54 +- .../QRcode3Ddetection_test.cpp | 170 ++- SG_Algorithm.sln | 26 + bagPositioning_test/bagPositioning_test.cpp | 6 +- baseAlgorithm/baseAlgorithm.vcxproj | 1 + sourceCode/BQ_workpieceCornerExtraction.cpp | 1097 ++++++++++++----- .../BQ_workpieceCornerExtraction_Export.h | 3 + sourceCode/SG_baseAlgo_Export.h | 52 +- sourceCode/SG_baseDataType.h | 35 +- sourceCode/SG_baseFunc.cpp | 185 ++- sourceCode/SG_errCode.h | 5 +- sourceCode/SG_featureGrow.cpp | 121 +- sourceCode/SG_lineFeature.cpp | 400 +++++- sourceCode/SG_regionGrow.cpp | 1 - 14 files changed, 1815 insertions(+), 341 deletions(-) diff --git a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp index 477d914..fc494f1 100644 --- a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp +++ b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp @@ -501,7 +501,7 @@ void _outputRGBDScanLapWeld_RGBD( sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; } - if (debugData) + if ((debugData) && (workpieceCorner.workpieceType > 0)) { 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; @@ -606,15 +606,20 @@ void _outputRGBDScanLapWeld_RGBD( #define CONVERT_TO_GRID 0 #define TEST_COMPUTE_CALIB_PARA 0 #define TEST_COMPUTE_CORNER 1 -#define TEST_GROUP 1 +#define TEST_GROUP 6 int main() { const char* dataPath[TEST_GROUP] = { - "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\" //0 + "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\", //0 + "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度1\\", //1 + "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度2\\", //2 + "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度3\\", //3 + "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度4\\", //4 + "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\", //5 }; SVzNLRange fileIdx[TEST_GROUP] = { - {1,3} + {1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,12} }; #if CONVERT_TO_GRID @@ -641,7 +646,7 @@ int main() #if TEST_COMPUTE_CALIB_PARA char _calib_datafile[256]; - sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\scanData_1.txt"); + sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\9_LazerData_Hi229229.txt"); int lineNum = 0; float lineV = 0.0f; int dataCalib = 0; @@ -677,10 +682,10 @@ int main() } // char calibFile[250]; - sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\ground_calib_para.txt"); + 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"); + 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); @@ -688,7 +693,10 @@ int main() #endif #if TEST_COMPUTE_CORNER - for (int grp = 0; grp <= 0; grp++) + const char* ver = wd_BQWorkpieceCornerVersion(); + printf("ver:%s\n", ver); + + for (int grp = 1; grp <= 5; grp++) { SSG_planeCalibPara poseCalibPara; //初始化成单位阵 @@ -710,12 +718,40 @@ int main() sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\ground_calib_para.txt"); poseCalibPara = _readCalibPara(calibFile); } + else if (grp == 1) + { + sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度1\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if (grp == 2) + { + sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度2\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if (grp == 3) + { + sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度3\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if (grp == 4) + { + sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度4\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if (grp == 5) + { + 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); + if(0 == grp) + sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx); + else + sprintf_s(_scan_file, "%s%d_LazerData_Hi229229.txt", dataPath[grp], fidx); std::vector> scanLines; vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); diff --git a/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp b/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp index 4e384c3..15cb830 100644 --- a/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp +++ b/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp @@ -73,6 +73,95 @@ void vzReadLaserScanPointFromFile_plyTxt(const char* fileName, std::vector< SVzN return; } +typedef struct +{ + int x; + double y; + double z; +} WD_Encode3DPoint; +void vzReadLaserScanPointFromFile_encodePlyTxt(const char* fileName, std::vector< WD_Encode3DPoint>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + while (std::getline(inputFile, linedata)) + { + if (linedata.empty()) + continue; + + int X; + double Y, Z, tmp; + sscanf_s(linedata.c_str(), "%d,%lf,%lf", &X, &Y, &Z); + WD_Encode3DPoint a_pt; + a_pt.x = X; + a_pt.y = Y; + a_pt.z = Z; + scanData.push_back(a_pt); + } + return; +} +void convertEncodePlyToVzData(std::vector< WD_Encode3DPoint>& scanData, std::vector>& scanLines, double stepping, bool toGrid) +{ + int size = (int)scanData.size(); + int preEnc = -1; + std::vector a_line; + int vldNum = 0; + double lineX = 0; + for (int i = 0; i < size; i++) + { + WD_Encode3DPoint a_ply = scanData[i]; + if (a_ply.x != preEnc) //new line + { + if (vldNum > 0) + { + scanLines.push_back(a_line); + a_line.clear(); + } + if (preEnc < 0) + lineX = 0; + else + lineX += (double)(a_ply.x - preEnc) * stepping; + preEnc = a_ply.x; + vldNum = 0; + } + + SVzNL3DPosition a_pt; + a_pt.nPointIdx = vldNum; + + bool zeroPt = true; + if ((a_ply.z > 29.0) && (a_ply.z < 34.0)) + { + a_pt.pt3D.x = lineX; + a_pt.pt3D.y = a_ply.y; + a_pt.pt3D.z = a_ply.z; + zeroPt = false; + } + else + { + a_pt.pt3D.x = 0; + a_pt.pt3D.y = 0; + a_pt.pt3D.z = 0; + } + if (toGrid == true) + { + a_line.push_back(a_pt); + vldNum++; + } + else + { + if (false == zeroPt) + { + a_line.push_back(a_pt); + vldNum++; + } + } + + } +} + void _outputScanDataFile_vector(char* fileName, std::vector>& scanLines, bool removeZeros, int* headNullLines) { std::ofstream sw(fileName); @@ -133,6 +222,46 @@ void _outputScanDataFile_vector(char* fileName, std::vector>& scanLines, bool removeZeros, int* headNullLines) +{ + std::ofstream sw(fileName); + int lineNum = scanLines.size(); + if (lineNum == 0) + return; + + int null_lines = 0; + bool counterNull = true; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = scanLines[line].size(); + if (linePtNum == 0) + continue; + + 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<< std::endl; + } + if (true == counterNull) + { + if (true == isNull) + null_lines++; + else + counterNull = false; + } + } + *headNullLines = null_lines; + sw.close(); +} + void _outputRGBDScanDataFile_RGBD( char* fileName, std::vector>& scanLines, @@ -562,20 +691,53 @@ void _getRoiClouds( return; } +#define CONVERT_TO_GRID 1 #define TEST_COMPUTE_CALIB_PARA 0 -#define TEST_COMPUTE_QRCODE_IMG 1 -#define TEST_GROUP 2 +#define TEST_COMPUTE_QRCODE_IMG 0 +#define TEST_GROUP 2 int main() { const char* dataPath[TEST_GROUP] = { - "F:\\ShangGu\\项目\\工件端部圆点二维码\\", //0 - "F:\\ShangGu\\项目\\工件端部圆点二维码\\字符\\" //1 + "F:\\ShangGu\\项目\\国铭铸管\\二维码\\", //0 + "F:\\ShangGu\\项目\\国铭铸管\\字符\\" //1 }; SVzNLRange fileIdx[TEST_GROUP] = { {3,3}, {1,8} }; +#if CONVERT_TO_GRID + int convertGrp = 1; + for (int fidx = fileIdx[convertGrp].nMin; fidx <= fileIdx[convertGrp].nMax; fidx++) + { + char _scan_file[256]; + sprintf_s(_scan_file, "%sdata%d.txt", dataPath[convertGrp], fidx); + + std::vector< WD_Encode3DPoint> scanPly; + vzReadLaserScanPointFromFile_encodePlyTxt(_scan_file, scanPly); + + double stepping = 0.0625; + std::vector> scanData; + std::vector> gridScanData; + bool toGrid = false; + convertEncodePlyToVzData(scanPly, scanData, stepping, toGrid); + toGrid = true; + convertEncodePlyToVzData(scanPly, gridScanData, stepping, toGrid); + //将数据恢复为按扫描线存储格式 + sprintf_s(_scan_file, "%sscanDataPlyTxt_%d.txt", dataPath[convertGrp], fidx); + int headNullLines = 0; + _outputScanPlyTxtFile_vector(_scan_file, scanData, false, &headNullLines); +#if 0 + sprintf_s(_scan_file, "%svzScanData_%d.txt", dataPath[convertGrp], fidx); + int headNullLines = 0; + _outputScanDataFile_vector(_scan_file, scanData, false, &headNullLines); + sprintf_s(_scan_file, "%svzScanData_grid_%d.txt", dataPath[convertGrp], fidx); + _outputScanDataFile_vector(_scan_file, gridScanData, false, &headNullLines); + printf("%s: head null lines = %d\n", _scan_file, headNullLines); +#endif + } +#endif + #if TEST_COMPUTE_CALIB_PARA char _calib_datafile[256]; sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\工件端部圆点二维码\\LaserLine3_grid.txt"); diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index 65b4703..5957761 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -118,6 +118,16 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_workpieceCornerExtract_t {AD8415B7-A745-4184-87B8-95619E5066D6} = {AD8415B7-A745-4184-87B8-95619E5066D6} EndProjectSection EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "particleSizeMeasurement_test", "particleSizeMeasurement_test\particleSizeMeasurement_test.vcxproj", "{0F4995A6-3978-4EF6-87AD-CC15EC9B007B}" + ProjectSection(ProjectDependencies) = postProject + {8FD0F574-61C6-4094-8521-33AEDDB44797} = {8FD0F574-61C6-4094-8521-33AEDDB44797} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "particleSizeMeasurement", "particleSizeMeasurement\particleSizeMeasurement.vcxproj", "{8FD0F574-61C6-4094-8521-33AEDDB44797}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -310,6 +320,22 @@ Global {CF563709-0402-447E-BFCC-7701CC90D0AF}.Release|x64.Build.0 = Release|x64 {CF563709-0402-447E-BFCC-7701CC90D0AF}.Release|x86.ActiveCfg = Release|Win32 {CF563709-0402-447E-BFCC-7701CC90D0AF}.Release|x86.Build.0 = Release|Win32 + {0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Debug|x64.ActiveCfg = Debug|x64 + {0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Debug|x64.Build.0 = Debug|x64 + {0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Debug|x86.ActiveCfg = Debug|Win32 + {0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Debug|x86.Build.0 = Debug|Win32 + {0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Release|x64.ActiveCfg = Release|x64 + {0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Release|x64.Build.0 = Release|x64 + {0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Release|x86.ActiveCfg = Release|Win32 + {0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Release|x86.Build.0 = Release|Win32 + {8FD0F574-61C6-4094-8521-33AEDDB44797}.Debug|x64.ActiveCfg = Debug|x64 + {8FD0F574-61C6-4094-8521-33AEDDB44797}.Debug|x64.Build.0 = Debug|x64 + {8FD0F574-61C6-4094-8521-33AEDDB44797}.Debug|x86.ActiveCfg = Debug|Win32 + {8FD0F574-61C6-4094-8521-33AEDDB44797}.Debug|x86.Build.0 = Debug|Win32 + {8FD0F574-61C6-4094-8521-33AEDDB44797}.Release|x64.ActiveCfg = Release|x64 + {8FD0F574-61C6-4094-8521-33AEDDB44797}.Release|x64.Build.0 = Release|x64 + {8FD0F574-61C6-4094-8521-33AEDDB44797}.Release|x86.ActiveCfg = Release|Win32 + {8FD0F574-61C6-4094-8521-33AEDDB44797}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/bagPositioning_test/bagPositioning_test.cpp b/bagPositioning_test/bagPositioning_test.cpp index c34b771..32af194 100644 --- a/bagPositioning_test/bagPositioning_test.cpp +++ b/bagPositioning_test/bagPositioning_test.cpp @@ -2868,8 +2868,8 @@ int main() else if (grp == 19) //纸箱拆垛数据\vizum数据 { algoParam.bagParam.bagL = 500;// 500; // 420; //袋子长65cm - algoParam.bagParam.bagW = 320; // 420; // 320; //袋子宽40cm - algoParam.bagParam.bagH = 70; //袋子高16cm + algoParam.bagParam.bagW = 350; // 420; // 320; //袋子宽40cm + algoParam.bagParam.bagH = 80; //袋子高16cm algoParam.growParam.maxLineSkipNum = 5; algoParam.growParam.yDeviation_max = 20.0; algoParam.growParam.maxSkipDistance = 20.0; @@ -3036,7 +3036,7 @@ int main() //fidx = 193; if (grp < TEST_TOP_VIEW_GROUP) //正面抓取 { - algoParam.supportRotate = 1; //支持相机与对象之间有一个旋转角度（小于30度） + algoParam.supportRotate = 0; //支持相机与对象之间有一个旋转角度（小于30度） int lineNum = 0; float lineV = 0.0f; diff --git a/baseAlgorithm/baseAlgorithm.vcxproj b/baseAlgorithm/baseAlgorithm.vcxproj index 84eca3c..f858582 100644 --- a/baseAlgorithm/baseAlgorithm.vcxproj +++ b/baseAlgorithm/baseAlgorithm.vcxproj @@ -173,6 +173,7 @@ + diff --git a/sourceCode/BQ_workpieceCornerExtraction.cpp b/sourceCode/BQ_workpieceCornerExtraction.cpp index 4744e53..efa1f23 100644 --- a/sourceCode/BQ_workpieceCornerExtraction.cpp +++ b/sourceCode/BQ_workpieceCornerExtraction.cpp @@ -5,6 +5,12 @@ #include #include +std::string m_strVersion = "1.1.0"; +const char* wd_BQWorkpieceCornerVersion(void) +{ + return m_strVersion.c_str(); +} + //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ @@ -154,6 +160,27 @@ typedef struct SVzNL3DPoint edge_link2_ends[2]; }SSX_featureContour; +typedef struct +{ + int lineIdx; + int ptIdx; + double R; + double angle; + double x; + double y; + double z; +}SWD_polarPt; + +//ʱתʱ > 0 ˳ʱתʱ < 0 +cv::Point2f _rotate2D(cv::Point2f pt, double sinTheta, double cosTheta) +{ + return (cv::Point2f((float)(pt.x * cosTheta - pt.y * sinTheta), (float)(pt.x * sinTheta + pt.y * cosTheta))); +} + +bool compareByAngle(const SWD_polarPt& a, const SWD_polarPt& b) { + return a.angle < b.angle; +} + SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( std::vector< std::vector>& scanLines, const SSG_cornerParam cornerPara, @@ -179,328 +206,763 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( 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); - if (idx0 < 0) + //ӦתǶ { - *errCode = SX_ERR_INVLD_CLOSES_PT; - return workpieceCorners; - } - _getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[0].edgeLink_1, scanLines, true, workpieceParam.lineLen); - int idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, true); - if (idx1 < 0) - { - *errCode = SX_ERR_INVLD_CLOSES_PT; - return workpieceCorners; - } - _getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[0].edgeLink_2, scanLines, true, workpieceParam.lineLen); - if ((region[0].edgeLink_1.size() < 5) || (region[0].edgeLink_2.size() < 5)) - { - *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; - return workpieceCorners; + //ֱȡ + std::vector> jumpFeatures_v_raw; + 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_raw.push_back(line_features); + } + + if (false == isGridData)//ݲʽ + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return workpieceCorners; + } + + //ˮƽɨ + std::vector> hLines_raw; + hLines_raw.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + hLines_raw[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã + hLines_raw[j][line] = scanLines[line][j]; + hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y; + hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + //ˮƽarcȡ + std::vector> jumpFeatures_h_raw; + int lineNum_h_raw = (int)hLines_raw.size(); + for (int line = 0; line < lineNum_h_raw; line++) + { + if (line == 416) + int kkk = 1; + std::vector& lineData = hLines_raw[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_raw[line][0], + dataSize, + line, + groundCalibPara.planeHeight, + cornerPara, //scaleͨȡbagH1/4 + line_features); + jumpFeatures_h_raw.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_raw[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_raw; line++) + { + if (line == 650) + int kkk = 1; + bool isLastLine = false; + if (line == lineNum_h_raw - 1) + isLastLine = true; + std::vector& a_lineJumpFeature = jumpFeatures_h_raw[line]; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineJumpFeature, + h_trees, + growParam); + } + + std::vector polarPoints; +#if 0 + for (int line = 0; line < lineNum; line++) + { + std::vector& a_lineJumpFeature = jumpFeatures_v_raw[line]; + for (int pi = 0, pi_max = (int)a_lineJumpFeature.size(); pi < pi_max; pi++) + { + int lineIdx = a_lineJumpFeature[pi].jumpPos2D.x; + int ptIdx = a_lineJumpFeature[pi].jumpPos2D.y; + if (scanLines[lineIdx][ptIdx].nPointIdx >= 0) + { + SWD_polarPt a_polarPt; + a_polarPt.lineIdx = lineIdx; + a_polarPt.ptIdx = ptIdx; + a_polarPt.R = 0; + a_polarPt.angle = 0; + a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x; + a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y; + a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z; + polarPoints.push_back(a_polarPt); + scanLines[lineIdx][ptIdx].nPointIdx = -1; + } + } + } + for (int line = 0; line < lineNum_h_raw; line++) + { + std::vector& a_lineJumpFeature = jumpFeatures_h_raw[line]; + for (int pi = 0, pi_max = (int)a_lineJumpFeature.size(); pi < pi_max; pi++) + { + int lineIdx = a_lineJumpFeature[pi].jumpPos2D.y; + int ptIdx = a_lineJumpFeature[pi].jumpPos2D.x; + if (scanLines[lineIdx][ptIdx].nPointIdx >= 0) + { + SWD_polarPt a_polarPt; + a_polarPt.lineIdx = lineIdx; + a_polarPt.ptIdx = ptIdx; + a_polarPt.R = 0; + a_polarPt.angle = 0; + a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x; + a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y; + a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z; + polarPoints.push_back(a_polarPt); + scanLines[lineIdx][ptIdx].nPointIdx = -1; + } + } + } +#else + for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_vTree = &v_trees[i]; + //ԭʼϱǣͬʱMaskϱ + for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++) + { + int lineIdx = a_vTree->treeNodes[j].jumpPos2D.x; + int ptIdx = a_vTree->treeNodes[j].jumpPos2D.y; + if (scanLines[lineIdx][ptIdx].nPointIdx >= 0) + { + SWD_polarPt a_polarPt; + a_polarPt.lineIdx = lineIdx; + a_polarPt.ptIdx = ptIdx; + a_polarPt.R = 0; + a_polarPt.angle = 0; + a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x; + a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y; + a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z; + polarPoints.push_back(a_polarPt); + scanLines[lineIdx][ptIdx].nPointIdx = -1; + } + } + } + for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_hTree = &h_trees[i]; + //ԭʼϱǣͬʱMaskϱ + for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++) + { + int lineIdx = a_hTree->treeNodes[j].jumpPos2D.y; + int ptIdx = a_hTree->treeNodes[j].jumpPos2D.x; + if (scanLines[lineIdx][ptIdx].nPointIdx >= 0) + { + SWD_polarPt a_polarPt; + a_polarPt.lineIdx = lineIdx; + a_polarPt.ptIdx = ptIdx; + a_polarPt.R = 0; + a_polarPt.angle = 0; + a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x; + a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y; + a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z; + polarPoints.push_back(a_polarPt); + scanLines[lineIdx][ptIdx].nPointIdx = -1; + } + } + } + +#endif + //㼸 + int contourPtSize = (int)polarPoints.size(); + if (contourPtSize == 0) + { + *errCode = SX_ERR_ZERO_CONTOUR_PT; + return workpieceCorners; + } + double center_x = 0; + double center_y = 0; + for (int pi = 0; pi < contourPtSize; pi++) + { + center_x += polarPoints[pi].x; + center_y += polarPoints[pi].y; + } + center_x = center_x / (double)contourPtSize; + center_y = center_y / (double)contourPtSize; + //㼫RTheta + for (int pi = 0; pi < contourPtSize; pi++) + { + double angle = atan2(polarPoints[pi].y - center_y, polarPoints[pi].x - center_x); + angle = (angle / PI) * 180 +180.0; + double R = sqrt(pow(polarPoints[pi].y - center_y, 2) + pow(polarPoints[pi].x - center_x, 2)); + polarPoints[pi].R = R; + polarPoints[pi].angle = angle; + } + //ǶȴС + std::sort(polarPoints.begin(), polarPoints.end(), compareByAngle); + //ȡRֵ + std::vector rPeaks; + std::vector polarRPeakPts; + int winSize = contourPtSize / 36; //+-10ȷΧ + if (winSize < 5) + winSize = 5; + for (int pi = 0; pi < contourPtSize; pi++) + { + double currR = polarPoints[pi].R; + bool isPeak = true; + for (int k = -winSize; k <= winSize; k++) + { + int idx = (pi + k + contourPtSize) % contourPtSize; //Ͳνṹ + if (polarPoints[idx].R > currR) + { + isPeak = false; + break; + } + } + if (true == isPeak) + { + rPeaks.push_back(pi); + polarRPeakPts.push_back(polarPoints[pi]); + } + } + if (rPeaks.size() != 8) + { + *errCode = SX_ERR_INVLID_RPEAK_NUM; + return workpieceCorners; + } + //SSX_featureContour region[4]; + //Left, top, right, bottom + SSG_intPair peakPair[4]; + int pairIdx = 0; + for (int i = 0; i < 8; i++) + { + if (polarRPeakPts[i].lineIdx < 0) + continue; + //ǰĿ + int pre_idx = (i - 1 + 8) % 8; + int nxt_idx = (i + 1 + 8) % 8; + bool pre_isPair = false; + if (rPeaks[pre_idx] >= 0) + { + int mid_idx; + if (rPeaks[i] < rPeaks[pre_idx]) + mid_idx = ((contourPtSize + rPeaks[i] + rPeaks[pre_idx]) / 2) % contourPtSize; + else + mid_idx = (rPeaks[i] + rPeaks[pre_idx]) / 2; + double mid_R = polarPoints[mid_idx].R; + cv::Point2f midChord = cv::Point2f((polarRPeakPts[i].x + polarRPeakPts[pre_idx].x) / 2, (polarRPeakPts[i].y + polarRPeakPts[pre_idx].y) / 2); + double meanR = sqrt(pow(midChord.x - center_x,2) + pow(midChord.y-center_y,2)); + double delta_R = abs(mid_R - meanR); + if (delta_R < 10) + pre_isPair = true; + } + bool nxt_isPair = false; + if (rPeaks[nxt_idx] >= 0) + { + int mid_idx; + if (rPeaks[nxt_idx] < rPeaks[i]) + mid_idx = ((contourPtSize + rPeaks[nxt_idx] + rPeaks[i]) / 2) % contourPtSize; + else + mid_idx = (rPeaks[i] + rPeaks[nxt_idx]) / 2; + double mid_R = polarPoints[mid_idx].R; + cv::Point2f midChord = cv::Point2f((polarRPeakPts[i].x + polarRPeakPts[nxt_idx].x) / 2, (polarRPeakPts[i].y + polarRPeakPts[nxt_idx].y) / 2); + double meanR = sqrt(pow(midChord.x - center_x, 2) + pow(midChord.y - center_y, 2)); + double delta_R = abs(mid_R - meanR); + if (delta_R < 10) + nxt_isPair = true; + } + + if ((true == pre_isPair) && (false == nxt_isPair)) + { + peakPair[pairIdx].idx = pairIdx; + peakPair[pairIdx].data_0 = pre_idx; + peakPair[pairIdx].data_1 = i; + pairIdx++; + polarRPeakPts[pre_idx].lineIdx = -1; + polarRPeakPts[i].lineIdx = -1; + } + else if ((false == pre_isPair) && (true == nxt_isPair)) + { + peakPair[pairIdx].idx = pairIdx; + peakPair[pairIdx].data_0 = i; + peakPair[pairIdx].data_1 = nxt_idx; + pairIdx++; + polarRPeakPts[nxt_idx].lineIdx = -1; + polarRPeakPts[i].lineIdx = -1; + } + else + { + *errCode = SX_ERR_INVLID_RPEAK_PAIR; + return workpieceCorners; + } + } + //SSX_featureContour region[4] + double centerAngle; + int idx0 = peakPair[0].data_0; + int idx1 = peakPair[0].data_1; + if (polarRPeakPts[idx0].angle > polarRPeakPts[idx1].angle) + { + centerAngle = (360 + polarRPeakPts[idx0].angle + polarRPeakPts[idx1].angle) / 2; + if (centerAngle >= 360) + centerAngle = centerAngle - 360; + } + else + centerAngle = (polarRPeakPts[idx0].angle + polarRPeakPts[idx1].angle) / 2; + int LRTB[4]; + if ((centerAngle > 315) || (centerAngle <= 45)) //Left + { + LRTB[0] = 0; //left + LRTB[1] = 1; //top + LRTB[2] = 2; //right + LRTB[3] = 3; //bottom + } + else if ((centerAngle > 45) && (centerAngle <= 135)) //Top + { + LRTB[0] = 1; //top + LRTB[1] = 2; //right + LRTB[2] = 3; //bottom + LRTB[3] = 0; //left + } + else if ((centerAngle > 135) && (centerAngle <= 225)) //Right + { + LRTB[0] = 2; //right + LRTB[1] = 3; //bottom + LRTB[2] = 0; //left + LRTB[3] = 1; //top + } + else //Bottom + { + LRTB[0] = 3; //bottom + LRTB[1] = 0; //left + LRTB[2] = 1; //top + LRTB[3] = 2; //right + } + + for (int i = 0; i < 4; i++) + { + int rgnIdx = LRTB[i]; + int idx_0 = peakPair[i].data_0; + int idx_1 = peakPair[i].data_1; + std::vector edge; + std::vector edge_link1; + std::vector edge_link2; + int startIdx = rPeaks[idx_0]; + int endIdx = rPeaks[idx_1]; + if (startIdx < endIdx) + { + for (int m = startIdx + 1; m < endIdx; m++) + { + SVzNL3DPoint a_pt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z }; + edge.push_back(a_pt); + } + } + else // + { + for (int m = startIdx + 1; m < contourPtSize; m++) + { + SVzNL3DPoint a_pt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z }; + edge.push_back(a_pt); + } + for (int m = 0; m < endIdx; m++) + { + SVzNL3DPoint a_pt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z }; + edge.push_back(a_pt); + } + } + //edge_link1 + for (int m = 1; m < contourPtSize; m++) + { + int idx = (startIdx - m + contourPtSize) % contourPtSize; + SVzNL3DPoint a_pt = { polarPoints[idx].x, polarPoints[idx].y, polarPoints[idx].z }; + double dist = sqrt(pow(a_pt.x - polarPoints[startIdx].x, 2) + pow(a_pt.y - polarPoints[startIdx].y, 2)); + if (dist > workpieceParam.lineLen) + break; + edge_link1.push_back(a_pt); + } + //edge_link2 + for (int m = 1; m < contourPtSize; m++) + { + int idx = (endIdx + m) % contourPtSize; + SVzNL3DPoint a_pt = { polarPoints[idx].x, polarPoints[idx].y, polarPoints[idx].z }; + double dist = sqrt(pow(a_pt.x - polarPoints[endIdx].x, 2) + pow(a_pt.y - polarPoints[endIdx].y, 2)); + if (dist > workpieceParam.lineLen) + break; + edge_link2.push_back(a_pt); + } + + region[rgnIdx].rgnIdx = rgnIdx; + region[rgnIdx].edge.insert(region[rgnIdx].edge.end(), edge.begin(), edge.end()); + region[rgnIdx].edgeLink_1.insert(region[rgnIdx].edgeLink_1.end(), edge_link1.begin(), edge_link1.end()); + region[rgnIdx].edgeLink_2.insert(region[rgnIdx].edgeLink_2.end(), edge_link2.begin(), edge_link2.end()); + } } - 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); - if (idx0 < 0) +#if 0 //㷨ҪɨΪˮƽ-ֱ { - *errCode = SX_ERR_INVLD_CLOSES_PT; - return workpieceCorners; - } - _getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[1].edgeLink_1, scanLines, true, workpieceParam.lineLen); - idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true); - if (idx1 < 0) - { - *errCode = SX_ERR_INVLD_CLOSES_PT; - return workpieceCorners; - } - _getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[1].edgeLink_2, scanLines, true, workpieceParam.lineLen); - if ((region[1].edgeLink_1.size() < 5) || (region[1].edgeLink_2.size() < 5)) - { - *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; - return workpieceCorners; - } - 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); - if (idx0 < 0) - { - *errCode = SX_ERR_INVLD_CLOSES_PT; - return workpieceCorners; - } - _getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[2].edgeLink_1, scanLines, false, workpieceParam.lineLen); - idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, false); - if (idx1 < 0) - { - *errCode = SX_ERR_INVLD_CLOSES_PT; - return workpieceCorners; - } - _getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[2].edgeLink_2, scanLines, false, workpieceParam.lineLen); - if ((region[2].edgeLink_1.size() < 5) || (region[2].edgeLink_2.size() < 5)) - { - *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; - return workpieceCorners; - } - 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); - if (idx0 < 0) - { - *errCode = SX_ERR_INVLD_CLOSES_PT; - return workpieceCorners; - } - _getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[3].edgeLink_1, scanLines, false, workpieceParam.lineLen); - idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true); - if (idx1 < 0) - { - *errCode = SX_ERR_INVLD_CLOSES_PT; - return workpieceCorners; - } - _getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[3].edgeLink_2, scanLines, false, workpieceParam.lineLen); - if ((region[3].edgeLink_1.size() < 5) || (region[3].edgeLink_2.size() < 5)) - { - *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; - return workpieceCorners; + //ֱȡ + 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; + } + + 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); + if (idx0 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } + _getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[0].edgeLink_1, scanLines, true, workpieceParam.lineLen); + int idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, true); + if (idx1 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } + _getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[0].edgeLink_2, scanLines, true, workpieceParam.lineLen); + if ((region[0].edgeLink_1.size() < 5) || (region[0].edgeLink_2.size() < 5)) + { + *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; + return workpieceCorners; + } + + 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); + if (idx0 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } + _getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[1].edgeLink_1, scanLines, true, workpieceParam.lineLen); + idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true); + if (idx1 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } + _getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[1].edgeLink_2, scanLines, true, workpieceParam.lineLen); + if ((region[1].edgeLink_1.size() < 5) || (region[1].edgeLink_2.size() < 5)) + { + *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; + return workpieceCorners; + } + 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); + if (idx0 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } + _getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[2].edgeLink_1, scanLines, false, workpieceParam.lineLen); + idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, false); + if (idx1 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } + _getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[2].edgeLink_2, scanLines, false, workpieceParam.lineLen); + if ((region[2].edgeLink_1.size() < 5) || (region[2].edgeLink_2.size() < 5)) + { + *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; + return workpieceCorners; + } + 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); + if (idx0 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } + _getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[3].edgeLink_1, scanLines, false, workpieceParam.lineLen); + idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true); + if (idx1 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } + _getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[3].edgeLink_2, scanLines, false, workpieceParam.lineLen); + if ((region[3].edgeLink_1.size() < 5) || (region[3].edgeLink_2.size() < 5)) + { + *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; + return workpieceCorners; + } } +#endif for (int i = 0; i < 4; i++) { @@ -618,7 +1080,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( 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(); @@ -644,6 +1106,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( } } } + diff --git a/sourceCode/BQ_workpieceCornerExtraction_Export.h b/sourceCode/BQ_workpieceCornerExtraction_Export.h index b61d217..7e501d2 100644 --- a/sourceCode/BQ_workpieceCornerExtraction_Export.h +++ b/sourceCode/BQ_workpieceCornerExtraction_Export.h @@ -40,6 +40,9 @@ typedef struct SVzNL3DPoint edge_link2_ends[2]; }SSX_debugInfo; +//汾 +SG_APISHARED_EXPORT const char* wd_BQWorkpieceCornerVersion(void); + //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 45dd066..61efce0 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -22,7 +22,10 @@ SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_changeOriginData( SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam); - +//˳Ⱥ㣺z޷, vecotr +SG_APISHARED_EXPORT void wd_vectorDataRemoveOutlier_overwrite( + std::vector& a_line, + SSG_outlierFilterParam filterParam); //˳Ⱥ㣺޷Ϊεж SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_ptDistMethod( SVzNL3DPosition* lineData, @@ -30,6 +33,7 @@ SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_ptDistMethod( SSG_outlierFilterParam filterParam, std::vector& filerData, std::vector& noisePts); + //ƽ SG_APISHARED_EXPORT void sg_lineDataSmoothing( std::vector& input, @@ -68,6 +72,24 @@ SG_APISHARED_EXPORT void sg_getLineCornerFeature_BQ( const SSG_cornerParam cornerPara, std::vector& line_features); +/// +/// ȡϵ䡢zֵVLͣ⣨PSM) +/// seg˵㣺z +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +SG_APISHARED_EXPORT void wd_getLineCornerFeature_PSM( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const double groundZ, + const SSG_cornerParam cornerPara, + SSG_lineFeature* line_features); + /// /// ȡϵJumping /// nPointIdx¶Feature @@ -133,6 +155,13 @@ SG_APISHARED_EXPORT void sg_getFeatureGrowingTrees( std::vector& trees, SSG_treeGrowParam growParam); +//feature:ȽtypeֻжλǷ +SG_APISHARED_EXPORT void wd_getFeatureGrowingTrees_noTypeMatch( + std::vector& lineFeatures, + std::vector& feature_trees, + std::vector& ending_trees, + SSG_treeGrowParam growParam); + SG_APISHARED_EXPORT void sg_lineFeaturesGrowing( int lineIdx, bool isLastLine, @@ -281,6 +310,10 @@ SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI_vector( std::vector< std::vector>& scanLines ); +//ƵROIscale: vecotrʽ +SG_APISHARED_EXPORT SWD_pointCloudPara wd_getPointCloudPara( + std::vector< std::vector>& scanLines); + //XYƽֱ SG_APISHARED_EXPORT void lineFitting( std::vector< SVzNL3DPoint>& inliers, @@ -365,4 +398,19 @@ SG_APISHARED_EXPORT void sg_pointClustering( std::vector< SVzNL3DPosition>& pts, double clusterDist, std::vector>& objClusters //result -); \ No newline at end of file +); + +//դݽXYƽϵͶӰĿհ׵вֵ +SG_APISHARED_EXPORT void pointClout2DProjection( + std::vector< std::vector>& gridScanData, + SVzNLRangeD x_range, + SVzNLRangeD y_range, + double scale, + int edgeSkip, + double inerPolateDistTh, //ֵֵڴֵIJֵ + cv::Mat& projectionData,//ͶӰݣʼΪһֵ1e+6 + cv::Mat& backIndexing //ڻ3D +); + +//ˮ㷨 +SG_APISHARED_EXPORT void watershed(SWD_waterShedImage& img); \ No newline at end of file diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 3ec07fd..be84d9e 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -4,7 +4,7 @@ #include #include -#define PI 3.141592654 +#define PI 3.14159265358979323846 // ŷǽṹ壨λȣ typedef struct{ @@ -199,6 +199,22 @@ typedef struct double bagH; // }SSG_bagParam; +typedef struct +{ + double length; // + double width; // + double height; // +}SWD_sizeParam; + +typedef struct +{ + SVzNLRangeD xRange; //< XΧ + SVzNLRangeD yRange; //< YΧ + SVzNLRangeD zRange; //< ZΧ + double scale_x; + double scale_y; +} SWD_pointCloudPara; + typedef struct { ESG_poseSortingMode sortMode; @@ -267,6 +283,15 @@ typedef struct int value; }SSG_RUN; +typedef struct +{ + int start; + int len; + int value; + bool start_zRising; //z־ + bool end_zRising;//յz־ +}SSG_RUN_EX; + typedef struct { double mean; @@ -414,3 +439,11 @@ typedef struct double forward_z; double backward_z; }SSG_pntDirAngle; + +// ͼݽṹ +typedef struct { + int width; + int height; + std::vector> gray; // Ҷͼ + std::vector> markers; // ͼ-1ʾˮ룬0ʾδǣ>0ʾ +}SWD_waterShedImage; diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index ead9928..0bd8a8b 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -5,6 +5,7 @@ #include #include +//ɨROI SVzNL3DRangeD sg_getScanDataROI( SVzNL3DLaserLine* laser3DPoints, int lineNum) @@ -65,6 +66,7 @@ SVzNL3DRangeD sg_getScanDataROI( return roi; } +//ɨROI: vecotrʽ SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector>& scanLines) { SVzNL3DRangeD roi; @@ -125,6 +127,93 @@ SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector return roi; } +//ƵROIscale: vecotrʽ +SWD_pointCloudPara wd_getPointCloudPara(std::vector< std::vector>& scanLines) +{ + SWD_pointCloudPara para; + para.xRange = { 0, -1 }; + para.yRange = { 0, -1 }; + para.zRange = { 0, -1 }; + para.scale_x = -1; //ʼֵ + para.scale_y = -1; + int lineNum = (int)scanLines.size(); + double x_scale = 0; + int x_scale_cnt = 0; + double y_scale = 0; + double y_scale_cnt = 0; + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = (int)scanLines[line].size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + if (pt3D->pt3D.z < 1e-4) + continue; + + if (i > 0) + { + if (scanLines[line][i - 1].pt3D.z > 1e-4) + { + y_scale += abs(pt3D->pt3D.y - scanLines[line][i - 1].pt3D.y); + y_scale_cnt++; + } + } + if (line > 0) + { + if (scanLines[line - 1][i].pt3D.z > 1e-4) + { + x_scale += abs(pt3D->pt3D.x - scanLines[line-1][i].pt3D.x); + x_scale_cnt++; + } + } + + if (para.xRange.max < para.xRange.min) + { + para.xRange.min = pt3D->pt3D.x; + para.xRange.max = pt3D->pt3D.x; + } + else + { + if (para.xRange.min > pt3D->pt3D.x) + para.xRange.min = pt3D->pt3D.x; + if (para.xRange.max < pt3D->pt3D.x) + para.xRange.max = pt3D->pt3D.x; + } + //y + if (para.yRange.max < para.yRange.min) + { + para.yRange.min = pt3D->pt3D.y; + para.yRange.max = pt3D->pt3D.y; + } + else + { + if (para.yRange.min > pt3D->pt3D.y) + para.yRange.min = pt3D->pt3D.y; + if (para.yRange.max < pt3D->pt3D.y) + para.yRange.max = pt3D->pt3D.y; + } + //z + if (para.zRange.max < para.zRange.min) + { + para.zRange.min = pt3D->pt3D.z; + para.zRange.max = pt3D->pt3D.z; + } + else + { + if (para.zRange.min > pt3D->pt3D.z) + para.zRange.min = pt3D->pt3D.z; + if (para.zRange.max < pt3D->pt3D.z) + para.zRange.max = pt3D->pt3D.z; + } + } + } + if (x_scale_cnt > 0) + para.scale_x = x_scale / (double)x_scale_cnt; + if (y_scale_cnt > 0) + para.scale_y = y_scale / (double)y_scale_cnt; + return para; +} + void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b) { //Сֱ߲ @@ -1979,4 +2068,98 @@ void lineDataRT_RGBD(SVzNLXYZRGBDLaserLine* a_line, const double* camPoseR, doub a_line->p3DPoint[i] = a_pt; } return; -} \ No newline at end of file +} + +//դݽXYƽϵͶӰĿհ׵вֵ +void pointClout2DProjection( + std::vector< std::vector>& gridScanData, + SVzNLRangeD x_range, + SVzNLRangeD y_range, + double scale, + int edgeSkip, + double inerPolateDistTh, //ֵֵڴֵIJֵ + cv::Mat& projectionData,//ͶӰݣʼΪһֵ1e+6 + cv::Mat& backIndexing //ڻ3D +) +{ + int lineNum = (int)gridScanData.size(); + if (lineNum == 0) + return; + int nPointCnt = (int)gridScanData[0].size(); + for (int line = 0; line < lineNum; line++) + { + int pre_x = -1, pre_y = -1; + SVzNL3DPosition* prePt = NULL; + for (int i = 0; i < nPointCnt; i++) + { + SVzNL3DPosition* pt3D = &gridScanData[line][i]; + if (pt3D->pt3D.z < 1e-4) + continue; + double x = pt3D->pt3D.x; + double y = pt3D->pt3D.y; + int px = (int)(x - x_range.min)/scale + edgeSkip; + int py = (int)(y - y_range.min)/scale + edgeSkip; + + cv::Vec2i v2i_exist = backIndexing.at(py, px); +#if 0 + if ((v2i_exist[0] > 0) || (v2i_exist[1] > 0)) //ظͶӰͬһϣֻһЧ + { + pt3D->pt3D.z = 0; //invalidate + } + else +#endif + { + cv::Vec2i v2i = { line, i }; + backIndexing.at(py, px) = v2i; + projectionData.at(py, px) = 1e+6; + //ֱֵ + if (prePt) + { + //룬һ򲻲ֵ + double dist = sqrt(pow(pt3D->pt3D.x - prePt->pt3D.x, 2) + + pow(pt3D->pt3D.y - prePt->pt3D.y, 2) + + pow(pt3D->pt3D.z - prePt->pt3D.z, 2)); + if (dist < inerPolateDistTh) + { + std::vector interPts; + drawLine( + pre_x, + pre_y, + px, + py, + interPts); + for (int m = 0, m_max = (int)interPts.size(); m < m_max; m++) + projectionData.at(interPts[m].y, interPts[m].x) = 1e+6; + } + } + prePt = pt3D; + pre_x = px; + pre_y = py; + } + } + } + //ˮƽֵ + int pixWin = (int)(inerPolateDistTh / scale); + for (int y = 0; y < projectionData.rows; y++) + { + int pre_x = -1; + for (int x = 0; x < projectionData.cols; x++) + { + double value = projectionData.at(y, x); + if (value > 1e-4) + { + if (pre_x >= 0) + { + //ֵ + int x_diff = x - pre_x; + if ((x_diff > 1) && (x_diff < pixWin)) + { + for (int m = pre_x + 1; m < x; m++) + projectionData.at(y, m) = 1e+6; + } + } + pre_x = x; + } + } + } +} diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index a35b02a..afcf3bf 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -11,4 +11,7 @@ #define SX_ERR_INVLD_VTREE_NUM -2001 #define SX_ERR_INVLD_HTREE_NUM -2002 #define SX_ERR_INVLD_EDGE_LINK_NUM -2003 -#define SX_ERR_INVLD_CLOSES_PT -2004 \ No newline at end of file +#define SX_ERR_INVLD_CLOSES_PT -2004 +#define SX_ERR_ZERO_CONTOUR_PT -2005 +#define SX_ERR_INVLID_RPEAK_NUM -2006 +#define SX_ERR_INVLID_RPEAK_PAIR -2007 \ No newline at end of file diff --git a/sourceCode/SG_featureGrow.cpp b/sourceCode/SG_featureGrow.cpp index 465dbfd..71f9b5c 100644 --- a/sourceCode/SG_featureGrow.cpp +++ b/sourceCode/SG_featureGrow.cpp @@ -82,6 +82,37 @@ bool _featureGrowing(SSG_basicFeature1D& a_feature, const int lineIdx, std::vect return false; } +//featuretreesѰҺʵûкʵ㣬 false +//ûʹȫƥ䡣һfeatureһƥϣƥɡûʹƥ䡣 +bool _featureGrowing_noTypeMatch(SSG_basicFeature1D& a_feature, const int lineIdx, std::vector& trees, SSG_treeGrowParam growParam) +{ + for (int i = 0, i_max = (int)trees.size(); i < i_max; i++) + { + SSG_featureTree& a_tree = trees[i]; + if (TREE_STATE_DEAD == a_tree.treeState) + continue; + // + SSG_basicFeature1D last_node = a_tree.treeNodes.back(); + if (last_node.jumpPos2D.x == a_feature.jumpPos2D.x) //xΪlineIdxͬһɨϵIJ + continue; + + //ж + double y_diff = abs(a_feature.jumpPos.y - last_node.jumpPos.y); + double z_diff = abs(a_feature.jumpPos.z - last_node.jumpPos.z); + int line_diff = abs(a_feature.jumpPos2D.x - last_node.jumpPos2D.x); + double x_diff = abs(a_feature.jumpPos.x - last_node.jumpPos.x); + if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) && + ((line_diff < growParam.maxLineSkipNum) || (x_diff < growParam.maxSkipDistance))) + { + a_tree.eLineIdx = lineIdx; + a_tree.treeNodes.push_back(a_feature); + a_tree.tree_value += a_feature.featureValue; + return true; + } + } + return false; +} + int _getMatchedTree_angleCheck( SSG_basicFeature1D& a_feature, const int lineIdx, @@ -481,6 +512,95 @@ void sg_getFeatureGrowingTrees( } } +void wd_getFeatureGrowingTrees_noTypeMatch( + std::vector& lineFeatures, + std::vector& feature_trees, + std::vector& ending_trees, + SSG_treeGrowParam growParam) +{ + for (int i = 0, i_max = (int)lineFeatures.size(); i < i_max; i++) + { + SSG_lineFeature& a_line = lineFeatures[i]; + if (a_line.features.size() > 0) + { + for (int j = 0, j_max = (int)a_line.features.size(); j < j_max; j++) + { + SSG_basicFeature1D& a_feature = a_line.features[j]; + if (a_feature.jumpPos2D.x == 207) + int kkk = 1; + bool isMatched = _featureGrowing_noTypeMatch(a_feature, a_line.lineIdx, feature_trees, growParam); + if (false == isMatched) + { + //µ + SSG_featureTree a_newTree; + a_newTree.treeNodes.push_back(a_feature); + a_newTree.treeState = TREE_STATE_ALIVE; + a_newTree.treeType = a_feature.featureType; + a_newTree.sLineIdx = a_line.lineIdx; + a_newTree.eLineIdx = a_line.lineIdx; + feature_trees.push_back(a_newTree); + } + } + } + if (a_line.endings.size() > 0) + { + for (int j = 0, j_max = (int)a_line.endings.size(); j < j_max; j++) + { + SSG_basicFeature1D& a_feature = a_line.endings[j]; + if (a_feature.jumpPos2D.x == 207) + int kkk = 1; + bool isMatched = _featureGrowing_noTypeMatch(a_feature, a_line.lineIdx, ending_trees, growParam); + if (false == isMatched) + { + //µ + SSG_featureTree a_newTree; + a_newTree.treeNodes.push_back(a_feature); + a_newTree.treeState = TREE_STATE_ALIVE; + a_newTree.treeType = a_feature.featureType; + a_newTree.sLineIdx = a_line.lineIdx; + a_newTree.eLineIdx = a_line.lineIdx; + ending_trees.push_back(a_newTree); + } + } + } + //ֹͣ,١ + //ڵΪ1Ƴ + int lineIdx = a_line.lineIdx; + int m_max = (int)feature_trees.size(); + for (int m = m_max - 1; m >= 0; m--) //ӺǰɾӰѭ + { + if (TREE_STATE_ALIVE == feature_trees[m].treeState) + { + int line_diff = abs(lineIdx - feature_trees[m].treeNodes.back().jumpPos2D.x); + if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) || + (i == i_max - 1)) + { + feature_trees[m].treeState = TREE_STATE_DEAD; + bool isValid = _invalidateVSlopeTrees(feature_trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen); + if (false == isValid) + feature_trees.erase(feature_trees.begin() + m); + } + } + } + m_max = (int)ending_trees.size(); + for (int m = m_max - 1; m >= 0; m--) //ӺǰɾӰѭ + { + if (TREE_STATE_ALIVE == ending_trees[m].treeState) + { + int line_diff = abs(lineIdx - ending_trees[m].treeNodes.back().jumpPos2D.x); + if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) || + (i == i_max - 1)) + { + ending_trees[m].treeState = TREE_STATE_DEAD; + bool isValid = _invalidateVSlopeTrees(ending_trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen); + if (false == isValid) + ending_trees.erase(ending_trees.begin() + m); + } + } + } + } +} + void sg_lineFeaturesGrowing( int lineIdx, bool isLastLine, @@ -811,7 +931,6 @@ if (edge1_pts[i].x == 622) #endif } - void sg_getPeakGrowingTrees( std::vector>& peakFeatures, std::vector& trees, diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 60bfa39..ad58a88 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -261,6 +261,62 @@ void sg_lineDataRemoveOutlier_changeOriginData( return; } +//˳Ⱥ㣺z޷, vecotr +void wd_vectorDataRemoveOutlier_overwrite( + std::vector& a_line, + SSG_outlierFilterParam filterParam) +{ + int dataSize = (int)a_line.size(); + std::vector< SSG_RUN> continueRuns; + SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx + double pre_z = 0; + for (int i = 0; i < dataSize; i++) + { + if (i == 370) + int kkk = 1; + if (a_line[i].pt3D.z > 1e-4) + { + if (a_run.len < 0) + { + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + else + { + double z_diff = abs(a_line[i].pt3D.z - pre_z); + if (z_diff < filterParam.continuityTh) + { + a_run.len++; + a_run.value = i; + } + else + { + continueRuns.push_back(a_run); + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + } + pre_z = a_line[i].pt3D.z; + } + } + if (a_run.len > 0) + continueRuns.push_back(a_run); + + for (int i = 0, i_max = (int)continueRuns.size(); i < i_max; i++) + { + if (continueRuns[i].len < filterParam.outlierTh) // + { + for (int j = continueRuns[i].start; j <= continueRuns[i].value; j++) + { + a_line[j].pt3D.z = 0; + } + } + } + return; +} + //˳Ⱥ㣺뷽 void sg_lineDataRemoveOutlier_ptDistMethod(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam, std::vector& filerData, std::vector& noisePts) { @@ -1134,6 +1190,7 @@ void sg_getLineLVFeature(SVzNL3DPosition* lineData, int dataSize, int lineIdx, c /// /// ȡϵĹյ +/// seg˵㣺z /// nPointIdx¶Feature /// 㷨̣ /// 1ǰǺͺ @@ -1445,6 +1502,7 @@ void sg_getLineCornerFeature( /// /// ȡϵĹյˮƽ˵ΪCorner +/// seg˵㣺zy /// Segcornerȡ /// nPointIdx¶Feature /// 㷨̣ @@ -1787,11 +1845,351 @@ void sg_getLineCornerFeature_BQ( return; } +/// +/// ȡϵ䡢zֵVLͣ⣨PSM) +/// seg˵㣺z +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +void wd_getLineCornerFeature_PSM( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const double groundZ, //ƽZgrondZĵΪĿ߽ + const SSG_cornerParam cornerPara, + SSG_lineFeature* line_features) +{ + line_features->lineIdx = lineIdx; + if (lineIdx == 538) + int kkk = 1; + + //groundZнض + for (int i = 0; i < dataSize; i++) + { + if (lineData[i].pt3D.z > groundZ) + lineData[i].pt3D.z = 0; + } + + //ȥ + std::vector< SVzNL3DPosition> vldPts; + std::vector< int> vldPtSegIdx; + std::vector segs; + std::vector backIndexing; + backIndexing.resize(dataSize); + + int runIdx = 1; + SSG_RUN_EX a_run = { 0, -1, 0, false, false }; //startIdx, len, lastIdx + double pre_z = 0; + double pre_y = 0; + for (int i = 0; i < dataSize; i++) + { + if (i == 370) + int kkk = 1; + if (lineData[i].pt3D.z > 1e-4) + { + if (a_run.len < 0) + { + a_run.start_zRising = true; + a_run.start = i; + a_run.len = 1; + a_run.end_zRising = true; + a_run.value = i; + } + else + { + double z_diff = abs(lineData[i].pt3D.z - pre_z); + if (z_diff < cornerPara.minEndingGap_z) + { + a_run.len = i - a_run.start + 1; + a_run.value = i; + } + else + { + bool next_zRising; + if (pre_z > lineData[i].pt3D.z) + { + a_run.end_zRising = true; + next_zRising = false; + } + else + { + a_run.end_zRising = false; + next_zRising = true; + } + a_run.value = runIdx; + runIdx++; + segs.push_back(a_run); + + a_run.start = i; + a_run.start_zRising = next_zRising; + a_run.len = 1; + a_run.value = i; + a_run.end_zRising = true; + } + } + int bIdx = (int)vldPts.size(); + backIndexing[i] = bIdx; + vldPts.push_back(lineData[i]); + vldPtSegIdx.push_back(runIdx); + + pre_z = lineData[i].pt3D.z; + } + } + if (a_run.len > 0) + segs.push_back(a_run); + + //ñ־ + for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) + { + int idx1 = segs[i].start; + int idx2 = segs[i].start + segs[i].len - 1; + lineData[idx1].nPointIdx |= 0x100000; + lineData[idx2].nPointIdx |= 0x200000; + } + + //ǰǺͺ + std::vector< SSG_pntDirAngle> corners; + corners.resize(vldPts.size()); + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + if ((lineIdx == 399) && (i == 419)) + int kkk = 1; + //ǰѰ + int pre_i = -1; + for (int j = i - 1; j >= 0; j--) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (dist >= cornerPara.scale) + { + pre_i = j; + break; + } + } + //Ѱ + int post_i = -1; + for (int j = i + 1; j < i_max; j++) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (dist >= cornerPara.scale) + { + post_i = j; + break; + } + } + //ս + if ((pre_i < 0) || (post_i < 0)) + { + corners[i].pntIdx = -1; + corners[i].forwardAngle = 0; + corners[i].backwardAngle = 0; + corners[i].corner = 0; + corners[i].forwardDiffZ = 0; + corners[i].backwardDiffZ = 0; + } + else + { + double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y); + double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y); + double forwardAngle = atan(tanValue_post) * 180.0 / PI; + double backwardAngle = atan(tanValue_pre) * 180.0 / PI; + corners[i].pntIdx = i; + corners[i].forwardAngle = forwardAngle; + corners[i].backwardAngle = backwardAngle; + corners[i].corner = -(forwardAngle - backwardAngle); //ͼϵϵy෴С- + corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z; + corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z; + } + } + + //սǼֵ + int _state = 0; + int pre_i = -1; + int sEdgePtIdx = -1; + int eEdgePtIdx = -1; + SSG_pntDirAngle* pre_data = NULL; + std::vector< SSG_pntDirAngle> cornerPeakP; + std::vector< SSG_pntDirAngle> cornerPeakM; + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + if (i == 275) + int kkk = 1; + SSG_pntDirAngle* curr_data = &corners[i]; + if (curr_data->pntIdx < 0) + { + if (i == i_max - 1) //һ + { + if (1 == _state) // + { + cornerPeakP.push_back(corners[eEdgePtIdx]); + } + else if (2 == _state) //½ + { + cornerPeakM.push_back(corners[eEdgePtIdx]); + } + } + continue; + } + + if (NULL == pre_data) + { + sEdgePtIdx = i; + eEdgePtIdx = i; + pre_data = curr_data; + pre_i = i; + continue; + } + + eEdgePtIdx = i; + double cornerDiff = curr_data->corner - pre_data->corner; + switch (_state) + { + case 0: //̬ + if (cornerDiff < 0) //½ + { + _state = 2; + } + else if (cornerDiff > 0) // + { + _state = 1; + } + break; + case 1: // + if (cornerDiff < 0) //½ + { + cornerPeakP.push_back(*pre_data); + _state = 2; + } + break; + case 2: //½ + if (cornerDiff > 0) // + { + cornerPeakM.push_back(*pre_data); + _state = 1; + } + break; + default: + _state = 0; + break; + } + pre_data = curr_data; + pre_i = i; + } + //ע⣺һΪλ + + //Сֵ㣨嶥 + //ֵȽϣڳ߶ȴѰҾֲֵ + double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2cornerScale + for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++) + { + if (cornerPeakP[i].corner < cornerPara.cornerTh) + continue; + + bool isPeak = true; + //ǰ + int cornerPtIdx = cornerPeakP[i].pntIdx; + for (int j = i - 1; j >= 0; j--) + { + int prePtIdx = cornerPeakP[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ; + if (dist > square_distTh) //߶ȴ + break; + + if (cornerPeakP[i].corner < cornerPeakP[j].corner) + { + isPeak = false; + break; + } + } + // + if (true == isPeak) + { + cornerPtIdx = cornerPeakP[i].pntIdx; + for (int j = i + 1; j < i_max; j++) + { + int postPtIdx = cornerPeakP[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); + if (dist > square_distTh) //߶ȴ + break; + + if (cornerPeakP[i].corner < cornerPeakP[j].corner) + { + isPeak = false; + break; + } + } + } + if (true == isPeak) + { + SSG_basicFeature1D a_feature; + if ((cornerPeakP[i].backwardAngle > cornerPara.jumpCornerTh_1) && (cornerPeakP[i].forwardAngle > -cornerPara.jumpCornerTh_2)) + a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; + else if ((cornerPeakP[i].forwardAngle < -cornerPara.jumpCornerTh_1) && (cornerPeakP[i].backwardAngle < cornerPara.jumpCornerTh_2)) + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + else + a_feature.featureType = LINE_FEATURE_CORNER_V; + + + a_feature.jumpPos = vldPts[cornerPtIdx].pt3D; + a_feature.jumpPos2D = { lineIdx, vldPts[cornerPtIdx].nPointIdx }; + line_features->features.push_back(a_feature); + } + } + + //ӿʼͽ߽ + //segǷҪϲϲ + for (int i = 0, i_max = (int)segs.size(); i < i_max - 1; i++) + { + SSG_RUN_EX* nxt_seg = &segs[i + 1]; + SSG_RUN_EX* curr_seg = &segs[i]; + + int idx_1 = curr_seg->start + curr_seg->len - 1; + int idx_2 = nxt_seg->start; + double y_diff = abs(lineData[idx_1].pt3D.y - lineData[idx_2].pt3D.y); + double z_diff = abs(lineData[idx_1].pt3D.z - lineData[idx_2].pt3D.z); + if ((y_diff < cornerPara.minEndingGap) && (z_diff < cornerPara.minEndingGap_z)) //ϲ + { + int idx_end = nxt_seg->start + nxt_seg->len - 1; + nxt_seg->start = curr_seg->start; + nxt_seg->len = idx_end - curr_seg->start + 1; + curr_seg->value = 0; + } + } + for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) + { + if (0 == segs[i].value) //ϲ + continue; + + int idx_1 = segs[i].start; + int idx_2 = segs[i].start + segs[i].len - 1; + + SSG_basicFeature1D an_edge; + memset(&an_edge, 0, sizeof(SSG_basicFeature1D)); + an_edge.featureType = LINE_FEATURE_LINE_ENDING_0; + an_edge.jumpPos = lineData[idx_1].pt3D; + an_edge.jumpPos2D = { lineIdx, idx_1 }; + line_features->endings.push_back(an_edge); + //line_features.insert(line_features.begin(), an_edge); //ͷ + //β + an_edge.featureType = LINE_FEATURE_LINE_ENDING_1; + an_edge.jumpPos = lineData[idx_2].pt3D; + an_edge.jumpPos2D = { lineIdx, idx_2 }; + line_features->endings.push_back(an_edge); + } + return; +} + bool compareByIdx(const SSG_pntDirAngle& a, const SSG_pntDirAngle& b) { return a.pntIdx < b.pntIdx; } /// -/// ȡϵJumping +/// ȡϵJumping, ڴӺ +/// seg˵㣺z /// nPointIdx¶Feature /// 㷨̣ /// 1ǰǺͺ diff --git a/sourceCode/SG_regionGrow.cpp b/sourceCode/SG_regionGrow.cpp index 0f615fd..f6692ce 100644 --- a/sourceCode/SG_regionGrow.cpp +++ b/sourceCode/SG_regionGrow.cpp @@ -1359,4 +1359,3 @@ void sg_getLocalPeaks_distTransform(cv::Mat& input, std::vector& p } return; } - From bafb08b41ecb062ae70b0ab7a89c4a6fc7b2b5b8 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Sat, 8 Nov 2025 09:55:15 +0800 Subject: [PATCH 07/12] =?UTF-8?q?=E6=B7=BB=E5=8A=A0watershed=E7=AE=97?= =?UTF-8?q?=E6=B3=95=E5=92=8C=E7=B2=92=E5=BE=84=E6=B5=8B=E9=87=8F=E7=AE=97?= =?UTF-8?q?=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/WD_particleSizeMeasure.cpp | 1105 ++++++++++++++++++++ sourceCode/WD_particleSizeMeasure_Export.h | 54 + sourceCode/WD_watershed.cpp | 366 +++++++ 3 files changed, 1525 insertions(+) create mode 100644 sourceCode/WD_particleSizeMeasure.cpp create mode 100644 sourceCode/WD_particleSizeMeasure_Export.h create mode 100644 sourceCode/WD_watershed.cpp diff --git a/sourceCode/WD_particleSizeMeasure.cpp b/sourceCode/WD_particleSizeMeasure.cpp new file mode 100644 index 0000000..747c5ba --- /dev/null +++ b/sourceCode/WD_particleSizeMeasure.cpp @@ -0,0 +1,1105 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "WD_particleSizeMeasure_Export.h" +#include +#include + +//һƽƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SSG_planeCalibPara wd_getBaseCalibPara( + std::vector< std::vector>& scanLines) +{ + return sg_getPlaneCalibPara2(scanLines); +} + +//̬ƽȥ +void wd_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH) +{ + lineDataRT_vector(a_line, camPoseR, groundH); +} + +void wd_noiseFilter( + std::vector< std::vector>& scanLines, + const SWD_PSM_algoParam algoParam, + int* errCode) +{ + *errCode = 0; + int lineNum = (int)scanLines.size(); + int nPointCnt = (int)scanLines[0].size(); + bool vldGrid = true; + for (int i = 0; i < lineNum; i++) + { + if (nPointCnt != (int)scanLines[i].size()) + vldGrid = false; + wd_vectorDataRemoveOutlier_overwrite( + scanLines[i], + algoParam.filterParam); + } + if (false == vldGrid) + { + *errCode = SG_ERR_3D_DATA_INVLD; + return; + } + //ˮƽ + int hLineNum = nPointCnt; //Gridʽɨߵĵһ + //ˮƽɨ + std::vector> filterHLines; + filterHLines.resize(hLineNum); + for (int i = 0; i < hLineNum; i++) + filterHLines[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < hLineNum; j++) + { + filterHLines[j][line] = scanLines[line][j]; + filterHLines[j][line].pt3D.x = scanLines[line][j].pt3D.y; + filterHLines[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + for (int hLine = 0; hLine < hLineNum; hLine++) + { + //˲˳쳣 + std::vector filterData; + std::vector lineNoise; + sg_lineDataRemoveOutlier( + (SVzNL3DPosition*)filterHLines[hLine].data(), + (int)filterHLines[hLine].size(), + algoParam.filterParam, + filterData, + lineNoise); + for (int j = 0; j < lineNoise.size(); j++) + { + int lineIdx = lineNoise[j]; + scanLines[lineIdx][hLine].pt3D.z = 0; + } + } + return; +} + +// +void wd_particleSizeMeasure( + std::vector< std::vector>& scanLines, + const SWD_paricleSizeParam particleSizeParam, + const SSG_planeCalibPara groundCalibPara, + const SWD_PSM_algoParam algoParam, + std::vector& particles, + int* errCode) +{ + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + { + *errCode = SG_ERR_3D_DATA_NULL; + return; + } + // + //ֱ + wd_noiseFilter(scanLines, algoParam, errCode); + if (*errCode != 0) + return; + + ///ʼݴ + //ȡ䡢zֵVL + //ֱݴ + std::vector all_vLineFeatures; + for (int i = 0; i < lineNum; i++) + { + if (i == 202) + int k = 1; + SSG_lineFeature a_line_features; + a_line_features.lineIdx = i; + wd_getLineCornerFeature_PSM( + &scanLines[i][0], + (int)scanLines[i].size(), + i, + groundCalibPara.planeHeight, + algoParam.cornerParam, + &a_line_features); + + all_vLineFeatures.push_back(a_line_features); //Ҳ룬֤ܰк + } + //ˮƽɨ + int nPointCnt = (int)scanLines[0].size(); + std::vector> hLines; + hLines.resize(nPointCnt); + for (int i = 0; i < nPointCnt; i++) + hLines[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < nPointCnt; j++) + { + scanLines[line][j].nPointIdx = 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; + } + } + std::vector all_hLineFeatures; + //ȡ + for (int hLine = 0; hLine < nPointCnt; hLine++) + { + if (hLine == 14) + int kkk = 1; + + SSG_lineFeature a_hLine_featrues; + a_hLine_featrues.lineIdx = hLine; + + wd_getLineCornerFeature_PSM( + &hLines[hLine][0], + (int)hLines[hLine].size(), + hLine, + groundCalibPara.planeHeight, + algoParam.cornerParam, + &a_hLine_featrues); + + //if ((a_hLine_featrues.features.size() > 0) || (a_hLine_featrues.endings.size() > 0)) + all_hLineFeatures.push_back(a_hLine_featrues);//Ҳ룬֤ܰк + } + + /// ͳҰС + SWD_pointCloudPara pntCloudPara = wd_getPointCloudPara(scanLines); + SVzNLRangeD x_range = pntCloudPara.xRange; + SVzNLRangeD y_range = pntCloudPara.yRange; + + SSG_ROIRectD globalROI; + globalROI.left = pntCloudPara.xRange.min; + globalROI.right = pntCloudPara.xRange.max; + globalROI.top = pntCloudPara.yRange.min; + globalROI.bottom = pntCloudPara.yRange.max; + + //ֱɨ跽 + std::vector v_feature_trees; + std::vector v_ending_trees; + // + wd_getFeatureGrowingTrees_noTypeMatch( + all_vLineFeatures, + v_feature_trees, + v_ending_trees, + algoParam.growParam); + + //ˮƽ + std::vector h_feature_trees; + std::vector h_ending_trees; + // + wd_getFeatureGrowingTrees_noTypeMatch( + all_hLineFeatures, + h_feature_trees, + h_ending_trees, + algoParam.growParam); + + //Mask + double scale; + if ((pntCloudPara.scale_x < 0) || (pntCloudPara.scale_y < 0)) + { + *errCode = SG_ERR_3D_DATA_INVLD; + return; + } + if(pntCloudPara.scale_x < pntCloudPara.scale_y) + scale = pntCloudPara.scale_x; + else + scale = pntCloudPara.scale_y; + + + //任Mask1mmΪ߶ + double inerPolateDistTh = scale * 10; //ֵޣ ڴֵֵ + int edgeSkip = 2; + int maskX = (int)(x_range.max - x_range.min)/scale + 1; + int maskY = (int)(y_range.max - y_range.min)/scale + 1; + if ((maskX < 16) || (maskY < 16)) + return; + maskY = maskY + edgeSkip * 2; + maskX = maskX + edgeSkip * 2; + cv::Mat distTranformMask(maskY, maskX, CV_32FC1, 0.0f); //任MaskʼΪһֵ1e+6 + cv::Mat distTranformIndexing(maskY, maskX, CV_32SC2, cv::Vec2i(0, 0)); // + cv::Mat featureMask = cv::Mat::zeros(nPointCnt, lineNum, CV_32SC4); + pointClout2DProjection( + scanLines, + x_range, + y_range, + scale, + edgeSkip, + inerPolateDistTh, //ֵֵڴֵIJֵ + distTranformMask,//ͶӰݣʼΪһֵ1e+6 + distTranformIndexing //ڻ3D + ); + + std::vector allTreesInfo; //߽ + //ע:ֱ + SSG_treeInfo a_nullTree; + memset(&a_nullTree, 0, sizeof(SSG_treeInfo)); + allTreesInfo.push_back(a_nullTree); //ִ洢λtreeIdxͬλã + //עֱ߽ + int treeID = 1; + for (int i = 0, i_max = (int)v_ending_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_vEdgeTree = &v_ending_trees[i]; + + //¼TreeϢ + SSG_treeInfo a_treeInfo; + a_treeInfo.vTreeFlag = 1; + a_treeInfo.treeIdx = treeID; + a_treeInfo.treeType = a_vEdgeTree->treeType; + a_treeInfo.sLineIdx = a_vEdgeTree->sLineIdx; + a_treeInfo.eLineIdx = a_vEdgeTree->eLineIdx; + a_treeInfo.roi = a_vEdgeTree->roi; + allTreesInfo.push_back(a_treeInfo); + //ԭʼϱǣͬʱMaskϱ + for (int j = 0, j_max = (int)a_vEdgeTree->treeNodes.size(); j < j_max; j++) + { + SSG_basicFeature1D* a_feature = &a_vEdgeTree->treeNodes[j]; + if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//Ŀ˺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 += treeID << 16; + featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[0] = treeID; //edgeID + featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vEdgeTree->treeType; + featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1; //vscan + + int px = (int)((scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.x - x_range.min)/scale) + edgeSkip; + int py = (int)((scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.y - y_range.min)/scale) + edgeSkip; + distTranformMask.at(py, px) = 0; + } + } + treeID++; + } + //עˮƽ߽ + for (int i = 0, i_max = (int)h_ending_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_hEdgeTree = &h_ending_trees[i]; + //¼TreeϢ + SSG_treeInfo a_treeInfo; + a_treeInfo.vTreeFlag = 0; + a_treeInfo.treeIdx = treeID; + a_treeInfo.treeType = a_hEdgeTree->treeType; + a_treeInfo.sLineIdx = a_hEdgeTree->sLineIdx; + a_treeInfo.eLineIdx = a_hEdgeTree->eLineIdx; + a_treeInfo.roi.left = a_hEdgeTree->roi.top; //ˮƽɨxyǽ + a_treeInfo.roi.right = a_hEdgeTree->roi.bottom; + a_treeInfo.roi.top = a_hEdgeTree->roi.left; + a_treeInfo.roi.bottom = a_hEdgeTree->roi.right; + allTreesInfo.push_back(a_treeInfo); + //ԭʼϱǣͬʱMaskϱ + for (int j = 0, j_max = (int)a_hEdgeTree->treeNodes.size(); j < j_max; j++) + { + SSG_basicFeature1D* a_feature = &a_hEdgeTree->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 += treeID << 16; + featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[0] = treeID; + featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hEdgeTree->treeType << 4; + featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag + + int px = (int)((scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.x - x_range.min)/scale) + edgeSkip; + int py = (int)((scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.y - y_range.min)/scale) + edgeSkip; + distTranformMask.at(py, px) = 0; + } + } + } + treeID++; + } + //ֱע + int hvTreeIdx = treeID; + int vTreeStart = treeID; + for (int i = 0, i_max = (int)v_feature_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_vTree = &v_feature_trees[i]; + + //¼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); + //ԭʼϱǣͬʱ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; + featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[0] = hvTreeIdx; //edgeID + featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vTree->treeType; + featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1; //vscan + + int px = (int)((scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.x - x_range.min)/scale) + edgeSkip; + int py = (int)((scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.y - y_range.min)/scale) + edgeSkip; + distTranformMask.at(py, px) = 0; + } + } + } + hvTreeIdx++; + } + int hTreeStart = hvTreeIdx; + //ע:ˮƽ + for (int i = 0, i_max = (int)h_feature_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_hTree = &h_feature_trees[i]; + //¼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); + //ԭʼϱǣͬʱ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; + featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[0] = hvTreeIdx; + featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hTree->treeType << 4; + featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag + + int px = (int)((scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.x - x_range.min)/scale) + edgeSkip; + int py = (int)((scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.y - y_range.min)/scale) + edgeSkip; + distTranformMask.at(py, px) = 0; + } + } + } + hvTreeIdx++; + } + int hvTreeSize = hvTreeIdx; + double x_scale = pntCloudPara.scale_x; + double y_scale = pntCloudPara.scale_y; + + //о任Ȼʹ÷ˮ㷨зָ + cv::Mat distTransform; + sg_distanceTrans(distTranformMask, distTransform, 0); +#if OUTPUT_DEBUG //debug + cv::Mat maskImage; + cv::normalize(distTranformMask, maskImage, 0, 255, cv::NORM_MINMAX, CV_8U); + cv::imwrite("distTransformMask.png", maskImage); + cv::Mat dtImage; + cv::normalize(distTransform, dtImage, 0, 255, cv::NORM_MINMAX, CV_8U); + cv::imwrite("distTransform.png", dtImage); +#endif + + //ѰPeakPeakȷPeakΪӵзˮ뷽ָ + double minW = particleSizeParam.minSize.width; + SSG_localPkParam searchWin; + searchWin.seachW_lines = (int)(minW * 0.4/scale); + searchWin.searchW_pts = (int)(minW * 0.4/scale); + std::vector dt_peaks; + sg_getLocalPeaks_distTransform(distTransform, dt_peaks, searchWin); + //ԴСй + double minDistTh = minW * 0.4 / scale; + std::vector filter_dt_peaks; + for (int i = 0, i_max = (int)dt_peaks.size(); i < i_max; i++) + { + if (dt_peaks[i].valueD > minDistTh) + filter_dt_peaks.push_back(dt_peaks[i]); + } + //Ծйˣ任൱СԲͬĿڵԲһཻˣR1+R2 < Բľ + int filterSize = (int)filter_dt_peaks.size(); + for (int i = 0; i < filterSize; i++) + { + for (int j = i + 1; j < filterSize; j++) + { + + } + } + + //ˮָ + + //ɨ߽磬Ŀ߽缯 + //ͨĿڱ߽жǷϲĿ + + + //ȡСֵ + double minVal, maxVal; + cv::Point minLoc, maxLoc; + // minMaxLoc + cv::minMaxLoc(distTransform, &minVal, &maxVal, &minLoc, &maxLoc); + + //ˮ㷨зָ + SWD_waterShedImage wsImg; + wsImg.width = distTransform.cols; + wsImg.height = distTransform.rows; + wsImg.gray.resize(wsImg.height, std::vector(wsImg.width)); + wsImg.markers.resize(wsImg.height, std::vector(wsImg.width, 1)); // ʼͼΪ0 + int maxValue = (int)maxVal + 2; + for (int i = 0; i < distTransform.rows; i++) + { + float* rowPtr = distTransform.ptr(i); + for (int j = 0; j < distTransform.cols; j++) + { + float disValue = rowPtr[j]; + if (disValue < 1e-4) //߽ͱ + wsImg.gray[i][j] = maxValue; + else + { + wsImg.gray[i][j] = (int)(maxVal - disValue); + wsImg.markers[i][j] = 0; + } + } + } + + watershed(wsImg); + +#if OUTPUT_DEBUG //debug + cv::Mat waterShedResult(wsImg.height, wsImg.width, CV_8UC3); + for (int i = 0; i < wsImg.height; ++i) { + for (int j = 0; j < wsImg.width; ++j) { + if (wsImg.markers[i][j] == -1) { // ˮ߽磨ɫ + waterShedResult.at(i, j) = cv::Vec3b(0,0,255); + } + else { // 򣨸ݱֵɲͬɫ + int color_r = (wsImg.markers[i][j] * 50) % 256; + int color_g = (color_r + 85) % 256; + int color_b = (color_r + 170) % 256; + waterShedResult.at(i, j) = cv::Vec3b(color_b, color_g, color_r); + } + } + } + cv::imwrite("watershed.png", waterShedResult); +#endif +#if 0 + + + + + + + SSG_localPkParam searchWin; + searchWin.seachW_lines = (int)(algoParam.bagParam.bagW * 0.4); + searchWin.searchW_pts = (int)(algoParam.bagParam.bagW * 0.4); + std::vector dt_peaks; + sg_getLocalPeaks_distTransform(distTransform, dt_peaks, searchWin); + //ȡPeaks + int invlidDistToEdge = 50; //ԵPeakǷǷ㡣 + double minPeakValue = algoParam.bagParam.bagW / 8; + std::vector peaks; + for (int i = 0; i < dt_peaks.size(); i++) + { + //߽紦PeakΪϸPeakȥ + int x_diff_0 = dt_peaks[i].x; //߾룬λΪmm߶Ϊ1mm + int x_diff_1 = distTransform.cols - dt_peaks[i].x;//ұ߾ + int y_diff_0 = dt_peaks[i].y;//ϱ߾ + int y_diff_1 = distTransform.rows - dt_peaks[i].y;//±߾ + if ((x_diff_0 < invlidDistToEdge) || (x_diff_1 < invlidDistToEdge) || + (y_diff_0 < invlidDistToEdge) || (y_diff_1 < invlidDistToEdge) || + (dt_peaks[i].valueD < minPeakValue)) + continue; + + //distTranformIndexingл + double pkValue = dt_peaks[i].valueD; + SSG_2DValueI a_peak = _backIndexingPeakPos(dt_peaks[i], distTranformIndexing); + a_peak.valueD = pkValue; // laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y].pt3D.z; + peaks.push_back(a_peak); + } + //ո߶ + std::sort(peaks.begin(), peaks.end(), compareByHeight); + for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++) + featureMask.at(peaks[i].y, peaks[i].x)[3] = 1; //peak flag + +#if 0 + //ʹʵzֵ任ֵ + for (int i = 0, i_max = peaks.size(); i < i_max; i++) + { + peaks[i].valueD = laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y].pt3D.z; + } +#endif + + /// ߵΪӽ + std::vector peakRgns; + int peakRgnId = 1; + for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++) + { + if (i == 3) + int kkk = 1; + + SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]); + int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff; + if (pkRgnId > 0) + continue; + + // + //ˮƽʹֱɨõ߽ + std::vector< SSG_lineConotours> topContour; + std::vector< SSG_lineConotours> bottomContour; + std::vector< SSG_lineConotours> leftContour; + std::vector< SSG_lineConotours> rightContour; + int maxEdgeId_top = 0, maxEdgeId_btm = 0, maxEdgeId_left = 0, maxEdgeId_right = 0; + sg_peakXYScan( + laser3DPoints, + lineNum, + featureMask, + peaks[i], + algoParam.growParam, + algoParam.bagParam, + false, + topContour, + bottomContour, + leftContour, + rightContour, + &maxEdgeId_top, + &maxEdgeId_btm, + &maxEdgeId_left, + &maxEdgeId_right); + + + int vldSide = 0; + if (leftContour.size() > 30) + vldSide++; + if (rightContour.size() > 30) + vldSide++; + if (topContour.size() > 30) + vldSide++; + if (bottomContour.size() > 30) + vldSide++; + + int invldSide = 0; + if (leftContour.size() < 10) + invldSide++; + if (rightContour.size() < 10) + invldSide++; + if (topContour.size() < 10) + invldSide++; + if (bottomContour.size() < 10) + invldSide++; + + if ((vldSide < 3) || (invldSide > 0)) + continue; + + //ȫƥ䣺϶ĴӣҪȫƥѰҵȷı + std::vector matchTable_TB; + std::vector< SSG_matchPair> TB_pairs; + std::vector TB_contourPairs; + int TB_matchNum = 0; + _getMatchTable( + topContour, + bottomContour, + maxEdgeId_top, + maxEdgeId_btm, + true, //isVScan, + vTreeStart, + hTreeStart, + matchTable_TB, + TB_pairs, + TB_contourPairs, + &TB_matchNum); + if (TB_matchNum < 25) + continue; + + std::vector< SSG_matchPair> LR_pairs; + std::vector matchTable_LR; + std::vector LR_contourPairs; + int LR_matchNum = 0; + _getMatchTable( + leftContour, + rightContour, + maxEdgeId_left, + maxEdgeId_right, + false, //isHScan, + vTreeStart, + hTreeStart, + matchTable_LR, + LR_pairs, + LR_contourPairs, + &LR_matchNum); + + if (LR_matchNum < 25) + continue; + + int lowLevelChkFlag = 0; + SSG_peakRgnInfo a_pkRgn = _maxLikelihoodMatch( + laser3DPoints, + lineNum, + hvTreeSize, + peaks[i], + matchTable_TB, + TB_pairs, + TB_contourPairs, + TB_matchNum, + maxEdgeId_btm, + matchTable_LR, + LR_pairs, + LR_contourPairs, + LR_matchNum, + maxEdgeId_right, + allTreesInfo, + vTreeStart, + hTreeStart, + globalROI, + algoParam, + peakRgnId, + &lowLevelChkFlag); + if (a_pkRgn.pkRgnIdx > 0) + { + peakRgns.push_back(a_pkRgn); + peakRgnId++; + } + } +#if 1 + ///֤ûдڴӵĿδ + ///ʣµĿ꣬ûмʹС + ///ɨʱⱣ֤ˮƽֱĿǷӳߴ磬Ȼһ + while (1) + { + std::vector iter_objs; + //ûдPeak㱣 + std::vector residualPeaks; + for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++) + { + SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]); + int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff; + if (pkRgnId == 0) + { + residualPeaks.push_back(peaks[i]); + } + } + if (residualPeaks.size() == 0) + break; + + bool rgnPtAsEdge = true; + for (int ri = 0; ri < residualPeaks.size(); ri++) + { + SVzNL3DPosition* pk_pt = &(laser3DPoints[residualPeaks[ri].x].p3DPosition[residualPeaks[ri].y]); + int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff; + if (pkRgnId > 0) + continue; + // + //ˮƽʹֱɨõ߽ + std::vector< SSG_lineConotours> resi_topContour; + std::vector< SSG_lineConotours> resi_bottomContour; + std::vector< SSG_lineConotours> resi_leftContour; + std::vector< SSG_lineConotours> resi_rightContour; + int resi_maxEdgeId_top = 0, resi_maxEdgeId_btm = 0, resi_maxEdgeId_left = 0, resi_maxEdgeId_right = 0; + sg_peakXYScan( + laser3DPoints, + lineNum, + featureMask, + residualPeaks[ri], + algoParam.growParam, + algoParam.bagParam, + true, + resi_topContour, + resi_bottomContour, + resi_leftContour, + resi_rightContour, + &resi_maxEdgeId_top, + &resi_maxEdgeId_btm, + &resi_maxEdgeId_left, + &resi_maxEdgeId_right); + + if ((resi_topContour.size() == 0) || (resi_bottomContour.size() == 0) || (resi_leftContour.size() == 0) || (resi_rightContour.size() == 0)) + continue; + + //ֶμƽȺƽ߶ԼֶROI + //ȫƥ䣨ˮƽзֶεпܾ + std::vector matchTable_TB; + std::vector< SSG_matchPair> TB_pairs; + std::vector TB_contourPairs; + int TB_matchNum = 0; + _getMatchTable( + resi_topContour, + resi_bottomContour, + resi_maxEdgeId_top, + resi_maxEdgeId_btm, + true, //isVScan, + vTreeStart, + hTreeStart, + matchTable_TB, + TB_pairs, + TB_contourPairs, + &TB_matchNum); + + if (TB_matchNum < 25) + continue; + + std::vector< SSG_matchPair> LR_pairs; + std::vector matchTable_LR; + std::vector LR_contourPairs; + int LR_matchNum = 0; + _getMatchTable( + resi_leftContour, + resi_rightContour, + resi_maxEdgeId_left, + resi_maxEdgeId_right, + false, //isHScan, + vTreeStart, + hTreeStart, + matchTable_LR, + LR_pairs, + LR_contourPairs, + &LR_matchNum); + + if (LR_matchNum < 25) + continue; + + int lowLevelChkFlag = 0; + SSG_peakRgnInfo a_pkRgn = _maxLikelihoodMatch( + laser3DPoints, + lineNum, + hvTreeSize, + peaks[ri], + matchTable_TB, + TB_pairs, + TB_contourPairs, + TB_matchNum, + resi_maxEdgeId_btm, + matchTable_LR, + LR_pairs, + LR_contourPairs, + LR_matchNum, + resi_maxEdgeId_right, + allTreesInfo, + vTreeStart, + hTreeStart, + globalROI, + algoParam, + peakRgnId, + &lowLevelChkFlag); +#if 0 + if (lowLevelChkFlag > 0) + { + //lowLevelFlag_T + lowLevelFlag_B<<1 + lowLevelFlag_L<<2 + lowLevelFlag_R<<3; + if (lowLevelChkFlag & 0x01) //Top + { + + } + if (lowLevelChkFlag & 0x02) //Bottom + { + } + if (lowLevelChkFlag & 0x04) //Left + { + } + if (lowLevelChkFlag & 0x08) //Rigjt + { + } + } +#endif + if (a_pkRgn.pkRgnIdx > 0) + { + iter_objs.push_back(a_pkRgn); + peakRgnId++; + } + } + if (iter_objs.size() == 0) + break; + + peakRgns.insert(peakRgns.end(), iter_objs.begin(), iter_objs.end()); + //Ϊһε׼ + peaks.clear(); + peaks.insert(peaks.end(), residualPeaks.begin(), residualPeaks.end()); + } + + ///ʣСĿײ + ///δ֪δԷֹδ֪ײ + ///¼еĴ1/4L*1/4WĿ + std::vector smallObjPeaks; //¼0.25L * 0.25WĿ꣬ײ + //ûдPeak㱣 + std::vector residualPeaks; + for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++) + { + SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]); + int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff; + if (pkRgnId == 0) + { + residualPeaks.push_back(peaks[i]); + } + } + if (residualPeaks.size() > 0) + { + bool rgnPtAsEdge = true; + for (int ri = 0; ri < residualPeaks.size(); ri++) + { + // + //ˮƽʹֱɨõ߽ + std::vector< SSG_lineConotours> resi_topContour; + std::vector< SSG_lineConotours> resi_bottomContour; + std::vector< SSG_lineConotours> resi_leftContour; + std::vector< SSG_lineConotours> resi_rightContour; + int resi_maxEdgeId_top = 0, resi_maxEdgeId_btm = 0, resi_maxEdgeId_left = 0, resi_maxEdgeId_right = 0; + sg_peakXYScan( + laser3DPoints, + lineNum, + featureMask, + residualPeaks[ri], + algoParam.growParam, + algoParam.bagParam, + true, + resi_topContour, + resi_bottomContour, + resi_leftContour, + resi_rightContour, + &resi_maxEdgeId_top, + &resi_maxEdgeId_btm, + &resi_maxEdgeId_left, + &resi_maxEdgeId_right); + //ROI + //޵ + SSG_ROIRectD objROI = { 0, -1, 0, 0 }; + for (int n = 0; n < resi_topContour.size(); n++) + { + std::vector& a_line_contourPts = resi_topContour[n].contourPts; + for (int m = 0; m < a_line_contourPts.size(); m++) + { + if (objROI.right < objROI.left) + { + objROI.left = a_line_contourPts[m].edgePt.x; + objROI.right = a_line_contourPts[m].edgePt.x; + objROI.top = a_line_contourPts[m].edgePt.y; + objROI.bottom = a_line_contourPts[m].edgePt.y; + } + else + { + objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left; + objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right; + objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top; + objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom; + } + } + } + for (int n = 0; n < resi_bottomContour.size(); n++) + { + std::vector& a_line_contourPts = resi_bottomContour[n].contourPts; + for (int m = 0; m < a_line_contourPts.size(); m++) + { + if (objROI.right < objROI.left) + { + objROI.left = a_line_contourPts[m].edgePt.x; + objROI.right = a_line_contourPts[m].edgePt.x; + objROI.top = a_line_contourPts[m].edgePt.y; + objROI.bottom = a_line_contourPts[m].edgePt.y; + } + else + { + objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left; + objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right; + objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top; + objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom; + } + } + } + for (int n = 0; n < resi_leftContour.size(); n++) + { + std::vector& a_line_contourPts = resi_leftContour[n].contourPts; + for (int m = 0; m < a_line_contourPts.size(); m++) + { + if (objROI.right < objROI.left) + { + objROI.left = a_line_contourPts[m].edgePt.x; + objROI.right = a_line_contourPts[m].edgePt.x; + objROI.top = a_line_contourPts[m].edgePt.y; + objROI.bottom = a_line_contourPts[m].edgePt.y; + } + else + { + objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left; + objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right; + objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top; + objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom; + } + } + } + for (int n = 0; n < resi_rightContour.size(); n++) + { + std::vector& a_line_contourPts = resi_rightContour[n].contourPts; + for (int m = 0; m < a_line_contourPts.size(); m++) + { + if (objROI.right < objROI.left) + { + objROI.left = a_line_contourPts[m].edgePt.x; + objROI.right = a_line_contourPts[m].edgePt.x; + objROI.top = a_line_contourPts[m].edgePt.y; + objROI.bottom = a_line_contourPts[m].edgePt.y; + } + else + { + objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left; + objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right; + objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top; + objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom; + } + } + } + //ROIС + double obj_L = objROI.right - objROI.left; + double obj_W = objROI.bottom - objROI.top; + if (obj_L < obj_W) + { + double tmp_value = obj_W; + obj_W = obj_L; + obj_L = tmp_value; + } + if ((obj_L > algoParam.bagParam.bagL * 0.25) && (obj_W > algoParam.bagParam.bagW * 0.25)) + { + smallObjPeaks.push_back(residualPeaks[ri]); + } + } + } + +#endif + + //Ŀ򣺷ֲ -> ߲ -> ߲һд + if (peakRgns.size() > 0) + { + double maxHeight = peakRgns[0].centerPos.z; + for (int i = 1; i < peakRgns.size(); i++) + { + if (maxHeight > peakRgns[i].centerPos.z) + maxHeight = peakRgns[i].centerPos.z; + } + //ȡͬ߶ȲĿ + std::vector level0_objs; + for (int i = 0, i_max = (int)peakRgns.size(); i < i_max; i++) + { + double z_diff = peakRgns[i].centerPos.z - maxHeight; + if (z_diff < algoParam.bagParam.bagH / 2) //ֲ + { + level0_objs.push_back(peakRgns[i]); + } + } + peakRgns.clear(); + peakRgns.insert(peakRgns.end(), level0_objs.begin(), level0_objs.end()); + int level0_size = (int)peakRgns.size(); + if (level0_size > 1) //һ򣬷 + { + //ȡYСĿ + double minY = 0; + double minY_idx = -1; + for (int i = 0; i < level0_size; i++) + { + if (minY_idx < 0) + { + minY = peakRgns[i].centerPos.y; + minY_idx = i; + } + else + { + if (minY > peakRgns[i].centerPos.y) + { + minY = peakRgns[i].centerPos.y; + minY_idx = i; + } + } + } + std::vector row_0_outlier; + for (int i = 0; i < level0_size; i++) + { + double y_diff = peakRgns[i].centerPos.y - minY; + if (y_diff < algoParam.bagParam.bagW / 2) //һ + objOps.push_back(peakRgns[i]); + else + row_0_outlier.push_back(i); //еż¼ + } + //ԵһеĿ갴 + if (objOps.size() > 1) + { + std::sort(objOps.begin(), objOps.end(), compareByXValue); + } + for (int i = 0; i < row_0_outlier.size(); i++) + objOps.push_back(peakRgns[row_0_outlier[i]]); +#if 0 + for (int i = level0_end + 1; i < peakRgns.size(); i++) + objOps.push_back(peakRgns[i]); +#endif + } + else + objOps.insert(objOps.end(), peakRgns.begin(), peakRgns.end()); + } + //ײ + if ((objOps.size() > 0) && (smallObjPeaks.size() > 0)) + { + SSG_peakRgnInfo* highest_obj = &objOps[0]; + double objZ = highest_obj->centerPos.z; + for (int i = 0; i < smallObjPeaks.size(); i++) + { + SSG_2DValueI* a_samllPk = &smallObjPeaks[i]; + if (highest_obj->centerPos.z > a_samllPk->valueD + algoParam.bagParam.bagH / 2) + { + SVzNL3DPosition* smallPkPt = &laser3DPoints[a_samllPk->x].p3DPosition[a_samllPk->y]; + double dist = sqrt(pow(highest_obj->centerPos.x - smallPkPt->pt3D.x, 2) + pow(highest_obj->centerPos.y - smallPkPt->pt3D.y, 2)); + double dia_angle = sqrt(pow(highest_obj->objSize.dWidth, 2) + pow(highest_obj->objSize.dHeight, 2)); + //ͬȽ + double z_diff = smallPkPt->pt3D.z - objZ; + if (z_diff < algoParam.bagParam.bagH / 2) //ֲ + { + if (dist < dia_angle / 2) + objOps.clear(); //μЧ + } + } + } + } + //ͶԭϵԱ۱궨ȷ + for (int i = 0; i < lineNum; i++) + sg_lineDataR(&laser3DPoints[i], poseCalibPara.invRMatrix, -1); + //Ͷԭϵ + double invMatrix[3][3]; + invMatrix[0][0] = poseCalibPara.invRMatrix[0]; + invMatrix[0][1] = poseCalibPara.invRMatrix[1]; + invMatrix[0][2] = poseCalibPara.invRMatrix[2]; + invMatrix[1][0] = poseCalibPara.invRMatrix[3]; + invMatrix[1][1] = poseCalibPara.invRMatrix[4]; + invMatrix[1][2] = poseCalibPara.invRMatrix[5]; + invMatrix[2][0] = poseCalibPara.invRMatrix[6]; + invMatrix[2][1] = poseCalibPara.invRMatrix[7]; + invMatrix[2][2] = poseCalibPara.invRMatrix[8]; + for (int i = 0, i_max = (int)objOps.size(); i < i_max; i++) + { + SSG_EulerAngles euAngle = { objOps[i].centerPos.x_roll, objOps[i].centerPos.y_pitch, objOps[i].centerPos.z_yaw }; + double pose[3][3]; + eulerToRotationMatrixZYX(euAngle, pose); + double resultMatrix[3][3]; + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + resultMatrix[i][j] = 0; + for (int m = 0; m < 3; m++) + resultMatrix[i][j] += invMatrix[i][m] * pose[m][j]; + } + } + SSG_EulerAngles resultEuAngle = rotationMatrixToEulerZYX(resultMatrix); + objOps[i].centerPos.z_yaw = resultEuAngle.yaw; + double x = objOps[i].centerPos.x * poseCalibPara.invRMatrix[0] + + objOps[i].centerPos.y * poseCalibPara.invRMatrix[1] + + objOps[i].centerPos.z * poseCalibPara.invRMatrix[2]; + double y = objOps[i].centerPos.x * poseCalibPara.invRMatrix[3] + + objOps[i].centerPos.y * poseCalibPara.invRMatrix[4] + + objOps[i].centerPos.z * poseCalibPara.invRMatrix[5]; + double z = objOps[i].centerPos.x * poseCalibPara.invRMatrix[6] + + objOps[i].centerPos.y * poseCalibPara.invRMatrix[7] + + objOps[i].centerPos.z * poseCalibPara.invRMatrix[8]; + objOps[i].centerPos.x = x; + objOps[i].centerPos.y = y; + objOps[i].centerPos.z = z; + } +#endif + //ˮƽʹֱȡ + + //о任 + //ˮָ + //z߶ȼĿ +} \ No newline at end of file diff --git a/sourceCode/WD_particleSizeMeasure_Export.h b/sourceCode/WD_particleSizeMeasure_Export.h new file mode 100644 index 0000000..133ddda --- /dev/null +++ b/sourceCode/WD_particleSizeMeasure_Export.h @@ -0,0 +1,54 @@ +#pragma once + +#if defined(SG_API_LIBRARY) +# define SG_APISHARED_EXPORT __declspec(dllexport) +#else +# define SG_APISHARED_EXPORT __declspec(dllimport) +#endif + +#include "SG_baseDataType.h" +#include +#include + +#define OUTPUT_DEBUG 1 + +typedef struct +{ + SWD_sizeParam minSize; + SWD_sizeParam alarmSize; +}SWD_paricleSizeParam; + +typedef struct +{ + SSG_outlierFilterParam filterParam; + SSG_cornerParam cornerParam; + SSG_treeGrowParam growParam; +}SWD_PSM_algoParam; + +typedef struct +{ + double EQRadius; + SVzNL3DPoint center_pos; +}SWD_ParticlePosInfo; + +//һƽƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SG_APISHARED_EXPORT SSG_planeCalibPara wd_getBaseCalibPara( + std::vector< std::vector>& scanLines); + +//̬ƽȥ +SG_APISHARED_EXPORT void wd_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); + +// +SG_APISHARED_EXPORT void wd_particleSizeMeasure( + std::vector< std::vector>& scanLines, + const SWD_paricleSizeParam particleSizeParam, + const SSG_planeCalibPara groundCalibPara, + const SWD_PSM_algoParam algoParam, + std::vector& particles, + int* errCode); + diff --git a/sourceCode/WD_watershed.cpp b/sourceCode/WD_watershed.cpp new file mode 100644 index 0000000..a718f89 --- /dev/null +++ b/sourceCode/WD_watershed.cpp @@ -0,0 +1,366 @@ + +#if 0 +#include +#include +#include +#include +#include +#include + +using namespace std; + +// ͼߴ +const int WIDTH = 256; +const int HEIGHT = 256; + +// 8 +const int dx[] = { -1, -1, -1, 0, 0, 1, 1, 1 }; +const int dy[] = { -1, 0, 1, -1, 1, -1, 0, 1 }; + +// ؽṹ +struct Pixel { + int x, y; // + int value; // ֵ(Ҷ) + Pixel(int x_, int y_, int v_) : x(x_), y(y_), value(v_) {} + // ȶҪıȽϺ(С) + bool operator<(const Pixel& other) const { + return value > other.value; // ע⣺ȶĬǴ󶥶ѣﷴתȽ + } +}; + +// ȡPGMʽͼ +bool readPGM(const string& filename, int image[HEIGHT][WIDTH]) { + ifstream file(filename, ios::binary); + if (!file) return false; + + string magic; + int w, h, max_val; + file >> magic >> w >> h >> max_val; + if (magic != "P5" || w != WIDTH || h != HEIGHT) return false; + + // з + file.ignore(1); + + // ȡֵ + unsigned char pixel; + for (int i = 0; i < HEIGHT; ++i) { + for (int j = 0; j < WIDTH; ++j) { + file.read((char*)&pixel, 1); + image[i][j] = (int)pixel; + } + } + return true; +} + +// ָΪPGMͼ +void writePGM(const string& filename, int labels[HEIGHT][WIDTH], int num_labels) { + ofstream file(filename, ios::binary); + file << "P5\n" << WIDTH << " " << HEIGHT << "\n255\n"; + + // ǩӳ䵽0-255Χ + int step = 255 / (num_labels + 1); + for (int i = 0; i < HEIGHT; ++i) { + for (int j = 0; j < WIDTH; ++j) { + unsigned char val = (labels[i][j] + 1) * step; + if (labels[i][j] == -1) val = 0; // ˮ߽ + file.write((char*)&val, 1); + } + } +} + +// ˮ㷨ʵ +int watershed(int image[HEIGHT][WIDTH], int labels[HEIGHT][WIDTH]) { + // ʼǩ-1ʾδǣ0ʾ߽ + memset(labels, -1, sizeof(int) * HEIGHT * WIDTH); + + // ȶ(ֵС) + priority_queue pq; + + // оֲСֵΪʼ + int current_label = 0; + bool is_min; + + // һҵоֲСֵ + for (int i = 0; i < HEIGHT; ++i) { + for (int j = 0; j < WIDTH; ++j) { + is_min = true; + for (int d = 0; d < 8; ++d) { + int ni = i + dx[d]; + int nj = j + dy[d]; + if (ni >= 0 && ni < HEIGHT && nj >= 0 && nj < WIDTH) { + if (image[ni][nj] < image[i][j]) { + is_min = false; + break; + } + } + } + if (is_min) { + labels[i][j] = current_label++; + pq.push(Pixel(i, j, image[i][j])); + } + } + } + + // ڶģû + while (!pq.empty()) { + Pixel p = pq.top(); + pq.pop(); + int x = p.x; + int y = p.y; + + for (int d = 0; d < 8; ++d) { + int nx = x + dx[d]; + int ny = y + dy[d]; + + if (nx >= 0 && nx < HEIGHT && ny >= 0 && ny < WIDTH) { + // δ + if (labels[nx][ny] == -1) { + // Ϊǰ + labels[nx][ny] = labels[x][y]; + pq.push(Pixel(nx, ny, image[nx][ny])); + } + // ѱڲͬΪ߽ + else if (labels[nx][ny] != labels[x][y]) { + labels[x][y] = -1; // ǰΪ߽ + } + } + } + } + + return current_label; +} + +#if 0 +㷨˵ +ʵѭˮ㷨Ļԭ + +αʾͼĻҶֵΪθ߶ȣҶֵԽͱʾԽ +ʼأʶͼеоֲСֵЩdzʼ "ˮ" +ụ̂ʹȶУСѣģӵ͵ߵụֵ̂С +ÿرڵ +߽ȷͬصˮʱγɷˮ߽ + +ʹ÷ + +׼һ 256x256 PGM ʽҶͼΪinput.pgm +룺g++ watershed.cpp - o watershed +г. / watershed +Ϊoutput.pgmвͬòͬҶȱʾ߽Ϊɫ + +ע + +ʵǻ汾ûмԤ裨ȥ룩ܶԸͼָЧ +ʵӦͨҪȶͼƽٹȷָ +ͨʽ4 8 ıָЧ +ڲɫͼҪתΪҶͼٴ + +Ҫߴͼ񣬿޸WIDTHHEIGHTӦڴ +int main() { + int image[HEIGHT][WIDTH]; + int labels[HEIGHT][WIDTH]; + + // ȡͼ + if (!readPGM("input.pgm", image)) { + cerr << "޷ȡͼļ" << endl; + return -1; + } + + // ִзˮ㷨 + int num_labels = watershed(image, labels); + cout << "ָɣõ " << num_labels << " " << endl; + + // + writePGM("output.pgm", labels, num_labels); + cout << "ѱΪ output.pgm" << endl; + + return 0; +} +#endif + +#else +#include +#include +#include +#include +#include +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +using namespace std; + +#if 0 +// ȡPPMʽͼ񣨼򻯰棩 +bool readPPM(const string& filename, Image& img) { + ifstream file(filename, ios::binary); + if (!file) return false; + + string magic; + int maxVal; + file >> magic >> img.width >> img.height >> maxVal; + if (magic != "P6") return false; // ֶ֧PPM + + file.ignore(); // Իз + vector data(img.width * img.height * 3); + file.read((char*)data.data(), data.size()); + + // תΪҶͼY = 0.299*R + 0.587*G + 0.114*B + img.gray.resize(img.height, vector(img.width)); + img.markers.resize(img.height, vector(img.width, 0)); // ʼͼΪ0 + + for (int i = 0; i < img.height; ++i) { + for (int j = 0; j < img.width; ++j) { + int idx = (i * img.width + j) * 3; + int r = data[idx]; + int g = data[idx + 1]; + int b = data[idx + 2]; + img.gray[i][j] = (int)(0.299 * r + 0.587 * g + 0.114 * b); + } + } + return true; +} + +// ָΪPPMʽ +void saveResult(const string& filename, const Image& img) { + ofstream file(filename, ios::binary); + file << "P6\n" << img.width << " " << img.height << "\n255\n"; + + for (int i = 0; i < img.height; ++i) { + for (int j = 0; j < img.width; ++j) { + if (img.markers[i][j] == -1) { // ˮ߽磨ɫ + file.put(255); file.put(0); file.put(0); + } + else { // 򣨸ݱֵɲͬɫ + int color = (img.markers[i][j] * 50) % 256; + file.put(color); + file.put((color + 85) % 256); + file.put((color + 170) % 256); + } + } + } +} +#endif + + + +// 8ƫ +const int dx[] = { -1, -1, -1, 0, 0, 1, 1, 1 }; +const int dy[] = { -1, 0, 1, -1, 1, -1, 0, 1 }; + +// ֲСֵΪʼӵ +void findMinima(SWD_waterShedImage& img, int& markerCount) { + markerCount = 1; //ӦID + // ÿأжǷΪֲСֵ8С + for (int i = 1; i < img.height - 1; ++i) { + for (int j = 1; j < img.width - 1; ++j) { + if (1 == img.markers[i][j]) // + continue; + bool isMin = true; +#if 0 + for (int d = 0; d < 8; ++d) { + int y = i + dy[d]; + int x = j + dx[d]; + if (img.gray[y][x] < img.gray[i][j]) { + isMin = false; + break; + } + } +#else + for (int dy = -7; dy < 7; dy++) + { + for (int dx = -7; dx < 7; dx++) + { + int y = i + dy; + int x = j + dx; + if ((x >= 0) && (x < img.width) && (y >= 0) && (y < img.height)) + { + if (img.gray[y][x] < img.gray[i][j]) { + isMin = false; + break; + } + } + } + } +#endif + if (isMin) { + // ظͬһļСֵ + bool hasMarker = false; + for (int d = 0; d < 8; ++d) { + int y = i + dy[d]; + int x = j + dx[d]; + if (img.markers[y][x] > 1) { + hasMarker = true; + img.markers[y][x] = img.markers[i][j]; + break; + } + } + if (!hasMarker) { + img.markers[i][j] = ++markerCount; // ӵ + } + } + } + } +} + +// ˮ㷨 +void watershed(SWD_waterShedImage& img) +{ + int markerCount; + findMinima(img, markerCount); // Զӵ + + // Ҷֵأģˮλ + vector>> pixels; // (Ҷֵ, (x,y)) + for (int i = 0; i < img.height; ++i) { + for (int j = 0; j < img.width; ++j) { + pixels.emplace_back(img.gray[i][j], make_pair(i, j)); + } + } + sort(pixels.begin(), pixels.end()); + + // أִעˮ + for (const auto& p : pixels) { + int x = p.second.first; + int y = p.second.second; + if (img.markers[x][y] > 0) continue; // ѱǵӵ + + // ռѱǵ + vector neighborMarkers; + for (int d = 0; d < 8; ++d) { + int nx = x + dx[d]; + int ny = y + dy[d]; + if (nx >= 0 && nx < img.height && ny >= 0 && ny < img.width) { + int m = img.markers[nx][ny]; + if (m > 0) { + neighborMarkers.push_back(m); + } + } + } + + if (neighborMarkers.empty()) { + continue; // ǣݲ + } + else if (neighborMarkers.size() == 1) { + img.markers[x][y] = neighborMarkers[0]; // ͬһ + } + else { + // 򽻻㣬Ϊˮ + img.markers[x][y] = -1; + } + } +} + +#if 0 +int main() { + Image img; + if (!readPPM("input.ppm", img)) { // PPMͼ + cerr << "޷ȡͼ" << endl; + return -1; + } + + watershed(img); // ִзˮָ + saveResult("watershed_result.ppm", img); // + + cout << "ָɣѱΪ watershed_result.ppm" << endl; + return 0; +} +#endif +#endif \ No newline at end of file From f6f09064f6cfbb2870262ed97f2ad6c6a92993fd Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Sun, 9 Nov 2025 22:54:23 +0800 Subject: [PATCH 08/12] =?UTF-8?q?=E7=B2=92=E5=BE=84=E7=AE=97=E6=B3=95=20ve?= =?UTF-8?q?r=5F1.0.0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../BQ_workpieceCornerExtract_test.cpp | 27 +- .../particleSizeMeasurement.vcxproj | 172 ++++ .../particleSizeMeasurement_test.cpp | 632 +++++++++++++++ .../particleSizeMeasurement_test.vcxproj | 159 ++++ sourceCode/SG_baseAlgo_Export.h | 9 +- sourceCode/SG_regionGrow.cpp | 2 +- sourceCode/WD_particleSizeMeasure.cpp | 741 +++--------------- sourceCode/WD_particleSizeMeasure_Export.h | 10 +- sourceCode/WD_watershed.cpp | 166 +++- 9 files changed, 1284 insertions(+), 634 deletions(-) create mode 100644 particleSizeMeasurement/particleSizeMeasurement.vcxproj create mode 100644 particleSizeMeasurement_test/particleSizeMeasurement_test.cpp create mode 100644 particleSizeMeasurement_test/particleSizeMeasurement_test.vcxproj diff --git a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp index fc494f1..3b7f34c 100644 --- a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp +++ b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp @@ -81,6 +81,27 @@ void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector> scanLines) +{ + std::ofstream sw(fileName); + int lineNum = scanLines.size(); + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = scanLines[line].size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D; + if (pt3D->z < 1e-4) + continue; + double x = (double)pt3D->x; + double y = (double)pt3D->y; + double z = (double)pt3D->z; + sw << x << "," << y << "," << z << std::endl; + } + } + sw.close(); +} + void _convertToGridData_XYZ_vector(std::vector>& scanData, double _F, std::vector>& scanData_grid) { int min_y = 100000000; @@ -619,7 +640,7 @@ int main() }; SVzNLRange fileIdx[TEST_GROUP] = { - {1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,12} + {1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,11} }; #if CONVERT_TO_GRID @@ -755,6 +776,10 @@ int main() std::vector> scanLines; vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + //转成plyTxt格式 + //sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx); + //wdSavePlyTxt(_scan_file, scanLines); + long t1 = (long)GetTickCount64();//统计时间 for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++) diff --git a/particleSizeMeasurement/particleSizeMeasurement.vcxproj b/particleSizeMeasurement/particleSizeMeasurement.vcxproj new file mode 100644 index 0000000..82137c0 --- /dev/null +++ b/particleSizeMeasurement/particleSizeMeasurement.vcxproj @@ -0,0 +1,172 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + 16.0 + Win32Proj + {8fd0f574-61c6-4094-8521-33aeddb44797} + particleSizeMeasurement + 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;PARTICLESIZEMEASUREMENT_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;PARTICLESIZEMEASUREMENT_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + true + true + false + + + + + Level3 + true + _DEBUG;PARTICLESIZEMEASUREMENT_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + /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;PARTICLESIZEMEASUREMENT_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + /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/particleSizeMeasurement_test/particleSizeMeasurement_test.cpp b/particleSizeMeasurement_test/particleSizeMeasurement_test.cpp new file mode 100644 index 0000000..47275c3 --- /dev/null +++ b/particleSizeMeasurement_test/particleSizeMeasurement_test.cpp @@ -0,0 +1,632 @@ +// BQ_workpieceCornerExtract_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// + +#include +#include +#include +#include +#include +#include "direct.h" +#include +#include "WD_particleSizeMeasure_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 _outputParticleSizeInfo(char* fileName, std::vector& particles) +{ + int particleSize = (int)particles.size(); + if (particleSize == 0) + return; + + std::ofstream sw(fileName); + char dataStr[250]; + sprintf_s(dataStr, 250, "particleNum: %d", particleSize); + sw << dataStr << std::endl; + + for (int i = 0; i < particleSize; i++) + { + sprintf_s(dataStr, 250, " id_%d: (%g, %g, %g)", i, particles[i].size.length, particles[i].size.width, particles[i].size.height); + 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 _outputScanDataResult_RGBD( + char* fileName, + std::vector>& scanLines, + std::vector& particles) +{ + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + if (particles.size() > 0) + 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 }; + int size = 1; + int lineIdx = 0; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = (int)scanLines[line].size(); + if (linePtNum == 0) + continue; + + int vldNum = 0; + for (int i = 0; i < linePtNum; i++) + { + if (scanLines[line][i].pt3D.z > 1e-4) + vldNum++; + } + + sw << "Line_" << lineIdx << "_0_" << vldNum << std::endl; + lineIdx++; + for (int i = 0; i < linePtNum; i++) + { + if (scanLines[line][i].pt3D.z <= 1e-4) + continue; + + SVzNL3DPosition* pt3D = &scanLines[line][i]; + int mkID = pt3D->nPointIdx; + if (mkID < 2) + { + rgb = { 200, 200, 200 }; + size = 1; + } + else + { + rgb.r = (mkID * 97) % 256; + rgb.g = (mkID * 73) % 256; + rgb.b = (mkID * 59) % 256; + 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; + } + } + + rgb = { 255,0,0 }; + if (particles.size() > 0) + { + int objNum = (int)particles.size(); + int lineIdx = 0; + for (int i = 0; i < objNum; i++) + { + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[0].x << "," << particles[i].vertix[0].y << "," << particles[i].vertix[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[1].x << "," << particles[i].vertix[1].y << "," << particles[i].vertix[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[1].x << "," << particles[i].vertix[1].y << "," << particles[i].vertix[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[2].x << "," << particles[i].vertix[2].y << "," << particles[i].vertix[2].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[2].x << "," << particles[i].vertix[2].y << "," << particles[i].vertix[2].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[3].x << "," << particles[i].vertix[3].y << "," << particles[i].vertix[3].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[3].x << "," << particles[i].vertix[3].y << "," << particles[i].vertix[3].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[0].x << "," << particles[i].vertix[0].y << "," << particles[i].vertix[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[0].x << "," << particles[i].vertix[0].y << "," << particles[i].vertix[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[4].x << "," << particles[i].vertix[4].y << "," << particles[i].vertix[4].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[1].x << "," << particles[i].vertix[1].y << "," << particles[i].vertix[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[5].x << "," << particles[i].vertix[5].y << "," << particles[i].vertix[5].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[2].x << "," << particles[i].vertix[2].y << "," << particles[i].vertix[2].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[6].x << "," << particles[i].vertix[6].y << "," << particles[i].vertix[6].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[3].x << "," << particles[i].vertix[3].y << "," << particles[i].vertix[3].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[7].x << "," << particles[i].vertix[7].y << "," << particles[i].vertix[7].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[4].x << "," << particles[i].vertix[4].y << "," << particles[i].vertix[4].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[5].x << "," << particles[i].vertix[5].y << "," << particles[i].vertix[5].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[5].x << "," << particles[i].vertix[5].y << "," << particles[i].vertix[5].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[6].x << "," << particles[i].vertix[6].y << "," << particles[i].vertix[6].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[6].x << "," << particles[i].vertix[6].y << "," << particles[i].vertix[6].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[7].x << "," << particles[i].vertix[7].y << "," << particles[i].vertix[7].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[7].x << "," << particles[i].vertix[7].y << "," << particles[i].vertix[7].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[4].x << "," << particles[i].vertix[4].y << "," << particles[i].vertix[4].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[0].vertix[0].x << "," << particles[0].vertix[0].y << "," << particles[0].vertix[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[0].vertix[1].x << "," << particles[0].vertix[1].y << "," << particles[0].vertix[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } + + sw.close(); +} + +#define CONVERT_TO_GRID 0 +#define TEST_COMPUTE_CALIB_PARA 0 +#define TEST_COMPUTE_PARTICE_SIZE 1 +#define TEST_GROUP 1 +int main() +{ + const char* dataPath[TEST_GROUP] = { + "F:\\ShangGu\\粒径数据\\曝光\\3D数据\\" //0 + }; + + SVzNLRange fileIdx[TEST_GROUP] = { + {1,10} + }; + +#if TEST_COMPUTE_PARTICE_SIZE + 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 = 2600.0; + for (int i = 0; i < 9; i++) + poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; + char calibFile[250]; +#if 0 + if (grp == 0) + { + sprintf_s(calibFile, "F:\\ShangGu\\粒径数据\\曝光\\3D数据\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } +#endif + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =1; + char _scan_file[256]; + sprintf_s(_scan_file, "%sgridScanData_%d.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; + //行处理 + //调平，去除地面 + wd_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1); + } + + SWD_paricleSizeParam paricleSizeParam; + paricleSizeParam.minSize = { 100.0, 100.0, 100.0 }; + paricleSizeParam.alarmSize = { 1000.0, 1000.0, 1000.0 }; + SWD_PSM_algoParam algoParam; + algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 + algoParam.filterParam.outlierTh = 5; + algoParam.cornerParam.cornerTh = 60; //45度角 + algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = 20; + algoParam.cornerParam.jumpCornerTh_1 = 10; //水平角度，小于此角度视为水平 + algoParam.cornerParam.jumpCornerTh_2 = 60; + algoParam.growParam.maxLineSkipNum = 10; + algoParam.growParam.yDeviation_max = 10.0; + algoParam.growParam.maxSkipDistance = 10.0; + algoParam.growParam.zDeviation_max = 10.0;// algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 100; //mm + algoParam.growParam.minVTypeTreeLen = 100; //mm + + int errCode = 0; + std::vector particles; + wd_particleSizeMeasure( + scanLines, + paricleSizeParam, + poseCalibPara, + algoParam, + particles, + &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); + _outputScanDataResult_RGBD(_scan_file, scanLines, particles); + sprintf_s(calibFile, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx); + _outputParticleSizeInfo(calibFile, particles); + } + } +#endif +} + +// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 +// 调试程序: F5 或调试 >“开始调试”菜单 + +// 入门使用技巧: +// 1. 使用解决方案资源管理器窗口添加/管理文件 +// 2. 使用团队资源管理器窗口连接到源代码管理 +// 3. 使用输出窗口查看生成输出和其他消息 +// 4. 使用错误列表窗口查看错误 +// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目 +// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件 + diff --git a/particleSizeMeasurement_test/particleSizeMeasurement_test.vcxproj b/particleSizeMeasurement_test/particleSizeMeasurement_test.vcxproj new file mode 100644 index 0000000..080e33b --- /dev/null +++ b/particleSizeMeasurement_test/particleSizeMeasurement_test.vcxproj @@ -0,0 +1,159 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {0f4995a6-3978-4ef6-87ad-cc15ec9b007b} + particleSizeMeasurementtest + 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 + opencv_world320d.lib;particleSizeMeasurement.lib;%(AdditionalDependencies) + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + + + + + 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;particleSizeMeasurement.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 61efce0..a1cac70 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -413,4 +413,11 @@ SG_APISHARED_EXPORT void pointClout2DProjection( ); //ˮ㷨 -SG_APISHARED_EXPORT void watershed(SWD_waterShedImage& img); \ No newline at end of file +SG_APISHARED_EXPORT void watershed(SWD_waterShedImage& img); +// ӵзˮ㷨 +SG_APISHARED_EXPORT void wd_seedWatershed( + SWD_waterShedImage& img, + std::vector& watershedSeeds, //ӵ + int maxLevel, //ˮλ + int startMakerID //ʼMarkerID +); \ No newline at end of file diff --git a/sourceCode/SG_regionGrow.cpp b/sourceCode/SG_regionGrow.cpp index f6692ce..f184102 100644 --- a/sourceCode/SG_regionGrow.cpp +++ b/sourceCode/SG_regionGrow.cpp @@ -1339,7 +1339,7 @@ void sg_getLocalPeaks_distTransform(cv::Mat& input, std::vector& p for (int j = 0; j < winNum_cols; j++) { SVzNL2DPoint LTpos = { j * searchWin.searchW_pts / 2 , i * searchWin.seachW_lines / 2 }; - SSG_2DValueI pkPos = { -1, -1, 0, 0 }; + SSG_2DValueI pkPos = { -1, -1, 0, 0, 0}; _findDistTransformPeak(input, LTpos, searchWin, &pkPos); if ((pkPos.x >= 0) && (pkPos.y >= 0)) { diff --git a/sourceCode/WD_particleSizeMeasure.cpp b/sourceCode/WD_particleSizeMeasure.cpp index 747c5ba..53bd815 100644 --- a/sourceCode/WD_particleSizeMeasure.cpp +++ b/sourceCode/WD_particleSizeMeasure.cpp @@ -5,6 +5,12 @@ #include #include +std::string m_strVersion = "1.0.0"; +const char* wd_particleSegVersion(void) +{ + return m_strVersion.c_str(); +} + //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ @@ -401,7 +407,9 @@ void wd_particleSizeMeasure( cv::imwrite("distTransformMask.png", maskImage); cv::Mat dtImage; cv::normalize(distTransform, dtImage, 0, 255, cv::NORM_MINMAX, CV_8U); - cv::imwrite("distTransform.png", dtImage); + cv::Mat dtImage_color; + cv::cvtColor(dtImage, dtImage_color, cv::COLOR_GRAY2BGR); + cv::imwrite("distTransform.png", dtImage_color); #endif //ѰPeakPeakȷPeakΪӵзˮ뷽ָ @@ -423,17 +431,45 @@ void wd_particleSizeMeasure( int filterSize = (int)filter_dt_peaks.size(); for (int i = 0; i < filterSize; i++) { + SSG_2DValueI& obj_0 = filter_dt_peaks[i]; for (int j = i + 1; j < filterSize; j++) { - + SSG_2DValueI& obj_1 = filter_dt_peaks[j]; + double dist = sqrt(pow(obj_0.x - obj_1.x, 2) + pow(obj_0.y - obj_1.y, 2)); + double distTh = dist * 1.2; + if ((obj_0.valueD + obj_1.valueD) > distTh) //ϲ + { + if (obj_0.valueD < obj_1.valueD) + obj_0.value = -1; + else + obj_1.value = -1; + } } } - + + //Ч + std::vector vld_dt_peaks; + for (int i = 0; i < filterSize; i++) + { + if (filter_dt_peaks[i].value < 0) + continue; + vld_dt_peaks.push_back(filter_dt_peaks[i]); + } +#if OUTPUT_DEBUG //debug + int dbg_seedNum = (int)vld_dt_peaks.size(); + for (int i = 0; i < dbg_seedNum; i++) + { + int dbg_px = vld_dt_peaks[i].x; + int dbg_py = vld_dt_peaks[i].y; + //dtImage_color.at(dbg_py, dbg_px) = cv::Vec3b(0, 0, 255); + cv::circle(dtImage_color, cv::Point(dbg_px, dbg_py), 3, cv::Scalar(0, 0, 255), -1); + } + cv::imwrite("distTransform_seed.png", dtImage_color); +#endif + //ˮָ - //ɨ߽磬Ŀ߽缯 //ͨĿڱ߽жǷϲĿ - //ȡСֵ double minVal, maxVal; @@ -446,13 +482,18 @@ void wd_particleSizeMeasure( wsImg.width = distTransform.cols; wsImg.height = distTransform.rows; wsImg.gray.resize(wsImg.height, std::vector(wsImg.width)); - wsImg.markers.resize(wsImg.height, std::vector(wsImg.width, 1)); // ʼͼΪ0 + wsImg.markers.resize(wsImg.height, std::vector(wsImg.width, 1)); // ʼͼΪ1, int maxValue = (int)maxVal + 2; + int maxLevel = (int)(maxVal - minVal); for (int i = 0; i < distTransform.rows; i++) { + if (i == 758) + int kkk = 1; float* rowPtr = distTransform.ptr(i); for (int j = 0; j < distTransform.cols; j++) { + if (j == 171) + int kkk = 1; float disValue = rowPtr[j]; if (disValue < 1e-4) //߽ͱ wsImg.gray[i][j] = maxValue; @@ -463,9 +504,8 @@ void wd_particleSizeMeasure( } } } - - watershed(wsImg); - + int startMarkerID = 2; + wd_seedWatershed(wsImg, vld_dt_peaks, maxLevel, startMarkerID); #if OUTPUT_DEBUG //debug cv::Mat waterShedResult(wsImg.height, wsImg.width, CV_8UC3); for (int i = 0; i < wsImg.height; ++i) { @@ -473,633 +513,104 @@ void wd_particleSizeMeasure( if (wsImg.markers[i][j] == -1) { // ˮ߽磨ɫ waterShedResult.at(i, j) = cv::Vec3b(0,0,255); } - else { // 򣨸ݱֵɲͬɫ - int color_r = (wsImg.markers[i][j] * 50) % 256; - int color_g = (color_r + 85) % 256; - int color_b = (color_r + 170) % 256; + else if (wsImg.markers[i][j] < 2) + { + waterShedResult.at(i, j) = cv::Vec3b(200, 200, 200); + } + else + { // 򣨸ݱֵɲͬɫ + + int color_r = (wsImg.markers[i][j] * 97) % 256; + int color_g = (wsImg.markers[i][j] * 73) % 256; + int color_b = (wsImg.markers[i][j] * 59) % 256; waterShedResult.at(i, j) = cv::Vec3b(color_b, color_g, color_r); } } } cv::imwrite("watershed.png", waterShedResult); #endif -#if 0 - - - - - - SSG_localPkParam searchWin; - searchWin.seachW_lines = (int)(algoParam.bagParam.bagW * 0.4); - searchWin.searchW_pts = (int)(algoParam.bagParam.bagW * 0.4); - std::vector dt_peaks; - sg_getLocalPeaks_distTransform(distTransform, dt_peaks, searchWin); - //ȡPeaks - int invlidDistToEdge = 50; //ԵPeakǷǷ㡣 - double minPeakValue = algoParam.bagParam.bagW / 8; - std::vector peaks; - for (int i = 0; i < dt_peaks.size(); i++) + //ɷָĿ + //ÿ3DͶӰϵλãĿ3D + std::vector> segObjs; + int maxMkID = startMarkerID + (int)vld_dt_peaks.size(); + segObjs.resize(maxMkID); + for (int line = 0; line < lineNum; line++) + { + for (int i = 0; i < nPointCnt; i++) { - //߽紦PeakΪϸPeakȥ - int x_diff_0 = dt_peaks[i].x; //߾룬λΪmm߶Ϊ1mm - int x_diff_1 = distTransform.cols - dt_peaks[i].x;//ұ߾ - int y_diff_0 = dt_peaks[i].y;//ϱ߾ - int y_diff_1 = distTransform.rows - dt_peaks[i].y;//±߾ - if ((x_diff_0 < invlidDistToEdge) || (x_diff_1 < invlidDistToEdge) || - (y_diff_0 < invlidDistToEdge) || (y_diff_1 < invlidDistToEdge) || - (dt_peaks[i].valueD < minPeakValue)) + SVzNL3DPosition* pt3D = &scanLines[line][i]; + pt3D->nPointIdx = 0; + if (pt3D->pt3D.z < 1e-4) continue; - //distTranformIndexingл - double pkValue = dt_peaks[i].valueD; - SSG_2DValueI a_peak = _backIndexingPeakPos(dt_peaks[i], distTranformIndexing); - a_peak.valueD = pkValue; // laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y].pt3D.z; - peaks.push_back(a_peak); - } - //ո߶ - std::sort(peaks.begin(), peaks.end(), compareByHeight); - for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++) - featureMask.at(peaks[i].y, peaks[i].x)[3] = 1; //peak flag + double x = pt3D->pt3D.x; + double y = pt3D->pt3D.y; + int px = (int)(x - x_range.min) / scale + edgeSkip; + int py = (int)(y - y_range.min) / scale + edgeSkip; -#if 0 - //ʹʵzֵ任ֵ - for (int i = 0, i_max = peaks.size(); i < i_max; i++) - { - peaks[i].valueD = laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y].pt3D.z; - } -#endif - - /// ߵΪӽ - std::vector peakRgns; - int peakRgnId = 1; - for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++) - { - if (i == 3) - int kkk = 1; - - SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]); - int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff; - if (pkRgnId > 0) - continue; - - // - //ˮƽʹֱɨõ߽ - std::vector< SSG_lineConotours> topContour; - std::vector< SSG_lineConotours> bottomContour; - std::vector< SSG_lineConotours> leftContour; - std::vector< SSG_lineConotours> rightContour; - int maxEdgeId_top = 0, maxEdgeId_btm = 0, maxEdgeId_left = 0, maxEdgeId_right = 0; - sg_peakXYScan( - laser3DPoints, - lineNum, - featureMask, - peaks[i], - algoParam.growParam, - algoParam.bagParam, - false, - topContour, - bottomContour, - leftContour, - rightContour, - &maxEdgeId_top, - &maxEdgeId_btm, - &maxEdgeId_left, - &maxEdgeId_right); - - - int vldSide = 0; - if (leftContour.size() > 30) - vldSide++; - if (rightContour.size() > 30) - vldSide++; - if (topContour.size() > 30) - vldSide++; - if (bottomContour.size() > 30) - vldSide++; - - int invldSide = 0; - if (leftContour.size() < 10) - invldSide++; - if (rightContour.size() < 10) - invldSide++; - if (topContour.size() < 10) - invldSide++; - if (bottomContour.size() < 10) - invldSide++; - - if ((vldSide < 3) || (invldSide > 0)) - continue; - - //ȫƥ䣺϶ĴӣҪȫƥѰҵȷı - std::vector matchTable_TB; - std::vector< SSG_matchPair> TB_pairs; - std::vector TB_contourPairs; - int TB_matchNum = 0; - _getMatchTable( - topContour, - bottomContour, - maxEdgeId_top, - maxEdgeId_btm, - true, //isVScan, - vTreeStart, - hTreeStart, - matchTable_TB, - TB_pairs, - TB_contourPairs, - &TB_matchNum); - if (TB_matchNum < 25) - continue; - - std::vector< SSG_matchPair> LR_pairs; - std::vector matchTable_LR; - std::vector LR_contourPairs; - int LR_matchNum = 0; - _getMatchTable( - leftContour, - rightContour, - maxEdgeId_left, - maxEdgeId_right, - false, //isHScan, - vTreeStart, - hTreeStart, - matchTable_LR, - LR_pairs, - LR_contourPairs, - &LR_matchNum); - - if (LR_matchNum < 25) - continue; - - int lowLevelChkFlag = 0; - SSG_peakRgnInfo a_pkRgn = _maxLikelihoodMatch( - laser3DPoints, - lineNum, - hvTreeSize, - peaks[i], - matchTable_TB, - TB_pairs, - TB_contourPairs, - TB_matchNum, - maxEdgeId_btm, - matchTable_LR, - LR_pairs, - LR_contourPairs, - LR_matchNum, - maxEdgeId_right, - allTreesInfo, - vTreeStart, - hTreeStart, - globalROI, - algoParam, - peakRgnId, - &lowLevelChkFlag); - if (a_pkRgn.pkRgnIdx > 0) + int marker = wsImg.markers[py][px]; + if ((marker >= startMarkerID)&&( marker <= maxMkID)) { - peakRgns.push_back(a_pkRgn); - peakRgnId++; + pt3D->nPointIdx = marker; + segObjs[marker].push_back(pt3D->pt3D); } } -#if 1 - ///֤ûдڴӵĿδ - ///ʣµĿ꣬ûмʹС - ///ɨʱⱣ֤ˮƽֱĿǷӳߴ磬Ȼһ - while (1) + } + + //Ŀ + for (int i = startMarkerID; i < maxMkID; i++) + { + if (segObjs[i].size() < 10) + continue; + + //ȡͶӰ,ͳСZZ + double minZ = -1; + double maxZ = 0; + std::vector points; + int ptSize = (int)segObjs[i].size(); + for (int m = 0; m < ptSize; m++) { - std::vector iter_objs; - //ûдPeak㱣 - std::vector residualPeaks; - for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++) + float x = (float)segObjs[i][m].x; + float y = (float)segObjs[i][m].y; + points.push_back(cv::Point2f(x, y)); + if (minZ < 0) { - SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]); - int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff; - if (pkRgnId == 0) - { - residualPeaks.push_back(peaks[i]); - } - } - if (residualPeaks.size() == 0) - break; - - bool rgnPtAsEdge = true; - for (int ri = 0; ri < residualPeaks.size(); ri++) - { - SVzNL3DPosition* pk_pt = &(laser3DPoints[residualPeaks[ri].x].p3DPosition[residualPeaks[ri].y]); - int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff; - if (pkRgnId > 0) - continue; - // - //ˮƽʹֱɨõ߽ - std::vector< SSG_lineConotours> resi_topContour; - std::vector< SSG_lineConotours> resi_bottomContour; - std::vector< SSG_lineConotours> resi_leftContour; - std::vector< SSG_lineConotours> resi_rightContour; - int resi_maxEdgeId_top = 0, resi_maxEdgeId_btm = 0, resi_maxEdgeId_left = 0, resi_maxEdgeId_right = 0; - sg_peakXYScan( - laser3DPoints, - lineNum, - featureMask, - residualPeaks[ri], - algoParam.growParam, - algoParam.bagParam, - true, - resi_topContour, - resi_bottomContour, - resi_leftContour, - resi_rightContour, - &resi_maxEdgeId_top, - &resi_maxEdgeId_btm, - &resi_maxEdgeId_left, - &resi_maxEdgeId_right); - - if ((resi_topContour.size() == 0) || (resi_bottomContour.size() == 0) || (resi_leftContour.size() == 0) || (resi_rightContour.size() == 0)) - continue; - - //ֶμƽȺƽ߶ԼֶROI - //ȫƥ䣨ˮƽзֶεпܾ - std::vector matchTable_TB; - std::vector< SSG_matchPair> TB_pairs; - std::vector TB_contourPairs; - int TB_matchNum = 0; - _getMatchTable( - resi_topContour, - resi_bottomContour, - resi_maxEdgeId_top, - resi_maxEdgeId_btm, - true, //isVScan, - vTreeStart, - hTreeStart, - matchTable_TB, - TB_pairs, - TB_contourPairs, - &TB_matchNum); - - if (TB_matchNum < 25) - continue; - - std::vector< SSG_matchPair> LR_pairs; - std::vector matchTable_LR; - std::vector LR_contourPairs; - int LR_matchNum = 0; - _getMatchTable( - resi_leftContour, - resi_rightContour, - resi_maxEdgeId_left, - resi_maxEdgeId_right, - false, //isHScan, - vTreeStart, - hTreeStart, - matchTable_LR, - LR_pairs, - LR_contourPairs, - &LR_matchNum); - - if (LR_matchNum < 25) - continue; - - int lowLevelChkFlag = 0; - SSG_peakRgnInfo a_pkRgn = _maxLikelihoodMatch( - laser3DPoints, - lineNum, - hvTreeSize, - peaks[ri], - matchTable_TB, - TB_pairs, - TB_contourPairs, - TB_matchNum, - resi_maxEdgeId_btm, - matchTable_LR, - LR_pairs, - LR_contourPairs, - LR_matchNum, - resi_maxEdgeId_right, - allTreesInfo, - vTreeStart, - hTreeStart, - globalROI, - algoParam, - peakRgnId, - &lowLevelChkFlag); -#if 0 - if (lowLevelChkFlag > 0) - { - //lowLevelFlag_T + lowLevelFlag_B<<1 + lowLevelFlag_L<<2 + lowLevelFlag_R<<3; - if (lowLevelChkFlag & 0x01) //Top - { - - } - if (lowLevelChkFlag & 0x02) //Bottom - { - } - if (lowLevelChkFlag & 0x04) //Left - { - } - if (lowLevelChkFlag & 0x08) //Rigjt - { - } - } -#endif - if (a_pkRgn.pkRgnIdx > 0) - { - iter_objs.push_back(a_pkRgn); - peakRgnId++; - } - } - if (iter_objs.size() == 0) - break; - - peakRgns.insert(peakRgns.end(), iter_objs.begin(), iter_objs.end()); - //Ϊһε׼ - peaks.clear(); - peaks.insert(peaks.end(), residualPeaks.begin(), residualPeaks.end()); - } - - ///ʣСĿײ - ///δ֪δԷֹδ֪ײ - ///¼еĴ1/4L*1/4WĿ - std::vector smallObjPeaks; //¼0.25L * 0.25WĿ꣬ײ - //ûдPeak㱣 - std::vector residualPeaks; - for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++) - { - SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]); - int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff; - if (pkRgnId == 0) - { - residualPeaks.push_back(peaks[i]); - } - } - if (residualPeaks.size() > 0) - { - bool rgnPtAsEdge = true; - for (int ri = 0; ri < residualPeaks.size(); ri++) - { - // - //ˮƽʹֱɨõ߽ - std::vector< SSG_lineConotours> resi_topContour; - std::vector< SSG_lineConotours> resi_bottomContour; - std::vector< SSG_lineConotours> resi_leftContour; - std::vector< SSG_lineConotours> resi_rightContour; - int resi_maxEdgeId_top = 0, resi_maxEdgeId_btm = 0, resi_maxEdgeId_left = 0, resi_maxEdgeId_right = 0; - sg_peakXYScan( - laser3DPoints, - lineNum, - featureMask, - residualPeaks[ri], - algoParam.growParam, - algoParam.bagParam, - true, - resi_topContour, - resi_bottomContour, - resi_leftContour, - resi_rightContour, - &resi_maxEdgeId_top, - &resi_maxEdgeId_btm, - &resi_maxEdgeId_left, - &resi_maxEdgeId_right); - //ROI - //޵ - SSG_ROIRectD objROI = { 0, -1, 0, 0 }; - for (int n = 0; n < resi_topContour.size(); n++) - { - std::vector& a_line_contourPts = resi_topContour[n].contourPts; - for (int m = 0; m < a_line_contourPts.size(); m++) - { - if (objROI.right < objROI.left) - { - objROI.left = a_line_contourPts[m].edgePt.x; - objROI.right = a_line_contourPts[m].edgePt.x; - objROI.top = a_line_contourPts[m].edgePt.y; - objROI.bottom = a_line_contourPts[m].edgePt.y; - } - else - { - objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left; - objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right; - objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top; - objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom; - } - } - } - for (int n = 0; n < resi_bottomContour.size(); n++) - { - std::vector& a_line_contourPts = resi_bottomContour[n].contourPts; - for (int m = 0; m < a_line_contourPts.size(); m++) - { - if (objROI.right < objROI.left) - { - objROI.left = a_line_contourPts[m].edgePt.x; - objROI.right = a_line_contourPts[m].edgePt.x; - objROI.top = a_line_contourPts[m].edgePt.y; - objROI.bottom = a_line_contourPts[m].edgePt.y; - } - else - { - objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left; - objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right; - objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top; - objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom; - } - } - } - for (int n = 0; n < resi_leftContour.size(); n++) - { - std::vector& a_line_contourPts = resi_leftContour[n].contourPts; - for (int m = 0; m < a_line_contourPts.size(); m++) - { - if (objROI.right < objROI.left) - { - objROI.left = a_line_contourPts[m].edgePt.x; - objROI.right = a_line_contourPts[m].edgePt.x; - objROI.top = a_line_contourPts[m].edgePt.y; - objROI.bottom = a_line_contourPts[m].edgePt.y; - } - else - { - objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left; - objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right; - objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top; - objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom; - } - } - } - for (int n = 0; n < resi_rightContour.size(); n++) - { - std::vector& a_line_contourPts = resi_rightContour[n].contourPts; - for (int m = 0; m < a_line_contourPts.size(); m++) - { - if (objROI.right < objROI.left) - { - objROI.left = a_line_contourPts[m].edgePt.x; - objROI.right = a_line_contourPts[m].edgePt.x; - objROI.top = a_line_contourPts[m].edgePt.y; - objROI.bottom = a_line_contourPts[m].edgePt.y; - } - else - { - objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left; - objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right; - objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top; - objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom; - } - } - } - //ROIС - double obj_L = objROI.right - objROI.left; - double obj_W = objROI.bottom - objROI.top; - if (obj_L < obj_W) - { - double tmp_value = obj_W; - obj_W = obj_L; - obj_L = tmp_value; - } - if ((obj_L > algoParam.bagParam.bagL * 0.25) && (obj_W > algoParam.bagParam.bagW * 0.25)) - { - smallObjPeaks.push_back(residualPeaks[ri]); - } - } - } - -#endif - - //Ŀ򣺷ֲ -> ߲ -> ߲һд - if (peakRgns.size() > 0) - { - double maxHeight = peakRgns[0].centerPos.z; - for (int i = 1; i < peakRgns.size(); i++) - { - if (maxHeight > peakRgns[i].centerPos.z) - maxHeight = peakRgns[i].centerPos.z; - } - //ȡͬ߶ȲĿ - std::vector level0_objs; - for (int i = 0, i_max = (int)peakRgns.size(); i < i_max; i++) - { - double z_diff = peakRgns[i].centerPos.z - maxHeight; - if (z_diff < algoParam.bagParam.bagH / 2) //ֲ - { - level0_objs.push_back(peakRgns[i]); - } - } - peakRgns.clear(); - peakRgns.insert(peakRgns.end(), level0_objs.begin(), level0_objs.end()); - int level0_size = (int)peakRgns.size(); - if (level0_size > 1) //һ򣬷 - { - //ȡYСĿ - double minY = 0; - double minY_idx = -1; - for (int i = 0; i < level0_size; i++) - { - if (minY_idx < 0) - { - minY = peakRgns[i].centerPos.y; - minY_idx = i; - } - else - { - if (minY > peakRgns[i].centerPos.y) - { - minY = peakRgns[i].centerPos.y; - minY_idx = i; - } - } - } - std::vector row_0_outlier; - for (int i = 0; i < level0_size; i++) - { - double y_diff = peakRgns[i].centerPos.y - minY; - if (y_diff < algoParam.bagParam.bagW / 2) //һ - objOps.push_back(peakRgns[i]); - else - row_0_outlier.push_back(i); //еż¼ - } - //ԵһеĿ갴 - if (objOps.size() > 1) - { - std::sort(objOps.begin(), objOps.end(), compareByXValue); - } - for (int i = 0; i < row_0_outlier.size(); i++) - objOps.push_back(peakRgns[row_0_outlier[i]]); -#if 0 - for (int i = level0_end + 1; i < peakRgns.size(); i++) - objOps.push_back(peakRgns[i]); -#endif + minZ = segObjs[i][m].z; + maxZ = segObjs[i][m].z; } else - objOps.insert(objOps.end(), peakRgns.begin(), peakRgns.end()); - } - //ײ - if ((objOps.size() > 0) && (smallObjPeaks.size() > 0)) - { - SSG_peakRgnInfo* highest_obj = &objOps[0]; - double objZ = highest_obj->centerPos.z; - for (int i = 0; i < smallObjPeaks.size(); i++) { - SSG_2DValueI* a_samllPk = &smallObjPeaks[i]; - if (highest_obj->centerPos.z > a_samllPk->valueD + algoParam.bagParam.bagH / 2) - { - SVzNL3DPosition* smallPkPt = &laser3DPoints[a_samllPk->x].p3DPosition[a_samllPk->y]; - double dist = sqrt(pow(highest_obj->centerPos.x - smallPkPt->pt3D.x, 2) + pow(highest_obj->centerPos.y - smallPkPt->pt3D.y, 2)); - double dia_angle = sqrt(pow(highest_obj->objSize.dWidth, 2) + pow(highest_obj->objSize.dHeight, 2)); - //ͬȽ - double z_diff = smallPkPt->pt3D.z - objZ; - if (z_diff < algoParam.bagParam.bagH / 2) //ֲ - { - if (dist < dia_angle / 2) - objOps.clear(); //μЧ - } - } + if (minZ > segObjs[i][m].z) minZ = segObjs[i][m].z; + if (maxZ < segObjs[i][m].z) maxZ = segObjs[i][m].z; } } - //ͶԭϵԱ۱궨ȷ - for (int i = 0; i < lineNum; i++) - sg_lineDataR(&laser3DPoints[i], poseCalibPara.invRMatrix, -1); - //Ͷԭϵ - double invMatrix[3][3]; - invMatrix[0][0] = poseCalibPara.invRMatrix[0]; - invMatrix[0][1] = poseCalibPara.invRMatrix[1]; - invMatrix[0][2] = poseCalibPara.invRMatrix[2]; - invMatrix[1][0] = poseCalibPara.invRMatrix[3]; - invMatrix[1][1] = poseCalibPara.invRMatrix[4]; - invMatrix[1][2] = poseCalibPara.invRMatrix[5]; - invMatrix[2][0] = poseCalibPara.invRMatrix[6]; - invMatrix[2][1] = poseCalibPara.invRMatrix[7]; - invMatrix[2][2] = poseCalibPara.invRMatrix[8]; - for (int i = 0, i_max = (int)objOps.size(); i < i_max; i++) + if (points.size() == 0) + continue; + //СӾ + // СӾ + cv::RotatedRect rect = minAreaRect(points); + cv::Point2f vertices[4]; + rect.points(vertices); + double width = rect.size.width; //ͶӰĿ͸ߣ ӦӵijͿ + double height = rect.size.height; + if (width < height) { - SSG_EulerAngles euAngle = { objOps[i].centerPos.x_roll, objOps[i].centerPos.y_pitch, objOps[i].centerPos.z_yaw }; - double pose[3][3]; - eulerToRotationMatrixZYX(euAngle, pose); - double resultMatrix[3][3]; - for (int i = 0; i < 3; i++) - { - for (int j = 0; j < 3; j++) - { - resultMatrix[i][j] = 0; - for (int m = 0; m < 3; m++) - resultMatrix[i][j] += invMatrix[i][m] * pose[m][j]; - } - } - SSG_EulerAngles resultEuAngle = rotationMatrixToEulerZYX(resultMatrix); - objOps[i].centerPos.z_yaw = resultEuAngle.yaw; - double x = objOps[i].centerPos.x * poseCalibPara.invRMatrix[0] + - objOps[i].centerPos.y * poseCalibPara.invRMatrix[1] + - objOps[i].centerPos.z * poseCalibPara.invRMatrix[2]; - double y = objOps[i].centerPos.x * poseCalibPara.invRMatrix[3] + - objOps[i].centerPos.y * poseCalibPara.invRMatrix[4] + - objOps[i].centerPos.z * poseCalibPara.invRMatrix[5]; - double z = objOps[i].centerPos.x * poseCalibPara.invRMatrix[6] + - objOps[i].centerPos.y * poseCalibPara.invRMatrix[7] + - objOps[i].centerPos.z * poseCalibPara.invRMatrix[8]; - objOps[i].centerPos.x = x; - objOps[i].centerPos.y = y; - objOps[i].centerPos.z = z; + double tmp = height; + height = width; + width = tmp; } -#endif - //ˮƽʹֱȡ - - //о任 - //ˮָ - //z߶ȼĿ + SWD_ParticlePosInfo a_obj; + a_obj.size.length = width; + a_obj.size.width = height; + a_obj.size.height = maxZ - minZ; + for (int m = 0; m < 4; m++) + { + SVzNL3DPoint vPt_btm = { vertices[m].x, vertices[m].y, maxZ }; + SVzNL3DPoint vPt_top = { vertices[m].x, vertices[m].y, minZ }; + a_obj.vertix[m] = vPt_btm; + a_obj.vertix[m + 4] = vPt_top; + } + particles.push_back(a_obj); + } } \ No newline at end of file diff --git a/sourceCode/WD_particleSizeMeasure_Export.h b/sourceCode/WD_particleSizeMeasure_Export.h index 133ddda..960b3dd 100644 --- a/sourceCode/WD_particleSizeMeasure_Export.h +++ b/sourceCode/WD_particleSizeMeasure_Export.h @@ -10,7 +10,7 @@ #include #include -#define OUTPUT_DEBUG 1 +#define OUTPUT_DEBUG 0 typedef struct { @@ -27,10 +27,14 @@ typedef struct typedef struct { - double EQRadius; - SVzNL3DPoint center_pos; + //double EQRadius; + SWD_sizeParam size; + SVzNL3DPoint vertix[8]; }SWD_ParticlePosInfo; +//汾 +SG_APISHARED_EXPORT const char* wd_particleSegVersion(void); + //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ diff --git a/sourceCode/WD_watershed.cpp b/sourceCode/WD_watershed.cpp index a718f89..5da6894 100644 --- a/sourceCode/WD_watershed.cpp +++ b/sourceCode/WD_watershed.cpp @@ -246,6 +246,9 @@ void saveResult(const string& filename, const Image& img) { const int dx[] = { -1, -1, -1, 0, 0, 1, 1, 1 }; const int dy[] = { -1, 0, 1, -1, 1, -1, 0, 1 }; +const int dx4[] = { -1, 0, 0, 1}; +const int dy4[] = { 0, -1, 1, 0}; + // ֲСֵΪʼӵ void findMinima(SWD_waterShedImage& img, int& markerCount) { markerCount = 1; //ӦID @@ -302,7 +305,7 @@ void findMinima(SWD_waterShedImage& img, int& markerCount) { } // ˮ㷨 -void watershed(SWD_waterShedImage& img) +void watershed(SWD_waterShedImage& img) { int markerCount; findMinima(img, markerCount); // Զӵ @@ -311,7 +314,7 @@ void watershed(SWD_waterShedImage& img) vector>> pixels; // (Ҷֵ, (x,y)) for (int i = 0; i < img.height; ++i) { for (int j = 0; j < img.width; ++j) { - pixels.emplace_back(img.gray[i][j], make_pair(i, j)); + pixels.emplace_back(img.gray[i][j], make_pair(j, i)); } } sort(pixels.begin(), pixels.end()); @@ -348,19 +351,156 @@ void watershed(SWD_waterShedImage& img) } } -#if 0 -int main() { - Image img; - if (!readPPM("input.ppm", img)) { // PPMͼ - cerr << "޷ȡͼ" << endl; - return -1; +//ģעˮעˮڿʼͬˮλע +// inlet: עˮ +// level: ˮλ +void waterInjection(SWD_waterShedImage& img, SVzNL2DPoint inlet, int level) +{ + int py = inlet.y; + int px = inlet.x; + int marker = img.markers[py][px]; + std::vector levelPts; + levelPts.push_back(inlet); + int readPtr = 0; + while (readPtr < levelPts.size()) + { + SVzNL2DPoint seed = levelPts[readPtr]; + readPtr++; + + //4ڽ + for (int d = 0; d < 4; ++d) + { + int nx = seed.x + dx4[d]; + int ny = seed.y + dy4[d]; + if (ny >= 0 && ny < img.height && nx >= 0 && nx < img.width) + { + if ((img.gray[ny][nx] == level) && (img.markers[ny][nx] == 0)) + { + img.markers[ny][nx] = marker; + SVzNL2DPoint nxt_seed = { nx, ny }; + levelPts.push_back(nxt_seed); + } + } + } + } +} + +bool _checkMarkerExist(std::vector& MarkerLst, int marker) +{ + bool exist = false; + for (int i = 0, i_max = (int)MarkerLst.size(); i < i_max; i++) + { + if (MarkerLst[i] == marker) + { + exist = true; + break; + } + } + return exist; +} + +// ӵзˮ㷨 +// watershedSeeds:ӵ +// maxLevel:ˮλֵ +void wd_seedWatershed(SWD_waterShedImage& img, std::vector& watershedSeeds, int maxLevel, int startMakerID) +{ + int markerCount = startMakerID; + int seedSize = (int)watershedSeeds.size(); + for (int i = 0; i < seedSize; i++) + { + int px = watershedSeeds[i].x; + int py = watershedSeeds[i].y; + watershedSeeds[i].value = markerCount; + img.markers[py][px] = markerCount; // ӵ + int greyValue = img.gray[py][px]; //ˮλ + SVzNL2DPoint inlet = { px, py }; //עˮ + waterInjection(img, inlet, greyValue);//עˮͬһˮλӵIJע + markerCount++; } - watershed(img); // ִзˮָ - saveResult("watershed_result.ppm", img); // + // Ҷֵأģˮλ + std::vector> levelPtList; + levelPtList.resize(maxLevel + 1); + for (int y = 0; y < img.height; ++y) + { + for (int x = 0; x < img.width; ++x) + { + int level = img.gray[y][x]; + if (level <= maxLevel) + { + SVzNL2DPoint a_pt = { x, y }; + levelPtList[level].push_back(a_pt); + } + } + } - cout << "ָɣѱΪ watershed_result.ppm" << endl; - return 0; + while (1) + { + bool allDone = true; + // أִעˮ + for (int i = 0; i <= maxLevel; i++) + { + if (levelPtList[i].size() == 0) + continue; + + //ͬһˮλlevel)ִеעˮ + bool doInjection = true; + while (doInjection) + { + int injectionNum = 0; + std::vector resiPts; + int ptSize = (int)levelPtList[i].size(); + for (int pi = 0; pi < ptSize; pi++) + { + int x = levelPtList[i][pi].x; + int y = levelPtList[i][pi].y; + if (img.markers[y][x] > 0) continue; // ѱǵӵ + + // ռѱǵ + vector neighborMarkers; + for (int d = 0; d < 8; ++d) + { + int nx = x + dx[d]; + int ny = y + dy[d]; + if (ny >= 0 && ny < img.height && nx >= 0 && nx < img.width) + { + int m = img.markers[ny][nx]; + if (m > 0) + { + bool exist = _checkMarkerExist(neighborMarkers, m); + if (false == exist) + neighborMarkers.push_back(m); + } + } + } + + if (neighborMarkers.empty()) + { + resiPts.push_back(levelPtList[i][pi]);// ǣȴ + } + else if (neighborMarkers.size() == 1) { + img.markers[y][x] = neighborMarkers[0]; // ͬһ + injectionNum++; + } + else { + // 򽻻㣬Ϊˮ + img.markers[y][x] = -1; + injectionNum++; + } + } + levelPtList[i].clear(); + levelPtList[i].insert(levelPtList[i].end(), resiPts.begin(), resiPts.end()); + + if ((injectionNum == 0) || (levelPtList[i].size() == 0)) + { + doInjection = false; + if (levelPtList[i].size() > 0) + allDone = false; + } + } + } + if (true == allDone) + break; + } } -#endif #endif \ No newline at end of file From 98d917895dc0b4a76f5bb499cffedf562db10c76 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Fri, 5 Dec 2025 18:41:05 +0800 Subject: [PATCH 09/12] =?UTF-8?q?-=E6=B7=BB=E5=8A=A0=E5=8F=8C=E7=9B=AEMark?= =?UTF-8?q?=E7=9B=B8=E6=9C=BA=E9=A1=B9=E7=9B=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../BQ_workpieceCornerExtract_test.cpp | 74 +- SG_Algorithm.sln | 27 + baseAlgorithm/baseAlgorithm.vcxproj | 1 + .../motorStatorPosition_test.cpp | 12 +- sourceCode/BQ_workpieceCornerExtraction.cpp | 44 +- .../BQ_workpieceCornerExtraction_Export.h | 11 +- sourceCode/SG_baseAlgo_Export.h | 32 +- sourceCode/SG_baseDataType.h | 39 +- sourceCode/SG_baseFunc.cpp | 5 +- sourceCode/SG_errCode.h | 7 +- sourceCode/SG_featureGrow.cpp | 82 +++ sourceCode/SG_lineFeature.cpp | 383 +++++++++- sourceCode/WD_particleSizeMeasure.cpp | 63 +- sourceCode/motorStatorPosition.cpp | 681 +++++++++++++++++- sourceCode/motorStatorPosition_Export.h | 44 +- 15 files changed, 1347 insertions(+), 158 deletions(-) diff --git a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp index 3b7f34c..a502b36 100644 --- a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp +++ b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp @@ -445,26 +445,11 @@ void _outputRGBDScanLapWeld_RGBD( featureType_v &= 0x0f; if (true == outDebugInfo) { - if (LINE_FEATURE_L_JUMP_H2L == featureType_v) + if (pt3D->nPointIdx < 0) { 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 }; @@ -627,7 +612,7 @@ void _outputRGBDScanLapWeld_RGBD( #define CONVERT_TO_GRID 0 #define TEST_COMPUTE_CALIB_PARA 0 #define TEST_COMPUTE_CORNER 1 -#define TEST_GROUP 6 +#define TEST_GROUP 7 int main() { const char* dataPath[TEST_GROUP] = { @@ -637,10 +622,11 @@ int main() "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度3\\", //3 "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度4\\", //4 "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\", //5 + "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\顶板样式\\", //6 }; SVzNLRange fileIdx[TEST_GROUP] = { - {1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,11} + {1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,11}, {2,4} }; #if CONVERT_TO_GRID @@ -667,7 +653,7 @@ int main() #if TEST_COMPUTE_CALIB_PARA char _calib_datafile[256]; - sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\9_LazerData_Hi229229.txt"); + sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\顶板样式\\节点4.txt"); int lineNum = 0; float lineV = 0.0f; int dataCalib = 0; @@ -703,10 +689,10 @@ int main() } // char calibFile[250]; - sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\ground_calib_para.txt"); + 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"); + 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); @@ -717,7 +703,7 @@ int main() const char* ver = wd_BQWorkpieceCornerVersion(); printf("ver:%s\n", ver); - for (int grp = 1; grp <= 5; grp++) + for (int grp = 6; grp <= 6; grp++) { SSG_planeCalibPara poseCalibPara; //初始化成单位阵 @@ -734,36 +720,8 @@ int main() 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); - } - else if (grp == 1) - { - sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度1\\ground_calib_para.txt"); - poseCalibPara = _readCalibPara(calibFile); - } - else if (grp == 2) - { - sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度2\\ground_calib_para.txt"); - poseCalibPara = _readCalibPara(calibFile); - } - else if (grp == 3) - { - sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度3\\ground_calib_para.txt"); - poseCalibPara = _readCalibPara(calibFile); - } - else if (grp == 4) - { - sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度4\\ground_calib_para.txt"); - poseCalibPara = _readCalibPara(calibFile); - } - else if (grp == 5) - { - sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\ground_calib_para.txt"); - poseCalibPara = _readCalibPara(calibFile); - } + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); + poseCalibPara = _readCalibPara(calibFile); for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { @@ -771,6 +729,8 @@ int main() char _scan_file[256]; if(0 == grp) sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx); + else if(6 == grp) + sprintf_s(_scan_file, "%s节点%d.txt", dataPath[grp], fidx); else sprintf_s(_scan_file, "%s%d_LazerData_Hi229229.txt", dataPath[grp], fidx); std::vector> scanLines; @@ -788,7 +748,13 @@ int main() int kkk = 1; //行处理 //调平，去除地面 - sx_BQ_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1); + double cuttingZ = -1; + if (6 == grp) + { + if (2 == fidx) + cuttingZ = 1760.0; + } + sx_BQ_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ); } #if 0 char _out_file[256]; @@ -834,7 +800,7 @@ int main() //输出测试结果 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); + _outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, true, debug_conturs); #else _outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, true, NULL); #endif diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index 5957761..ab6ab62 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -128,6 +128,17 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "particleSizeMeasurement", " {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} EndProjectSection EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "binocularMarkCam", "binocularMarkCam\binocularMarkCam.vcxproj", "{AB25A4FE-6457-4CB4-A190-EBEF603335C7}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "binocularMarkCam_test", "BinocularMarkCam_test\BinocularMarkCam_test.vcxproj", "{FB3BCF92-DF8B-4CEC-AA39-4ECD31F82718}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + {AB25A4FE-6457-4CB4-A190-EBEF603335C7} = {AB25A4FE-6457-4CB4-A190-EBEF603335C7} + EndProjectSection +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -336,6 +347,22 @@ Global {8FD0F574-61C6-4094-8521-33AEDDB44797}.Release|x64.Build.0 = Release|x64 {8FD0F574-61C6-4094-8521-33AEDDB44797}.Release|x86.ActiveCfg = Release|Win32 {8FD0F574-61C6-4094-8521-33AEDDB44797}.Release|x86.Build.0 = Release|Win32 + {AB25A4FE-6457-4CB4-A190-EBEF603335C7}.Debug|x64.ActiveCfg = Debug|x64 + {AB25A4FE-6457-4CB4-A190-EBEF603335C7}.Debug|x64.Build.0 = Debug|x64 + {AB25A4FE-6457-4CB4-A190-EBEF603335C7}.Debug|x86.ActiveCfg = Debug|Win32 + {AB25A4FE-6457-4CB4-A190-EBEF603335C7}.Debug|x86.Build.0 = Debug|Win32 + {AB25A4FE-6457-4CB4-A190-EBEF603335C7}.Release|x64.ActiveCfg = Release|x64 + {AB25A4FE-6457-4CB4-A190-EBEF603335C7}.Release|x64.Build.0 = Release|x64 + {AB25A4FE-6457-4CB4-A190-EBEF603335C7}.Release|x86.ActiveCfg = Release|Win32 + {AB25A4FE-6457-4CB4-A190-EBEF603335C7}.Release|x86.Build.0 = Release|Win32 + {FB3BCF92-DF8B-4CEC-AA39-4ECD31F82718}.Debug|x64.ActiveCfg = Debug|x64 + {FB3BCF92-DF8B-4CEC-AA39-4ECD31F82718}.Debug|x64.Build.0 = Debug|x64 + {FB3BCF92-DF8B-4CEC-AA39-4ECD31F82718}.Debug|x86.ActiveCfg = Debug|Win32 + {FB3BCF92-DF8B-4CEC-AA39-4ECD31F82718}.Debug|x86.Build.0 = Debug|Win32 + {FB3BCF92-DF8B-4CEC-AA39-4ECD31F82718}.Release|x64.ActiveCfg = Release|x64 + {FB3BCF92-DF8B-4CEC-AA39-4ECD31F82718}.Release|x64.Build.0 = Release|x64 + {FB3BCF92-DF8B-4CEC-AA39-4ECD31F82718}.Release|x86.ActiveCfg = Release|Win32 + {FB3BCF92-DF8B-4CEC-AA39-4ECD31F82718}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/baseAlgorithm/baseAlgorithm.vcxproj b/baseAlgorithm/baseAlgorithm.vcxproj index f858582..f6dddb0 100644 --- a/baseAlgorithm/baseAlgorithm.vcxproj +++ b/baseAlgorithm/baseAlgorithm.vcxproj @@ -173,6 +173,7 @@ + diff --git a/motorStatorPosition_test/motorStatorPosition_test.cpp b/motorStatorPosition_test/motorStatorPosition_test.cpp index c8fcac6..2963c4f 100644 --- a/motorStatorPosition_test/motorStatorPosition_test.cpp +++ b/motorStatorPosition_test/motorStatorPosition_test.cpp @@ -842,10 +842,13 @@ void _genXOYProjectionImage(cv::String& fileName, SVzNL3DLaserLine* scanData, in return; } +#define CONVERT_TO_GRID 0 +#define TEST_COMPUTE_CALIB_PARA 0 +#define TEST_COMPUTE_GRASP_POS 1 #define TEST_GROUP 1 int main() { -#if 0 +#if CONVERT_TO_GRID //将数据转换成栅格格式格式 char _scan_dir[256]; int lineNum = 0; @@ -877,10 +880,11 @@ int main() lineV, maxTimeStamp, clockPerSecond); printf("%s: convert done!\n", _scan_file); } -#else +#endif +#if TEST_COMPUTE_GRASP_POS const char* dataPath[TEST_GROUP] = { - "E:\\上古\\电机定子\\", //0 + "E:\\ShangGu\\电机定子\\", //0 }; SVzNLRange fileIdx[TEST_GROUP] = { @@ -891,7 +895,7 @@ int main() 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; - SG_motorStatorPositionParam algoParam; + SWD_statorParam algoParam; algoParam.statorOuterD = 85.0; algoParam.statorInnerD = 50.0; diff --git a/sourceCode/BQ_workpieceCornerExtraction.cpp b/sourceCode/BQ_workpieceCornerExtraction.cpp index efa1f23..83f5dd5 100644 --- a/sourceCode/BQ_workpieceCornerExtraction.cpp +++ b/sourceCode/BQ_workpieceCornerExtraction.cpp @@ -5,7 +5,9 @@ #include #include -std::string m_strVersion = "1.1.0"; +//version 1.1.0 : base version release to customer, output corner coordinate +//version 1.2.0 : add position length output +std::string m_strVersion = "1.2.0"; const char* wd_BQWorkpieceCornerVersion(void) { return m_strVersion.c_str(); @@ -181,6 +183,17 @@ bool compareByAngle(const SWD_polarPt& a, const SWD_polarPt& b) { return a.angle < b.angle; } +int _counterLinePtNum(std::vector& lineData) +{ + int ptNum = 0; + for (int i = 0, i_max = (int)lineData.size(); i < i_max; i++) + { + if (lineData[i].pt3D.z > 1e-4) + ptNum++; + } + return ptNum; +} + SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( std::vector< std::vector>& scanLines, const SSG_cornerParam cornerPara, @@ -203,6 +216,33 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( return workpieceCorners; } + //ʼͽĿհɨȥɨ߽ + int validStartLine = -1; + for (int i = 0; i < lineNum; i++) + { + int linePtNum = _counterLinePtNum(scanLines[i]); + if (linePtNum > 0) + { + validStartLine = i; + break; + } + } + int validEndLine = -1; + for (int i = lineNum - 1; i >= 0; i--) + { + int linePtNum = _counterLinePtNum(scanLines[i]); + if (linePtNum > 0) + { + validEndLine = i; + break; + } + } + if ( (validStartLine < 0) || (validEndLine < 0)) + { + *errCode = SG_ERR_3D_DATA_NULL; + return workpieceCorners; + } + int linePtNum = (int)scanLines[0].size(); bool isGridData = true; @@ -213,7 +253,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( std::vector> jumpFeatures_v_raw; for (int line = 0; line < lineNum; line++) { - if (line == 202) + if (line == 250) int kkk = 1; std::vector& lineData = scanLines[line]; diff --git a/sourceCode/BQ_workpieceCornerExtraction_Export.h b/sourceCode/BQ_workpieceCornerExtraction_Export.h index 7e501d2..3fb2841 100644 --- a/sourceCode/BQ_workpieceCornerExtraction_Export.h +++ b/sourceCode/BQ_workpieceCornerExtraction_Export.h @@ -19,11 +19,16 @@ typedef struct typedef struct { - int workpieceType; - SVzNL3DPoint corner_L[3]; + int workpieceType; //1-ڵ1 2-ڵ2 3-ڵ34-ڵ40-δ֪ -Ƿ + SVzNL3DPoint corner_L[3]; SVzNL3DPoint corner_R[3]; SVzNL3DPoint corner_T[3]; SVzNL3DPoint corner_B[3]; + SVzNL3DPoint center; //ĵ + double len135_A1; //A1ȣӹ135ȷ + double len45_B1; //B1ȣӹ45ȷ + double len315_B2; //B2ȣӹ315ȷ + double len225_A2; //A2ȣӹ225ȷ }SSX_BQworkpieceResult; typedef struct @@ -55,7 +60,7 @@ SG_APISHARED_EXPORT void sx_BQ_lineDataR( const double* camPoseR, double groundH); -//ȡӺ +//ȡǵ㼰λϢ SG_APISHARED_EXPORT SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( std::vector< std::vector>& scanLines, const SSG_cornerParam cornerPara, diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index a1cac70..511d5d4 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -90,6 +90,23 @@ SG_APISHARED_EXPORT void wd_getLineCornerFeature_PSM( const SSG_cornerParam cornerPara, SSG_lineFeature* line_features); +/// ȡϵԲϰλԲĿȷ +/// seg˵㣺z +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +SG_APISHARED_EXPORT void wd_getRingArcFeature( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const SSG_cornerParam cornerPara, + double ringArcWidth, //ӵĻ + std::vector& line_ringArcs // +); + /// /// ȡϵJumping /// nPointIdx¶Feature @@ -162,6 +179,12 @@ SG_APISHARED_EXPORT void wd_getFeatureGrowingTrees_noTypeMatch( std::vector& ending_trees, SSG_treeGrowParam growParam); +//segment feature +SG_APISHARED_EXPORT void wd_getSegFeatureGrowingTrees( + std::vector>& all_lineFeatures, + std::vector& feature_trees, + SSG_treeGrowParam growParam); + SG_APISHARED_EXPORT void sg_lineFeaturesGrowing( int lineIdx, bool isLastLine, @@ -406,6 +429,7 @@ SG_APISHARED_EXPORT void pointClout2DProjection( SVzNLRangeD x_range, SVzNLRangeD y_range, double scale, + double cuttingGrndZ, int edgeSkip, double inerPolateDistTh, //ֵֵڴֵIJֵ cv::Mat& projectionData,//ͶӰݣʼΪһֵ1e+6 @@ -420,4 +444,10 @@ SG_APISHARED_EXPORT void wd_seedWatershed( std::vector& watershedSeeds, //ӵ int maxLevel, //ˮλ int startMakerID //ʼMarkerID -); \ No newline at end of file +); + +//˲Ԥ +SG_APISHARED_EXPORT void wd_noiseFilter( + std::vector< std::vector>& scanLines, + const SSG_outlierFilterParam filterParam, + int* errCode); \ No newline at end of file diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index be84d9e..4fbce8c 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -177,6 +177,15 @@ typedef struct double valleyMaxW; }SSG_VFeatureParam; +typedef struct +{ + int lineIdx; + int startPtIdx; + int endPtIdx; + SVzNL3DPoint startPt; + SVzNL3DPoint endPt; +}SWD_segFeature; + typedef struct { double sameGapTh; @@ -251,6 +260,15 @@ typedef struct std::vector< SSG_featureSemiCircle> treeNodes; }SSG_semiCircleFeatureTree; +typedef struct +{ + int treeState; + int sLineIdx; + int eLineIdx; + double tree_value; + std::vector< SWD_segFeature> treeNodes; +}SWD_segFeatureTree; + typedef struct { int vTreeFlag; @@ -347,10 +365,16 @@ typedef struct typedef struct { SVzNL3DPoint opCenter; //λ - int neighbourNum; //Ŀ - double opAngle; //ⲿ120ȲIJǶȡYΪ׼Ƕȣ0) - double obstacleDist; //ܱϰľ -}SSG_motorStatorPosition; + double objR; +}SWD_motorStatorPosition; + +typedef struct +{ + int objID; + double grasperAngle; + double rotateAngle; //ץ0λʱΪѾΣ120ȡ + double graspR; +}SWD_statorOuterGrasper; typedef struct { @@ -447,3 +471,10 @@ typedef struct { std::vector> gray; // Ҷͼ std::vector> markers; // ͼ-1ʾˮ룬0ʾδǣ>0ʾ }SWD_waterShedImage; + +//ƶάMark +typedef struct +{ + int markID; + SVzNL3DPoint mark3D; +}SWD_charuco3DMark; diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 0bd8a8b..1dd952f 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -1668,7 +1668,7 @@ SSG_planeCalibPara sg_getPlaneCalibPara2_ROI( planePara.planeCalib[i] = initCalib[i]; planePara.planeHeight = -1.0; - int lineNum = scanLines.size(); + int lineNum = (int)scanLines.size(); //ȡ std::vector Points3ds; for (int line = 0; line < lineNum; line++) @@ -2076,6 +2076,7 @@ void pointClout2DProjection( SVzNLRangeD x_range, SVzNLRangeD y_range, double scale, + double cuttingGrndZ, int edgeSkip, double inerPolateDistTh, //ֵֵڴֵIJֵ cv::Mat& projectionData,//ͶӰݣʼΪһֵ1e+6 @@ -2095,6 +2096,8 @@ void pointClout2DProjection( SVzNL3DPosition* pt3D = &gridScanData[line][i]; if (pt3D->pt3D.z < 1e-4) continue; + if ((cuttingGrndZ > 0) && (pt3D->pt3D.z > cuttingGrndZ)) + continue; double x = pt3D->pt3D.x; double y = pt3D->pt3D.y; int px = (int)(x - x_range.min)/scale + edgeSkip; diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index afcf3bf..e13228f 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -6,6 +6,7 @@ #define SG_ERR_NOT_GRID_FORMAT -1003 #define SG_ERR_LABEL_INFO_ERROR -1004 #define SG_ERR_INVLD_SORTING_MODE -1005 +#define SG_ERR_INVLD_Q_SCALE -1006 //BQ_workpiece #define SX_ERR_INVLD_VTREE_NUM -2001 @@ -14,4 +15,8 @@ #define SX_ERR_INVLD_CLOSES_PT -2004 #define SX_ERR_ZERO_CONTOUR_PT -2005 #define SX_ERR_INVLID_RPEAK_NUM -2006 -#define SX_ERR_INVLID_RPEAK_PAIR -2007 \ No newline at end of file +#define SX_ERR_INVLID_RPEAK_PAIR -2007 + +//ץȡ +#define SX_ERR_INVLID_CUTTING_Z -2101 +#define SX_ERR_ZERO_OBJ -2102 \ No newline at end of file diff --git a/sourceCode/SG_featureGrow.cpp b/sourceCode/SG_featureGrow.cpp index 71f9b5c..131ec8a 100644 --- a/sourceCode/SG_featureGrow.cpp +++ b/sourceCode/SG_featureGrow.cpp @@ -113,6 +113,39 @@ bool _featureGrowing_noTypeMatch(SSG_basicFeature1D& a_feature, const int lineId return false; } +//segment featuretreesѰҺʵûкʵ㣬 false +bool _segFeatureGrowing(SWD_segFeature& a_feature, const int lineIdx, std::vector& trees, SSG_treeGrowParam growParam) +{ + for (int i = 0, i_max = (int)trees.size(); i < i_max; i++) + { + SWD_segFeatureTree& a_tree = trees[i]; + if (TREE_STATE_DEAD == a_tree.treeState) + continue; + // + SWD_segFeature last_node = a_tree.treeNodes.back(); + if (last_node.lineIdx == a_feature.lineIdx) //xΪlineIdxͬһɨϵIJ + continue; + + //ж + double y_diff_1 = abs(a_feature.startPt.y - last_node.startPt.y); + double y_diff_2 = abs(a_feature.endPt.y - last_node.endPt.y); + double z_diff_1 = abs(a_feature.startPt.z - last_node.startPt.z); + double z_diff_2 = abs(a_feature.endPt.z - last_node.endPt.z); + int line_diff = abs(a_feature.lineIdx - last_node.lineIdx); + double x_diff = (abs(a_feature.startPt.x - last_node.startPt.x) + abs(a_feature.endPt.x - last_node.endPt.x))/2; + if ((y_diff_1 < growParam.yDeviation_max) && (y_diff_2 < growParam.yDeviation_max) && + (z_diff_1 < growParam.zDeviation_max) && (z_diff_2 < growParam.zDeviation_max) && + ((line_diff < growParam.maxLineSkipNum) || (x_diff < growParam.maxSkipDistance))) + { + a_tree.eLineIdx = lineIdx; + a_tree.treeNodes.push_back(a_feature); + a_tree.tree_value += (a_feature.startPt.z + a_feature.endPt.z)/2; + return true; + } + } + return false; +} + int _getMatchedTree_angleCheck( SSG_basicFeature1D& a_feature, const int lineIdx, @@ -601,6 +634,55 @@ void wd_getFeatureGrowingTrees_noTypeMatch( } } +//segment +void wd_getSegFeatureGrowingTrees( + std::vector>& all_lineFeatures, + std::vector& feature_trees, + SSG_treeGrowParam growParam) +{ + for (int i = 0, i_max = (int)all_lineFeatures.size(); i < i_max; i++) + { + std::vector& a_lineFeatures = all_lineFeatures[i]; + for (int j = 0, j_max = (int)a_lineFeatures.size(); j < j_max; j++) + { + SWD_segFeature& a_feature = a_lineFeatures[j]; + bool isMatched = _segFeatureGrowing(a_feature, i, feature_trees, growParam); + if (false == isMatched) + { + //µ + SWD_segFeatureTree a_newTree; + a_newTree.treeNodes.push_back(a_feature); + a_newTree.treeState = TREE_STATE_ALIVE; + a_newTree.sLineIdx = a_feature.lineIdx; + a_newTree.eLineIdx = a_feature.lineIdx; + a_newTree.tree_value = (a_feature.startPt.z + a_feature.endPt.z) / 2; + feature_trees.push_back(a_newTree); + } + } + //ֹͣ,١ + //ڵΪ1Ƴ + int lineIdx = i; + int m_max = (int)feature_trees.size(); + for (int m = m_max - 1; m >= 0; m--) //ӺǰɾӰѭ + { + if (TREE_STATE_ALIVE == feature_trees[m].treeState) + { + int line_diff = abs(lineIdx - feature_trees[m].treeNodes.back().lineIdx); + if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) || + (i == i_max - 1)) + { + feature_trees[m].treeState = TREE_STATE_DEAD; + SWD_segFeature* first_node = &(feature_trees[m].treeNodes[0]); + SWD_segFeature* last_node = &(feature_trees[m].treeNodes[feature_trees[m].treeNodes.size() - 1]); + double len = (abs(first_node->startPt.x - last_node->startPt.x) + abs(first_node->endPt.x - last_node->endPt.x))/2; + if (len <= growParam.minLTypeTreeLen) + feature_trees.erase(feature_trees.begin() + m); + } + } + } + } +} + void sg_lineFeaturesGrowing( int lineIdx, bool isLastLine, diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index ad58a88..194ed11 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -1812,8 +1812,8 @@ void sg_getLineCornerFeature_BQ( int cornerIdx = backIndexing[i]; if (abs(corners[cornerIdx].forwardAngle) < cornerPara.jumpCornerTh_1) { - double diffZ = abs(a_pt.pt3D.z - refSteppingZ); - if (diffZ < 10.0) + //double diffZ = abs(a_pt.pt3D.z - refSteppingZ); + //if (diffZ < 10.0) { SSG_basicFeature1D a_feature; memset(&a_feature, 0, sizeof(SSG_basicFeature1D)); @@ -1829,8 +1829,8 @@ void sg_getLineCornerFeature_BQ( int cornerIdx = backIndexing[i]; if (abs(corners[cornerIdx].backwardAngle) < cornerPara.jumpCornerTh_1) { - double diffZ = abs(a_pt.pt3D.z - refSteppingZ); - if (diffZ < 10.0) + //double diffZ = abs(a_pt.pt3D.z - refSteppingZ); + //if (diffZ < 10.0) { SSG_basicFeature1D a_feature; memset(&a_feature, 0, sizeof(SSG_basicFeature1D)); @@ -2184,9 +2184,384 @@ void wd_getLineCornerFeature_PSM( return; } +bool compareByPtIdx(const SSG_basicFeature1D& a, const SSG_basicFeature1D& b) { + return a.jumpPos2D.y < b.jumpPos2D.y; +} +/// +/// ȡϵԲϰλԲĿȷ +/// seg˵㣺z +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +void wd_getRingArcFeature( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const SSG_cornerParam cornerPara, + double ringArcWidth, //ӵĻ + std::vector& line_ringArcs // +) +{ + int dataSize = (int)lineData.size(); + //ȥ + std::vector< SVzNL3DPosition> vldPts; + std::vector< int> vldPtSegIdx; + std::vector segs; + std::vector backIndexing; + backIndexing.resize(dataSize); + + int runIdx = 1; + SSG_RUN_EX a_run = { 0, -1, 0, false, false }; //startIdx, len, lastIdx + double pre_z = 0; + double pre_y = 0; + for (int i = 0; i < dataSize; i++) + { + lineData[i].nPointIdx = i; //± + if (lineData[i].pt3D.z > 1e-4) + { + if (a_run.len < 0) + { + a_run.start_zRising = true; + a_run.start = i; + a_run.len = 1; + a_run.end_zRising = true; + a_run.value = i; + } + else + { + double z_diff = abs(lineData[i].pt3D.z - pre_z); + if (z_diff < cornerPara.minEndingGap_z) + { + a_run.len = i - a_run.start + 1; + a_run.value = i; + } + else + { + bool next_zRising; + if (pre_z > lineData[i].pt3D.z) + { + a_run.end_zRising = true; + next_zRising = false; + } + else + { + a_run.end_zRising = false; + next_zRising = true; + } + a_run.value = runIdx; + runIdx++; + segs.push_back(a_run); + + a_run.start = i; + a_run.start_zRising = next_zRising; + a_run.len = 1; + a_run.value = i; + a_run.end_zRising = true; + } + } + int bIdx = (int)vldPts.size(); + backIndexing[i] = bIdx; + vldPts.push_back(lineData[i]); + vldPtSegIdx.push_back(runIdx); + + pre_z = lineData[i].pt3D.z; + } + } + if (a_run.len > 0) + segs.push_back(a_run); + +#if 0 + //ñ־ + for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) + { + int idx1 = segs[i].start; + int idx2 = segs[i].start + segs[i].len - 1; + lineData[idx1].nPointIdx |= 0x100000; + lineData[idx2].nPointIdx |= 0x200000; + } +#endif + + //ǰǺͺ + std::vector< SSG_pntDirAngle> corners; + corners.resize(vldPts.size()); + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + //ǰѰ + int pre_i = -1; + for (int j = i - 1; j >= 0; j--) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (dist >= cornerPara.scale) + { + pre_i = j; + break; + } + } + //Ѱ + int post_i = -1; + for (int j = i + 1; j < i_max; j++) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (dist >= cornerPara.scale) + { + post_i = j; + break; + } + } + //ս + if ((pre_i < 0) || (post_i < 0)) + { + corners[i].pntIdx = -1; + corners[i].forwardAngle = 0; + corners[i].backwardAngle = 0; + corners[i].corner = 0; + corners[i].forwardDiffZ = 0; + corners[i].backwardDiffZ = 0; + } + else + { + double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y); + double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y); + double forwardAngle = atan(tanValue_post) * 180.0 / PI; + double backwardAngle = atan(tanValue_pre) * 180.0 / PI; + corners[i].pntIdx = i; + corners[i].forwardAngle = forwardAngle; + corners[i].backwardAngle = backwardAngle; + corners[i].corner = -(forwardAngle - backwardAngle); //ͼϵϵy෴С- + corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z; + corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z; + } + } + + //սǼֵ + int _state = 0; + int pre_i = -1; + int sEdgePtIdx = -1; + int eEdgePtIdx = -1; + SSG_pntDirAngle* pre_data = NULL; + std::vector< SSG_pntDirAngle> cornerPeakP; + std::vector< SSG_pntDirAngle> cornerPeakM; + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + if (i == 275) + int kkk = 1; + SSG_pntDirAngle* curr_data = &corners[i]; + if (curr_data->pntIdx < 0) + { + if (i == i_max - 1) //һ + { + if (1 == _state) // + { + cornerPeakP.push_back(corners[eEdgePtIdx]); + } + else if (2 == _state) //½ + { + cornerPeakM.push_back(corners[eEdgePtIdx]); + } + } + continue; + } + + if (NULL == pre_data) + { + sEdgePtIdx = i; + eEdgePtIdx = i; + pre_data = curr_data; + pre_i = i; + continue; + } + + eEdgePtIdx = i; + double cornerDiff = curr_data->corner - pre_data->corner; + switch (_state) + { + case 0: //̬ + if (cornerDiff < 0) //½ + { + _state = 2; + } + else if (cornerDiff > 0) // + { + _state = 1; + } + break; + case 1: // + if (cornerDiff < 0) //½ + { + cornerPeakP.push_back(*pre_data); + _state = 2; + } + break; + case 2: //½ + if (cornerDiff > 0) // + { + cornerPeakM.push_back(*pre_data); + _state = 1; + } + break; + default: + _state = 0; + break; + } + pre_data = curr_data; + pre_i = i; + } + + std::vector lineFeatures; + //Сֵ㣨嶥 + //ֵȽϣڳ߶ȴѰҾֲֵ + double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2cornerScale + for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++) + { + if (cornerPeakP[i].corner < cornerPara.cornerTh) + continue; + + bool isPeak = true; + //ǰ + int cornerPtIdx = cornerPeakP[i].pntIdx; + for (int j = i - 1; j >= 0; j--) + { + int prePtIdx = cornerPeakP[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ; + if (dist > square_distTh) //߶ȴ + break; + + if (cornerPeakP[i].corner < cornerPeakP[j].corner) + { + isPeak = false; + break; + } + } + // + if (true == isPeak) + { + cornerPtIdx = cornerPeakP[i].pntIdx; + for (int j = i + 1; j < i_max; j++) + { + int postPtIdx = cornerPeakP[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); + if (dist > square_distTh) //߶ȴ + break; + + if (cornerPeakP[i].corner < cornerPeakP[j].corner) + { + isPeak = false; + break; + } + } + } + if (true == isPeak) + { + + + SSG_basicFeature1D a_feature; + if ((cornerPeakP[i].backwardAngle > cornerPara.jumpCornerTh_1) && (cornerPeakP[i].forwardAngle > -cornerPara.jumpCornerTh_2)) + a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; + else if ((cornerPeakP[i].forwardAngle < -cornerPara.jumpCornerTh_1) && (cornerPeakP[i].backwardAngle < cornerPara.jumpCornerTh_2)) + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + else + a_feature.featureType = LINE_FEATURE_CORNER_V; + + + a_feature.jumpPos = vldPts[cornerPtIdx].pt3D; + a_feature.jumpPos2D = { 0, vldPts[cornerPtIdx].nPointIdx }; + lineFeatures.push_back(a_feature); + } + } + + //ӿʼͽ߽ + //segǷҪϲϲ + for (int i = 0, i_max = (int)segs.size(); i < i_max - 1; i++) + { + SSG_RUN_EX* nxt_seg = &segs[i + 1]; + SSG_RUN_EX* curr_seg = &segs[i]; + + int idx_1 = curr_seg->start + curr_seg->len - 1; + int idx_2 = nxt_seg->start; + double y_diff = abs(lineData[idx_1].pt3D.y - lineData[idx_2].pt3D.y); + double z_diff = abs(lineData[idx_1].pt3D.z - lineData[idx_2].pt3D.z); + if ((y_diff < cornerPara.minEndingGap) && (z_diff < cornerPara.minEndingGap_z)) //ϲ + { + int idx_end = nxt_seg->start + nxt_seg->len - 1; + nxt_seg->start = curr_seg->start; + nxt_seg->len = idx_end - curr_seg->start + 1; + curr_seg->value = 0; + } + } + for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) + { + if (0 == segs[i].value) //ϲ + continue; + + int idx_1 = segs[i].start; + int idx_2 = segs[i].start + segs[i].len - 1; + + SSG_basicFeature1D an_edge; + memset(&an_edge, 0, sizeof(SSG_basicFeature1D)); + an_edge.featureType = LINE_FEATURE_LINE_ENDING_0; + an_edge.jumpPos = lineData[idx_1].pt3D; + an_edge.jumpPos2D = { 0, idx_1 }; + lineFeatures.push_back(an_edge); + //line_features.insert(line_features.begin(), an_edge); //ͷ + //β + an_edge.featureType = LINE_FEATURE_LINE_ENDING_1; + an_edge.jumpPos = lineData[idx_2].pt3D; + an_edge.jumpPos2D = { 0, idx_2 }; + lineFeatures.push_back(an_edge); + } + + //򣺰 + std::sort(lineFeatures.begin(), lineFeatures.end(), compareByPtIdx); + //ҷϿȵĶӻ + for (int i = 0, i_max = (int)lineFeatures.size(); i < i_max-1; i++) + { + if (lineFeatures[i].featureType < 0) + continue; + + bool pairing = false; + if (lineFeatures[i].featureType == LINE_FEATURE_LINE_ENDING_0) + { + if ((lineFeatures[i + 1].featureType == LINE_FEATURE_LINE_ENDING_1) || + (lineFeatures[i + 1].featureType == LINE_FEATURE_L_JUMP_H2L)) + pairing = true; + } + else if (lineFeatures[i].featureType == LINE_FEATURE_L_JUMP_H2L) + { + if (lineFeatures[i + 1].featureType == LINE_FEATURE_LINE_ENDING_1) + pairing = true; + } + if (true == pairing) + { + double deltaY = abs(lineFeatures[i].jumpPos.y - lineFeatures[i + 1].jumpPos.y); + double deltaZ = abs(lineFeatures[i].jumpPos.z - lineFeatures[i + 1].jumpPos.z); + double th1 = ringArcWidth * 0.75; + double th2 = ringArcWidth * 1.25; + if ((deltaY >= th1) && (deltaY < th2) && (deltaZ < ringArcWidth * 0.5)) + { + SWD_segFeature a_ringFeautre; + a_ringFeautre.lineIdx = lineIdx; + a_ringFeautre.startPt = lineFeatures[i].jumpPos; + a_ringFeautre.endPt = lineFeatures[i + 1].jumpPos; + a_ringFeautre.startPtIdx = lineFeatures[i].jumpPos2D.y; + a_ringFeautre.endPtIdx = lineFeatures[i+1].jumpPos2D.y; + line_ringArcs.push_back(a_ringFeautre); + lineFeatures[i].featureType = -1; + lineFeatures[i + 1].featureType = -1; + } + } + } + return; +} + bool compareByIdx(const SSG_pntDirAngle& a, const SSG_pntDirAngle& b) { return a.pntIdx < b.pntIdx; } + /// /// ȡϵJumping, ڴӺ /// seg˵㣺z diff --git a/sourceCode/WD_particleSizeMeasure.cpp b/sourceCode/WD_particleSizeMeasure.cpp index 53bd815..345662f 100644 --- a/sourceCode/WD_particleSizeMeasure.cpp +++ b/sourceCode/WD_particleSizeMeasure.cpp @@ -29,64 +29,6 @@ void wd_lineDataR( lineDataRT_vector(a_line, camPoseR, groundH); } -void wd_noiseFilter( - std::vector< std::vector>& scanLines, - const SWD_PSM_algoParam algoParam, - int* errCode) -{ - *errCode = 0; - int lineNum = (int)scanLines.size(); - int nPointCnt = (int)scanLines[0].size(); - bool vldGrid = true; - for (int i = 0; i < lineNum; i++) - { - if (nPointCnt != (int)scanLines[i].size()) - vldGrid = false; - wd_vectorDataRemoveOutlier_overwrite( - scanLines[i], - algoParam.filterParam); - } - if (false == vldGrid) - { - *errCode = SG_ERR_3D_DATA_INVLD; - return; - } - //ˮƽ - int hLineNum = nPointCnt; //Gridʽɨߵĵһ - //ˮƽɨ - std::vector> filterHLines; - filterHLines.resize(hLineNum); - for (int i = 0; i < hLineNum; i++) - filterHLines[i].resize(lineNum); - for (int line = 0; line < lineNum; line++) - { - for (int j = 0; j < hLineNum; j++) - { - filterHLines[j][line] = scanLines[line][j]; - filterHLines[j][line].pt3D.x = scanLines[line][j].pt3D.y; - filterHLines[j][line].pt3D.y = scanLines[line][j].pt3D.x; - } - } - for (int hLine = 0; hLine < hLineNum; hLine++) - { - //˲˳쳣 - std::vector filterData; - std::vector lineNoise; - sg_lineDataRemoveOutlier( - (SVzNL3DPosition*)filterHLines[hLine].data(), - (int)filterHLines[hLine].size(), - algoParam.filterParam, - filterData, - lineNoise); - for (int j = 0; j < lineNoise.size(); j++) - { - int lineIdx = lineNoise[j]; - scanLines[lineIdx][hLine].pt3D.z = 0; - } - } - return; -} - // void wd_particleSizeMeasure( std::vector< std::vector>& scanLines, @@ -104,7 +46,7 @@ void wd_particleSizeMeasure( } // //ֱ - wd_noiseFilter(scanLines, algoParam, errCode); + wd_noiseFilter(scanLines, algoParam.filterParam, errCode); if (*errCode != 0) return; @@ -201,7 +143,7 @@ void wd_particleSizeMeasure( double scale; if ((pntCloudPara.scale_x < 0) || (pntCloudPara.scale_y < 0)) { - *errCode = SG_ERR_3D_DATA_INVLD; + *errCode = SG_ERR_INVLD_Q_SCALE; return; } if(pntCloudPara.scale_x < pntCloudPara.scale_y) @@ -227,6 +169,7 @@ void wd_particleSizeMeasure( x_range, y_range, scale, + -1.0, edgeSkip, inerPolateDistTh, //ֵֵڴֵIJֵ distTranformMask,//ͶӰݣʼΪһֵ1e+6 diff --git a/sourceCode/motorStatorPosition.cpp b/sourceCode/motorStatorPosition.cpp index f7b2c38..24cc86a 100644 --- a/sourceCode/motorStatorPosition.cpp +++ b/sourceCode/motorStatorPosition.cpp @@ -26,6 +26,37 @@ typedef struct SSG_objSideInfo sideInfo[4]; //ӦLRTP }SSG_statorNeighbourInfo; +typedef struct +{ + int ID; + int linkNum; + int neighborID[6]; +}SSG_hexagonNeighbour; + +std::string m_strVersion = "1.0.0"; +const char* wd_particleSegVersion(void) +{ + return m_strVersion.c_str(); +} + +//һƽƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SSG_planeCalibPara wd_getBaseCalibPara( + std::vector< std::vector>& scanLines) +{ + return sg_getPlaneCalibPara2(scanLines); +} + +//̬ƽȥ +void wd_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH) +{ + lineDataRT_vector(a_line, camPoseR, groundH); +} + SSG_objSideInfo _getSideX( int cx, int cy, @@ -98,7 +129,7 @@ SSG_objSideInfo _getSideY( } void _getNeighbouringInfo( - const SG_motorStatorPositionParam positionParam, + const SWD_statorParam positionParam, cv::Mat& zSliceData, //Ѱұ߽磨߿ std::vector& Objects, //Ŀλ std::vector< SSG_statorNeighbourInfo>& neighbouringInfo //ڽӹϵ @@ -318,13 +349,9 @@ void _computeGripperPose( return; } -bool compareByNeighbourDist(const SSG_motorStatorPosition& a, const SSG_motorStatorPosition& b) +bool compareByNeighbourNumber(const SSG_hexagonNeighbour& a, const SSG_hexagonNeighbour& b) { - return a.obstacleDist > b.obstacleDist; -} -bool compareByNeighbourNum(const SSG_motorStatorPosition& a, const SSG_motorStatorPosition& b) -{ - return a.neighbourNum < b.neighbourNum; + return a.linkNum < b.linkNum; } typedef struct { @@ -332,20 +359,637 @@ typedef struct SVzNL3DPoint objCenter; }SG_fittingInfo; -void sg_motorStatorPosition( - SVzNL3DLaserLine* laser3DPoints, - int lineNum, - const SG_motorStatorPositionParam positionParam, - int* errCode, - std::vector& resultOpPositions +//Բɨ趨ӻ +//scanCenterΪɨģԲɨscanR1scanR2Բ +void circileScan( + cv::Mat& scanImg, //ɨ + int scanCenterX, int scanCenterY, //ɨ + double scanR1, double scanR2, //ɨ + std::vector& peakPts //ɨ ) { - /// ͳҰС֮ - SVzNL3DRangeD roi3D = sg_getScanDataROI( - laser3DPoints, - lineNum); - SVzNLRangeD z_range = roi3D.zRange; + //1ȼɨ + double scale = 1.0; + for (int i = 0; i < 360; i++) + { + double angle = (double)i * scale * PI / 180; + double sinTheta = sin(angle); + double cosTheta = cos(angle); + int x0 = scanCenterX + (int)(scanR1 * cosTheta); + int y0 = scanCenterY - (int)(scanR1 * sinTheta); + int x1 = scanCenterX + (int)(scanR2 * cosTheta); + int y1 = scanCenterY - (int)(scanR2 * sinTheta); + + //Bresenham㷨 + std::vector pts; + drawLine(x0, y0, x1, y1, pts); + //ֵ + float peakValue = 0; + SVzNL2DPoint peakPos = { 0,0 }; + for (int m = 0, m_max = (int)pts.size(); m < m_max; m++) + { + if (peakValue < scanImg.at(pts[m].y, pts[m].x)) + { + peakValue = scanImg.at(pts[m].y, pts[m].x); + peakPos = pts[m]; + } + } + peakPts.push_back(peakPos); + } +} +//ԶȦץȡצץּתλá +//צԳƣֱ30,150,270ȡץȡǶȣСתǶ +double _computeGrasperRotateAngle(double grasperAngle) +{ + double rotateAngle = 0; + if ((grasperAngle >= 330) || (grasperAngle < 90)) //Ѿһ֦λ30 + { + if (grasperAngle < 90) + rotateAngle = grasperAngle - 30; + else + rotateAngle = grasperAngle - 360 - 30; + } + else if ((grasperAngle >= 90) && (grasperAngle < 210))//Ѿڶ֦λ150 + rotateAngle = grasperAngle - 150; + else + rotateAngle = grasperAngle - 270; + return rotateAngle; +} + +//ǵļֵ0-180Χ +double _computeAbsAngleInterval(double angle_0, double angle_1) +{ + double diff = angle_0 < angle_1 ? (angle_1 - angle_0) : (angle_0 - angle_1); + if (diff > 180) + diff = 360 - diff; + return diff; +} + +//ǶĿĽǶȼСֵΪ +double _computeObjAngleInterval(double angle_0, std::vector& linkObjAngles) +{ + if (linkObjAngles.size() == 0) + return -1; + double minInterval = _computeAbsAngleInterval(angle_0, linkObjAngles[0]); + for (int i = 1, i_max = (int)linkObjAngles.size(); i < i_max; i++) + { + double angleInterval = _computeAbsAngleInterval(angle_0, linkObjAngles[i]); + if (minInterval > angleInterval) + minInterval = angleInterval; + } + return minInterval; +} + +//ĿץȡǶȡץΪצƣ120ȡ +//ɨĿĽǶȼ㡣λΪץȡλ +double _scanBestGrasperAngle(std::vector& linkObjAngles, double* angleDistance) +{ + *angleDistance = 0; + if (linkObjAngles.size() == 0) + return -1; + //0.1ȼɨ120ȷΧ + double maxAngleDist = -1; + double bestAngleIdx = -1; + //ֶλȷ1 + for (int i = 0; i < 120; i++) + { + double angle_0 = (double)i; + double angle_1 = angle_0 + 120; + if (angle_1 >= 360) + angle_1 = angle_1 - 360; + double angle_2 = angle_0 + 240; + if (angle_2 >= 360) + angle_2 = angle_2 - 360; + + double angleInterval_0 = _computeObjAngleInterval(angle_0, linkObjAngles); + double angleInterval_1 = _computeObjAngleInterval(angle_1, linkObjAngles); + double angleInterval_2 = _computeObjAngleInterval(angle_2, linkObjAngles); + double angleInterval = angleInterval_0 < angleInterval_1 ? angleInterval_0 : angleInterval_1; + angleInterval = angleInterval < angleInterval_2 ? angleInterval : angleInterval_2; + + if (maxAngleDist < 0) + { + maxAngleDist = angleInterval; + bestAngleIdx = (double)i; + } + else + { + if (maxAngleDist < angleInterval) + { + maxAngleDist = angleInterval; + bestAngleIdx = (double)i; + } + } + } + if (bestAngleIdx < 0) + return -1; + //λ ȷ0.1 + maxAngleDist = -1; + double bestAngle = bestAngleIdx; + for (int i = -10; i <= 10; i++) + { + double angle_0 = (double)i * 0.1 + bestAngleIdx; + double angle_1 = angle_0 + 120; + if (angle_1 >= 360) + angle_1 = angle_1 - 360; + double angle_2 = angle_0 + 240; + if (angle_2 >= 360) + angle_2 = angle_2 - 360; + + double angleInterval_0 = _computeObjAngleInterval(angle_0, linkObjAngles); + double angleInterval_1 = _computeObjAngleInterval(angle_1, linkObjAngles); + double angleInterval_2 = _computeObjAngleInterval(angle_2, linkObjAngles); + double angleInterval = angleInterval_0 < angleInterval_1 ? angleInterval_0 : angleInterval_1; + angleInterval = angleInterval < angleInterval_2 ? angleInterval : angleInterval_2; + + if (maxAngleDist < 0) + { + maxAngleDist = angleInterval; + bestAngle = angle_0; + } + else + { + if (maxAngleDist < angleInterval) + { + maxAngleDist = angleInterval; + bestAngle = angle_0; + } + } + } + *angleDistance = maxAngleDist; + if (bestAngle < 0) + bestAngle = bestAngle + 360; + + return bestAngle; +} + +bool computeOuterGraspPoint( + int objID, + double outerR, + std::vector< SSG_peakRgnInfo>& objects, + std::vector< SSG_hexagonNeighbour>& objLinkings, + SWD_statorOuterGrasper& graspPoint, + double* angleDistance +) +{ + //R = 1.155outR + SSG_hexagonNeighbour& a_link = objLinkings[objID]; + std::vector linkIDs; + for (int i = 0; i < 6; i++) + { + if (a_link.neighborID[i] >= 0) + { + linkIDs.push_back(a_link.neighborID[i]); + } + } + if (linkIDs.size() != a_link.linkNum) + return false; + + //ץȡλ + if (a_link.linkNum == 0) //Ĭץȡλ + { + graspPoint.objID = objID; + graspPoint.grasperAngle = 0; + graspPoint.rotateAngle = 0; + graspPoint.graspR = 1.2 * outerR; + *angleDistance = 120; + return true; + } + else if (a_link.linkNum == 1) //Ŀ߷ƫ60 + { + int linkID = linkIDs[0]; + SSG_peakRgnInfo& obj = objects[objID]; + SSG_peakRgnInfo& nObj = objects[linkID]; + double dy = nObj.pos2D.y - obj.pos2D.y; + double dx = nObj.pos2D.x - obj.pos2D.x; + double dist = sqrt(pow(dx, 2) + pow(dy, 2)); + double angle = atan2(dy, dx) * 180.0 / PI + 180; //0-360 + double grasperAngle = angle + 60; + if (grasperAngle >= 360) + grasperAngle = grasperAngle - 360; + double rotateAngle = _computeGrasperRotateAngle(grasperAngle); + graspPoint.grasperAngle = grasperAngle; + graspPoint.rotateAngle = rotateAngle; + graspPoint.graspR = 1.2 * outerR; + graspPoint.objID = objID; + *angleDistance = 60; + return true; + } + else if ( (a_link.linkNum == 2) || (a_link.linkNum == 3)) //Ŀ3Ŀ + { + std::vector linkObjAngles; + for (int i = 0, i_max = (int)linkIDs.size(); i < i_max; i++) + { + int linkID = linkIDs[i]; + SSG_peakRgnInfo& obj = objects[objID]; + SSG_peakRgnInfo& nObj = objects[linkID]; + double dy = nObj.pos2D.y - obj.pos2D.y; + double dx = nObj.pos2D.x - obj.pos2D.x; + double dist = sqrt(pow(dx, 2) + pow(dy, 2)); + double angle = atan2(dy, dx) * 180.0 / PI + 180; //0-360 + linkObjAngles.push_back(angle); + } + double grasperAngle = _scanBestGrasperAngle(linkObjAngles, angleDistance); + double rotateAngle = _computeGrasperRotateAngle(grasperAngle); + graspPoint.grasperAngle = grasperAngle; + graspPoint.rotateAngle = rotateAngle; + graspPoint.graspR = 1.155 * outerR; + graspPoint.objID = objID; + return true; + } + else + return false; +} + +//Ӷλ +//㷨߼ҵӵĸ߶->ýȡZȥ-> +// ͶӰעʱ߿ҲͬʱͶӰ->任->ȡĿ-> +// ںͱ߿ѰץȡĿץȡ +void wd_motorStatorPosition( + std::vector< std::vector>& scanLines, + const SWD_statorParam statorParam, + const SSG_planeCalibPara groundCalibPara, + const SWD_statorPositonParam algoParam, + SWD_nextOpParam* refPos, //һθIJολãͬʱһεIJολ + int* errCode, + std::vector& resultObjPositions, + SWD_statorOuterGrasper& resultGraspPos +) +{ + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + { + *errCode = SG_ERR_3D_DATA_NULL; + return; + } + // + wd_noiseFilter(scanLines, algoParam.filterParam, errCode); + if (*errCode != 0) + return; + + ///ʼݴ + double statorRingWidth = (statorParam.statorOuterD - statorParam.statorInnerD) / 2; + if (refPos->cuttingZ < 0) //ȡϰ벿ĻȡĻж϶Ӹ߶ + { + //ֱݴ + std::vector> all_vLineArcs; + for (int i = 0; i < lineNum; i++) + { + if (i == 202) + int k = 1; + //ȡӻ + std::vector line_ringArcs; + wd_getRingArcFeature( + scanLines[i], + i, + algoParam.cornerParam, + statorRingWidth, //ӵĻ + line_ringArcs // + ); + all_vLineArcs.push_back(line_ringArcs); //Ҳ룬֤ܰк + } + //˳ٵ + //ֱɨ跽 + std::vector v_seg_trees; + // + wd_getSegFeatureGrowingTrees( + all_vLineArcs, + v_seg_trees, + algoParam.growParam); + //ȡ߶ + if (v_seg_trees.size() > 0) + { + int treeSize = (int)v_seg_trees.size(); + double z_sum = 0; + for (int m = 0; m < treeSize; m++) + { + z_sum += v_seg_trees[m].tree_value; + } + refPos->cuttingZ = z_sum / (double)treeSize + statorParam.statorHeight / 2; + } + } + if (refPos->cuttingZ < 0) + { + *errCode = SX_ERR_INVLID_CUTTING_Z; + return; + } + //ʹ refPos->cuttingZ ָ + //ͶӰ2D + SWD_pointCloudPara pntCloudPara = wd_getPointCloudPara(scanLines);// ͳҰС + double scale; + if ((pntCloudPara.scale_x < 0) || (pntCloudPara.scale_y < 0)) + { + *errCode = SG_ERR_INVLD_Q_SCALE; + return; + } + if (pntCloudPara.scale_x < pntCloudPara.scale_y) + scale = pntCloudPara.scale_x; + else + scale = pntCloudPara.scale_y; + + double inerPolateDistTh = scale * 10; //ֵޣ ڴֵֵ + int edgeSkip = 2; + int maskX = (int)(pntCloudPara.xRange.max - pntCloudPara.xRange.min) / scale + 1; + int maskY = (int)(pntCloudPara.yRange.max - pntCloudPara.yRange.min) / scale + 1; + if ((maskX < 16) || (maskY < 16)) + return; + maskY = maskY + edgeSkip * 2; + maskX = maskX + edgeSkip * 2; + cv::Mat projectionImg(maskY, maskX, CV_32FC1, 0.0f); //任MaskʼΪһֵ1e+6 + cv::Mat projectionIndexing(maskY, maskX, CV_32SC2, cv::Vec2i(0, 0)); // + pointClout2DProjection( + scanLines, + pntCloudPara.xRange, + pntCloudPara.yRange, + scale, + refPos->cuttingZ, + edgeSkip, + inerPolateDistTh, //ֵֵڴֵIJֵ + projectionImg,//ͶӰݣʼΪһֵ1e+6 + projectionIndexing //ڻ3D + ); + //任 + cv::Mat distTransform; + sg_distanceTrans(projectionImg, distTransform, 0); +#if OUTPUT_DEBUG //debug + cv::Mat maskImage; + cv::normalize(distTranformMask, maskImage, 0, 255, cv::NORM_MINMAX, CV_8U); + cv::imwrite("distTransformMask.png", maskImage); + cv::Mat dtImage; + cv::normalize(distTransform, dtImage, 0, 255, cv::NORM_MINMAX, CV_8U); + cv::Mat dtImage_color; + cv::cvtColor(dtImage, dtImage_color, cv::COLOR_GRAY2BGR); + cv::imwrite("distTransform.png", dtImage_color); +#endif + //ȡĿ꣬ĵ + //Ծ任ֵнȡʹĿ겻ߡ + double distanceTh = (statorParam.statorOuterD - statorParam.statorInnerD) / 8; + cv::Mat bwImg = cv::Mat::zeros(maskY, maskX, CV_8UC1);//rows, cols + for (int y = 0; y < maskY; y++) + { + for (int x = 0; x < maskX; x++) + { + if(distTransform.at(y,x) > distanceTh) + bwImg.at(y, x) = 1; + } + } + //ͨע + cv::Mat labImg; + std::vector labelRgns; + SG_TwoPassLabel(bwImg, labImg, labelRgns, 8); + //ݴСȡĿ + double objPixSize = statorParam.statorOuterD / scale; + int objSizeTh1 = (int)(objPixSize * 0.75); + int objSizeTh2 = (int)(objPixSize * 1.1); + std::vector objectRgns; + for (int i = 0, i_max = (int)labelRgns.size(); i < i_max; i++) + { + int width = labelRgns[i].roi.right - labelRgns[i].roi.left; + int height = labelRgns[i].roi.bottom - labelRgns[i].roi.top; + if ((width > objSizeTh1) && (width < objSizeTh2) && + (height > objSizeTh1) && (height < objSizeTh2)) + objectRgns.push_back(labelRgns[i]); + } + // ĵԲɨ裬ȷȷĵ + double scanR1 = (statorParam.statorInnerD / 2) * 0.75; + double scanR2 = (statorParam.statorOuterD / 2) * 1.25; + std::vector< SSG_peakRgnInfo> objects; + SSG_peakRgnInfo a_nullObj; + memset(&a_nullObj, 0, sizeof(SSG_peakRgnInfo)); + objects.push_back(a_nullObj); //ID1ʼID±ͬ + int objNumber = 1; + for (int i = 0, i_max = (int)objectRgns.size(); i < i_max; i++) + { + SSG_Region& a_objRgn = objectRgns[i]; + //y + int x = (a_objRgn.roi.left + a_objRgn.roi.right) / 2; + int y = (a_objRgn.roi.top + a_objRgn.roi.bottom) / 2; + //Բɨ趨ӻ + std::vector peakPts; + circileScan( + distTransform, //ɨ + x, y, //ɨ + scanR1, scanR2, //ɨ + peakPts //ɨ + ); + //ģϣʹüģ + double cx = 0, cy = 0, cz = 0; + int ptSize = (int)peakPts.size(); + if (ptSize == 0) + continue; + + for (int m = 0; m < ptSize; m++) + { + cx += (double)peakPts[m].x; + cy += (double)peakPts[m].y; + cz += projectionImg.at(peakPts[m].y, peakPts[m].x); + } + cx = cx / (double)ptSize; + cy = cy / (double)ptSize; + cz = cz / (double)ptSize; + // + SSG_peakRgnInfo a_obj; + a_obj.pos2D = {(int)cy,(int)cx}; + a_obj.centerPos.x = cx * scale + pntCloudPara.xRange.min; + a_obj.centerPos.y = cy * scale + pntCloudPara.yRange.min; + a_obj.centerPos.z = cz; + a_obj.pkRgnIdx = objNumber; + objNumber++; + objects.push_back(a_obj); + } + if (objNumber < 2) + { + *errCode = SX_ERR_ZERO_OBJ; + return; + } + //⵽Ŀλ + for (int i = 1; i < objNumber; i++) + { + SWD_motorStatorPosition a_obj; + a_obj.objR = statorParam.statorOuterD / 2; + a_obj.opCenter.x = objects[i].centerPos.x; + a_obj.opCenter.y = objects[i].centerPos.y; + a_obj.opCenter.z = objects[i].centerPos.z; + resultObjPositions.push_back(a_obj); + } + + //ץȡ + //ڽӹϵ,ÿĿ6ڽĿ + std::vector< SSG_hexagonNeighbour> objLinkings; + objLinkings.resize(objNumber);//һʹáĿID1ʼID±һ + for (int i = 0; i < objNumber; i++) + { + objLinkings[i].ID = i; + objLinkings[i].linkNum = 0; + for (int j = 0; j < 6; j++) + objLinkings[i].neighborID[j] = -1; + } + objLinkings[0].linkNum = INT_MAX; + double linkPixDistTh = (statorParam.statorOuterD * 1.5) / scale; + for (int i = 1; i < objNumber; i++) + { + SSG_peakRgnInfo& obj_0 = objects[i]; + for (int j = i + 1; j < objNumber; j++) + { + SSG_peakRgnInfo& obj_1 = objects[j]; + double dy = obj_1.pos2D.y - obj_0.pos2D.y; + double dx = obj_1.pos2D.x - obj_0.pos2D.x; + double dist = sqrt(pow(dx, 2) + pow(dy, 2)); + if (dist < linkPixDistTh) + { + double angle = atan2(dy, dx) * 180.0 / PI + 180; //0-360 + //60Ϊһ0-59Ϊ0 60-119Ϊ1 + int angleIndx = (int)(angle+ 30.0)/ 60; //˴30ȣֹĿ괦0ٽ硣ݶˮƽʹֱ + int angleIndx_1 = (angleIndx + 3) % 6; + objLinkings[i].neighborID[angleIndx] = j; + objLinkings[j].neighborID[angleIndx_1] = i; + } + } + } + + //ȷץȡ + int objID = -1; + bool validGrasper = false; + if (refPos->refPos.z > 1e-4) //һθλýץȡ + { + double minDist = -1; + int refx = (refPos->refPos.x - pntCloudPara.xRange.min) / scale; + int refy = (refPos->refPos.y - pntCloudPara.yRange.min) / scale; + for (int i = 1; i < objNumber; i++) //ĿΪץȡĿ + { + double dist = sqrt(pow(objects[i].pos2D.x-refx, 2) + pow(objects[i].pos2D.y - refy, 2)); + if (minDist < 0) + { + minDist = dist; + objID = i; + } + else + { + if (minDist > dist) + { + minDist = dist; + objID = i; + } + } + } + //ץȡ + SWD_statorOuterGrasper outerGrasper; + double angleDistance; + bool validGrasper = computeOuterGraspPoint( + objID, + statorParam.statorOuterD / 2, + objects, + objLinkings, + outerGrasper, + &angleDistance + ); + resultGraspPos = outerGrasper; + + } + else + { + //ͳĿ + int minLinkNum = -1; + for (int i = 1; i < objNumber; i++) + { + objLinkings[i].linkNum = 0; + for (int j = 0; j < 6; j++) + { + if (objLinkings[i].neighborID[j] >= 0) + objLinkings[i].linkNum++; + } + if (minLinkNum < 0) + minLinkNum = objLinkings[i].linkNum; + else + { + if (minLinkNum > objLinkings[i].linkNum) + minLinkNum = objLinkings[i].linkNum; + } + } + + std::vector objCandidates; + for (int i = 1; i < objNumber; i++) + { + if (minLinkNum = objLinkings[i].linkNum) + objCandidates.push_back(i); + } + + //std::sort(objLinkings.begin(), objLinkings.end(), compareByNeighbourNumber); + double maxAngleInterval = -1; + SWD_statorOuterGrasper bestGrasper; + for (int i = 0, i_max=(int)objCandidates.size(); i < i_max; i++) + { + int id = objCandidates[i]; + //ץȡ + SWD_statorOuterGrasper outerGrasper; + double angleDistance; + bool validGrasper = computeOuterGraspPoint( + id, + statorParam.statorOuterD / 2, + objects, + objLinkings, + outerGrasper, + &angleDistance + ); + if (true == validGrasper) + { + if (maxAngleInterval < 0) + { + maxAngleInterval = angleDistance; + bestGrasper = outerGrasper; + } + else + { + if (maxAngleInterval < angleDistance) + { + maxAngleInterval = angleDistance; + bestGrasper = outerGrasper; + } + } + } + } + resultGraspPos = bestGrasper; + } + //һοץȡ + int candiID = -1; + int candiLinkNum = -1; + for (int i = 0; i < 6; i++) + { + int neighborID = objLinkings[objID].neighborID[i]; + if (neighborID >= 0) + { + int linkNum = objLinkings[neighborID].linkNum; + if (candiLinkNum < 0) + { + candiID = neighborID; + candiLinkNum = linkNum; + } + else + { + if (candiLinkNum > linkNum) + { + candiID = neighborID; + candiLinkNum = linkNum; + } + } + } + } + if (candiID >= 0) + { + refPos->refPos.x = objects[candiID].centerPos.x; + refPos->refPos.y = objects[candiID].centerPos.y; + refPos->refPos.z = objects[candiID].centerPos.z; + } + else + { + refPos->refPos.x = 0; + refPos->refPos.y = 0; + refPos->refPos.z = -1; + } + return; + +#if 0 //ZͳơΪ1mm int zHistSize = (int)(z_range.max - z_range.min) + 1; std::vector zHist; @@ -577,4 +1221,5 @@ void sg_motorStatorPosition( //ϲ grabPoses.insert(grabPoses.end(), grabPoses_2.begin(), grabPoses_2.end()); return; +#endif } \ No newline at end of file diff --git a/sourceCode/motorStatorPosition_Export.h b/sourceCode/motorStatorPosition_Export.h index 6017fac..5d9b3b4 100644 --- a/sourceCode/motorStatorPosition_Export.h +++ b/sourceCode/motorStatorPosition_Export.h @@ -13,14 +13,46 @@ typedef struct { double statorOuterD; //ֱ double statorInnerD; //ڿֱ + double statorHeight; double gripperR;//צ뾶 -}SG_motorStatorPositionParam; +}SWD_statorParam; + +typedef struct +{ + SSG_outlierFilterParam filterParam; + SSG_cornerParam cornerParam; + SSG_treeGrowParam growParam; +}SWD_statorPositonParam; + +typedef struct +{ + SVzNL3DPoint refPos; + double cuttingZ; //zضֵڷָϰ벿͵ +}SWD_nextOpParam; + +//汾 +SG_APISHARED_EXPORT const char* wd_particleSegVersion(void); + +//һƽƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SG_APISHARED_EXPORT SSG_planeCalibPara wd_getBaseCalibPara( + std::vector< std::vector>& scanLines); + +//̬ƽȥ +SG_APISHARED_EXPORT void wd_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); ///gridʽԽˮƽʹֱĴ -SG_APISHARED_EXPORT void sg_motorStatorPosition( - SVzNL3DLaserLine* laser3DPoints, - int lineNum, - const SG_motorStatorPositionParam positionParam, +SG_APISHARED_EXPORT void wd_motorStatorPosition( + std::vector< std::vector>& scanLines, + const SWD_statorParam positionParam, + const SSG_planeCalibPara groundCalibPara, + const SWD_statorPositonParam algoParam, + SWD_nextOpParam* refPos, //һθIJολãͬʱһεIJολ int* errCode, - std::vector& resultOpPositions + std::vector& resultObjPositions, + SWD_statorOuterGrasper& resultGraspPos ); \ No newline at end of file From 19b51a73a7f69f892049985dacce8d76724f405c Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Fri, 5 Dec 2025 20:13:55 +0800 Subject: [PATCH 10/12] =?UTF-8?q?-=20=E6=B7=BB=E5=8A=A0=E4=BA=86=20?= =?UTF-8?q?=E2=80=9C=E5=8F=8C=E7=9B=AEMark=E7=9B=B8=E6=9C=BA=E2=80=9D?= =?UTF-8?q?=E9=A1=B9=E7=9B=AE=E5=8F=8A=E7=9B=B8=E5=BA=94=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../BinocularMarkCam_test.cpp | 104 ++++++++ .../BinocularMarkCam_test.vcxproj | 158 ++++++++++++ binocularMarkCam/binocularMarkCam.vcxproj | 176 +++++++++++++ sourceCode/aruco/aruco.cpp | 143 +++++++++++ sourceCode/aruco/aruco_calib.cpp | 99 ++++++++ sourceCode/aruco/aruco_calib.hpp | 180 +++++++++++++ sourceCode/aruco/charuco.cpp | 62 +++++ sourceCode/aruco/charuco.hpp | 110 ++++++++ sourceCode/aruco/precomp.hpp | 11 + sourceCode/binocularMarkCam.cpp | 238 ++++++++++++++++++ sourceCode/binocularMarkCam_Export.h | 47 ++++ 11 files changed, 1328 insertions(+) create mode 100644 BinocularMarkCam_test/BinocularMarkCam_test.cpp create mode 100644 BinocularMarkCam_test/BinocularMarkCam_test.vcxproj create mode 100644 binocularMarkCam/binocularMarkCam.vcxproj create mode 100644 sourceCode/aruco/aruco.cpp create mode 100644 sourceCode/aruco/aruco_calib.cpp create mode 100644 sourceCode/aruco/aruco_calib.hpp create mode 100644 sourceCode/aruco/charuco.cpp create mode 100644 sourceCode/aruco/charuco.hpp create mode 100644 sourceCode/aruco/precomp.hpp create mode 100644 sourceCode/binocularMarkCam.cpp create mode 100644 sourceCode/binocularMarkCam_Export.h diff --git a/BinocularMarkCam_test/BinocularMarkCam_test.cpp b/BinocularMarkCam_test/BinocularMarkCam_test.cpp new file mode 100644 index 0000000..1b9f5fd --- /dev/null +++ b/BinocularMarkCam_test/BinocularMarkCam_test.cpp @@ -0,0 +1,104 @@ +// BinocularMarkCam_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// +#include +#include +#include +#include +#include +#include +#include "binocularMarkCam_Export.h" +#include +#include +#include +#include + +#define TEST_GROUP 2 +int main(int argc, char** argv) +{ + const char* dataPath[TEST_GROUP] = { + "F:/标定采图/charucoMark图/MarkTest/", //0 + "F:/标定采图/charucoMark图/Mark_13度/", + }; + + SVzNLRange fileIdx[TEST_GROUP] = { + {1,4}, {1,5}, + }; + + const char* ver = wd_charuco3DMarkVersion(); + printf("ver:%s\n", ver); + + for (int grp = 1; grp < 2; grp++) + { + SWD_BQ_CharucoMarkInfo markInfo; + markInfo.patternSize = cv::Size(3, 3); + markInfo.dictType = 1; //DICT_6x6 + markInfo.checkerSize = 60.0; + markInfo.markerSize = 45.0; + + SWD_BQ_MarkBoardInfo boardInfo; + boardInfo.totalBoardNum = 10; + boardInfo.boardIdInterval = 8; + boardInfo.boardChaucoIDNum = 4; + + // 1. 打开 XML 文件（READ 模式） + char calibFile[250]; + sprintf_s(calibFile, sizeof(calibFile), "%sStereoCamera.xml", dataPath[grp]); + cv::FileStorage fs("F:/标定采图/charucoMark图/Mark_13度/StereoCamera.xml", cv::FileStorage::READ); + + // 检查文件是否成功打开 + if (!fs.isOpened()) { + printf( "错误：无法打开 XML 文件！请检查路径是否正确。\n" ); + return -1; + } + cv::FileNode LeftCamera = fs["LeftCamera"]; + cv::Mat cameraMatrixL, distCoeffsL; + LeftCamera["CameraMatrix"] >> cameraMatrixL; + LeftCamera["DistCoeffs"] >> distCoeffsL; + cv::FileNode RightCamera = fs["RightCamera"]; + cv::Mat cameraMatrixR, distCoeffsR; + RightCamera["CameraMatrix"] >> cameraMatrixR; + RightCamera["DistCoeffs"] >> distCoeffsR; + cv::FileNode Rectification = fs["Rectification"]; + cv::Mat R1, R2,P1,P2,Q; + Rectification["R1"] >> R1; + Rectification["R2"] >> R2; + Rectification["P1"] >> P1; + Rectification["P2"] >> P2; + Rectification["Q"] >> Q; + + int fidx1 = fileIdx[grp].nMin; + int fidx2 = fileIdx[grp].nMax; + for (int index = fidx1; index < fidx2; index ++) + { + char filename[256]; + sprintf_s(filename, "%s/left%d.bmp", dataPath[grp], index); + cv::Mat leftimg = cv::imread(filename, cv::IMREAD_GRAYSCALE); + + sprintf_s(filename, "%s/right%d.bmp", dataPath[grp], index); + cv::Mat rightimg = cv::imread(filename, cv::IMREAD_GRAYSCALE); + + if (leftimg.empty() || rightimg.empty()) + break; + + double disparityOffset = 0; + std::vector marks; + wd_BQ_getCharuco3DMark( + leftimg, rightimg, + cameraMatrixL, distCoeffsL, cameraMatrixR, distCoeffsR, R1, R2, P1, P2, Q, + markInfo, boardInfo, + disparityOffset, + marks); + } + } +} + +// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 +// 调试程序: F5 或调试 >“开始调试”菜单 + +// 入门使用技巧: +// 1. 使用解决方案资源管理器窗口添加/管理文件 +// 2. 使用团队资源管理器窗口连接到源代码管理 +// 3. 使用输出窗口查看生成输出和其他消息 +// 4. 使用错误列表窗口查看错误 +// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目 +// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件 diff --git a/BinocularMarkCam_test/BinocularMarkCam_test.vcxproj b/BinocularMarkCam_test/BinocularMarkCam_test.vcxproj new file mode 100644 index 0000000..992e5cd --- /dev/null +++ b/BinocularMarkCam_test/BinocularMarkCam_test.vcxproj @@ -0,0 +1,158 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {fb3bcf92-df8b-4cec-aa39-4ecd31f82718} + BinocularMarkCamtest + 10.0 + binocularMarkCam_test + + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + $(SolutionDir)build\$(Platform)\$(Configuration)\ + + + 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\opencv\build\include; + + + Console + true + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world480d.lib;binocularMarkCam.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv\build\include; + + + Console + true + true + true + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world480.lib;binocularMarkCam.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/binocularMarkCam/binocularMarkCam.vcxproj b/binocularMarkCam/binocularMarkCam.vcxproj new file mode 100644 index 0000000..4cf1ae5 --- /dev/null +++ b/binocularMarkCam/binocularMarkCam.vcxproj @@ -0,0 +1,176 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + + + + 16.0 + Win32Proj + {ab25a4fe-6457-4cb4-a190-ebef603335c7} + binocularMarkCam + 10.0 + binocularMarkCam + + + + 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\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;BINOCULARMARKCAM_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;BINOCULARMARKCAM_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + true + true + false + + + + + Level3 + true + _DEBUG;BINOCULARMARKCAM_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + false + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug; + opencv_world480d.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + NDEBUG;BINOCULARMARKCAM_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + true + true + false + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release; + opencv_world480.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + + + + + + \ No newline at end of file diff --git a/sourceCode/aruco/aruco.cpp b/sourceCode/aruco/aruco.cpp new file mode 100644 index 0000000..c734a75 --- /dev/null +++ b/sourceCode/aruco/aruco.cpp @@ -0,0 +1,143 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +#include "precomp.hpp" +#include "../aruco.hpp" +#include +#include + +namespace cv { +namespace aruco { + +using namespace std; + +void detectMarkers(InputArray _image, const Ptr &_dictionary, OutputArrayOfArrays _corners, + OutputArray _ids, const Ptr &_params, + OutputArrayOfArrays _rejectedImgPoints) { + ArucoDetector detector(*_dictionary, *_params); + detector.detectMarkers(_image, _corners, _ids, _rejectedImgPoints); +} + +void refineDetectedMarkers(InputArray _image, const Ptr &_board, + InputOutputArrayOfArrays _detectedCorners, InputOutputArray _detectedIds, + InputOutputArrayOfArrays _rejectedCorners, InputArray _cameraMatrix, + InputArray _distCoeffs, float minRepDistance, float errorCorrectionRate, + bool checkAllOrders, OutputArray _recoveredIdxs, + const Ptr &_params) { + RefineParameters refineParams(minRepDistance, errorCorrectionRate, checkAllOrders); + ArucoDetector detector(_board->getDictionary(), *_params, refineParams); + detector.refineDetectedMarkers(_image, *_board, _detectedCorners, _detectedIds, _rejectedCorners, _cameraMatrix, + _distCoeffs, _recoveredIdxs); +} + +void drawPlanarBoard(const Ptr &board, Size outSize, const _OutputArray &img, int marginSize, int borderBits) { + board->generateImage(outSize, img, marginSize, borderBits); +} + +void getBoardObjectAndImagePoints(const Ptr &board, InputArrayOfArrays detectedCorners, InputArray detectedIds, + OutputArray objPoints, OutputArray imgPoints) { + board->matchImagePoints(detectedCorners, detectedIds, objPoints, imgPoints); +} + +int estimatePoseBoard(InputArrayOfArrays corners, InputArray ids, const Ptr &board, + InputArray cameraMatrix, InputArray distCoeffs, InputOutputArray rvec, + InputOutputArray tvec, bool useExtrinsicGuess) { + CV_Assert(corners.total() == ids.total()); + + // get object and image points for the solvePnP function + Mat objPoints, imgPoints; + board->matchImagePoints(corners, ids, objPoints, imgPoints); + + CV_Assert(imgPoints.total() == objPoints.total()); + + if(objPoints.total() == 0) // 0 of the detected markers in board + return 0; + + solvePnP(objPoints, imgPoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess); + + // divide by four since all the four corners are concatenated in the array for each marker + return (int)objPoints.total() / 4; +} + +bool estimatePoseCharucoBoard(InputArray charucoCorners, InputArray charucoIds, + const Ptr &board, InputArray cameraMatrix, + InputArray distCoeffs, InputOutputArray rvec, + InputOutputArray tvec, bool useExtrinsicGuess) { + CV_Assert((charucoCorners.getMat().total() == charucoIds.getMat().total())); + if(charucoIds.getMat().total() < 4) return false; + + // get object and image points for the solvePnP function + Mat objPoints, imgPoints; + board->matchImagePoints(charucoCorners, charucoIds, objPoints, imgPoints); + try { + solvePnP(objPoints, imgPoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess); + } + catch (const cv::Exception& e) { + CV_LOG_WARNING(NULL, "estimatePoseCharucoBoard: " << std::endl << e.what()); + return false; + } + + return objPoints.total() > 0ull; +} + +bool testCharucoCornersCollinear(const Ptr &board, InputArray charucoIds) { + return board->checkCharucoCornersCollinear(charucoIds); +} + +/** + * @brief Return object points for the system centered in a middle (by default) or in a top left corner of single + * marker, given the marker length + */ +static Mat _getSingleMarkerObjectPoints(float markerLength, const EstimateParameters& estimateParameters) { + CV_Assert(markerLength > 0); + Mat objPoints(4, 1, CV_32FC3); + // set coordinate system in the top-left corner of the marker, with Z pointing out + if (estimateParameters.pattern == ARUCO_CW_TOP_LEFT_CORNER) { + objPoints.ptr(0)[0] = Vec3f(0.f, 0.f, 0); + objPoints.ptr(0)[1] = Vec3f(markerLength, 0.f, 0); + objPoints.ptr(0)[2] = Vec3f(markerLength, markerLength, 0); + objPoints.ptr(0)[3] = Vec3f(0.f, markerLength, 0); + } + else if (estimateParameters.pattern == ARUCO_CCW_CENTER) { + objPoints.ptr(0)[0] = Vec3f(-markerLength/2.f, markerLength/2.f, 0); + objPoints.ptr(0)[1] = Vec3f(markerLength/2.f, markerLength/2.f, 0); + objPoints.ptr(0)[2] = Vec3f(markerLength/2.f, -markerLength/2.f, 0); + objPoints.ptr(0)[3] = Vec3f(-markerLength/2.f, -markerLength/2.f, 0); + } + else + CV_Error(Error::StsBadArg, "Unknown estimateParameters pattern"); + return objPoints; +} + +void estimatePoseSingleMarkers(InputArrayOfArrays _corners, float markerLength, + InputArray _cameraMatrix, InputArray _distCoeffs, + OutputArray _rvecs, OutputArray _tvecs, OutputArray _objPoints, + const Ptr& estimateParameters) { + CV_Assert(markerLength > 0); + + Mat markerObjPoints = _getSingleMarkerObjectPoints(markerLength, *estimateParameters); + int nMarkers = (int)_corners.total(); + _rvecs.create(nMarkers, 1, CV_64FC3); + _tvecs.create(nMarkers, 1, CV_64FC3); + + Mat rvecs = _rvecs.getMat(), tvecs = _tvecs.getMat(); + + //// for each marker, calculate its pose + parallel_for_(Range(0, nMarkers), [&](const Range& range) { + const int begin = range.start; + const int end = range.end; + + for (int i = begin; i < end; i++) { + solvePnP(markerObjPoints, _corners.getMat(i), _cameraMatrix, _distCoeffs, rvecs.at(i), + tvecs.at(i), estimateParameters->useExtrinsicGuess, estimateParameters->solvePnPMethod); + } + }); + + if(_objPoints.needed()){ + markerObjPoints.convertTo(_objPoints, -1); + } +} + +} +} diff --git a/sourceCode/aruco/aruco_calib.cpp b/sourceCode/aruco/aruco_calib.cpp new file mode 100644 index 0000000..643c69f --- /dev/null +++ b/sourceCode/aruco/aruco_calib.cpp @@ -0,0 +1,99 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +#include "aruco_calib.hpp" +#include + +namespace cv { +namespace aruco { +using namespace std; +using namespace cv; + +EstimateParameters::EstimateParameters() : pattern(ARUCO_CCW_CENTER), useExtrinsicGuess(false), + solvePnPMethod(SOLVEPNP_ITERATIVE) {} + +double calibrateCameraAruco(InputArrayOfArrays _corners, InputArray _ids, InputArray _counter, + const Ptr &board, Size imageSize, InputOutputArray _cameraMatrix, + InputOutputArray _distCoeffs, OutputArrayOfArrays _rvecs, + OutputArrayOfArrays _tvecs, + OutputArray _stdDeviationsIntrinsics, + OutputArray _stdDeviationsExtrinsics, + OutputArray _perViewErrors, + int flags, const TermCriteria& criteria) { + // for each frame, get properly processed imagePoints and objectPoints for the calibrateCamera + // function + vector processedObjectPoints, processedImagePoints; + size_t nFrames = _counter.total(); + int markerCounter = 0; + for(size_t frame = 0; frame < nFrames; frame++) { + int nMarkersInThisFrame = _counter.getMat().ptr< int >()[frame]; + vector thisFrameCorners; + vector thisFrameIds; + + CV_Assert(nMarkersInThisFrame > 0); + + thisFrameCorners.reserve((size_t) nMarkersInThisFrame); + thisFrameIds.reserve((size_t) nMarkersInThisFrame); + for(int j = markerCounter; j < markerCounter + nMarkersInThisFrame; j++) { + thisFrameCorners.push_back(_corners.getMat(j)); + thisFrameIds.push_back(_ids.getMat().ptr< int >()[j]); + } + markerCounter += nMarkersInThisFrame; + Mat currentImgPoints, currentObjPoints; + board->matchImagePoints(thisFrameCorners, thisFrameIds, currentObjPoints, + currentImgPoints); + if(currentImgPoints.total() > 0 && currentObjPoints.total() > 0) { + processedImagePoints.push_back(currentImgPoints); + processedObjectPoints.push_back(currentObjPoints); + } + } + return calibrateCamera(processedObjectPoints, processedImagePoints, imageSize, _cameraMatrix, _distCoeffs, _rvecs, + _tvecs, _stdDeviationsIntrinsics, _stdDeviationsExtrinsics, _perViewErrors, flags, criteria); +} + +double calibrateCameraAruco(InputArrayOfArrays _corners, InputArray _ids, InputArray _counter, const Ptr &board, + Size imageSize, InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs, + OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, int flags, const TermCriteria& criteria) { + return calibrateCameraAruco(_corners, _ids, _counter, board, imageSize, _cameraMatrix, _distCoeffs, + _rvecs, _tvecs, noArray(), noArray(), noArray(), flags, criteria); +} + +double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfArrays _charucoIds, + const Ptr &_board, Size imageSize, + InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs, + OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, + OutputArray _stdDeviationsIntrinsics, + OutputArray _stdDeviationsExtrinsics, + OutputArray _perViewErrors, + int flags, const TermCriteria& criteria) { + CV_Assert(_charucoIds.total() > 0 && (_charucoIds.total() == _charucoCorners.total())); + + // Join object points of charuco corners in a single vector for calibrateCamera() function + vector > allObjPoints; + allObjPoints.resize(_charucoIds.total()); + for(unsigned int i = 0; i < _charucoIds.total(); i++) { + unsigned int nCorners = (unsigned int)_charucoIds.getMat(i).total(); + CV_Assert(nCorners > 0 && nCorners == _charucoCorners.getMat(i).total()); + allObjPoints[i].reserve(nCorners); + + for(unsigned int j = 0; j < nCorners; j++) { + int pointId = _charucoIds.getMat(i).at< int >(j); + CV_Assert(pointId >= 0 && pointId < (int)_board->getChessboardCorners().size()); + allObjPoints[i].push_back(_board->getChessboardCorners()[pointId]); + } + } + return calibrateCamera(allObjPoints, _charucoCorners, imageSize, _cameraMatrix, _distCoeffs, _rvecs, _tvecs, + _stdDeviationsIntrinsics, _stdDeviationsExtrinsics, _perViewErrors, flags, criteria); +} + +double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfArrays _charucoIds, + const Ptr &_board, Size imageSize, InputOutputArray _cameraMatrix, + InputOutputArray _distCoeffs, OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, + int flags, const TermCriteria& criteria) { +return calibrateCameraCharuco(_charucoCorners, _charucoIds, _board, imageSize, _cameraMatrix, _distCoeffs, _rvecs, + _tvecs, noArray(), noArray(), noArray(), flags, criteria); +} + +} +} diff --git a/sourceCode/aruco/aruco_calib.hpp b/sourceCode/aruco/aruco_calib.hpp new file mode 100644 index 0000000..2fd344d --- /dev/null +++ b/sourceCode/aruco/aruco_calib.hpp @@ -0,0 +1,180 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html +#ifndef OPENCV_ARUCO_CALIB_POSE_HPP +#define OPENCV_ARUCO_CALIB_POSE_HPP +#include + +namespace cv { +namespace aruco { + +//! @addtogroup aruco +//! @{ + +/** @brief rvec/tvec define the right handed coordinate system of the marker. + * + * PatternPositionType defines center this system and axes direction. + * Axis X (red color) - first coordinate, axis Y (green color) - second coordinate, + * axis Z (blue color) - third coordinate. + * + * @deprecated Use Board::matchImagePoints and cv::solvePnP + * + * @sa estimatePoseSingleMarkers() + */ +enum PatternPositionType { + /** @brief The marker coordinate system is centered on the middle of the marker. + * + * The coordinates of the four corners (CCW order) of the marker in its own coordinate system are: + * (-markerLength/2, markerLength/2, 0), (markerLength/2, markerLength/2, 0), + * (markerLength/2, -markerLength/2, 0), (-markerLength/2, -markerLength/2, 0). + */ + ARUCO_CCW_CENTER, + /** @brief The marker coordinate system is centered on the top-left corner of the marker. + * + * The coordinates of the four corners (CW order) of the marker in its own coordinate system are: + * (0, 0, 0), (markerLength, 0, 0), + * (markerLength, markerLength, 0), (0, markerLength, 0). + * + * These pattern dots are convenient to use with a chessboard/ChArUco board. + */ + ARUCO_CW_TOP_LEFT_CORNER +}; + +/** @brief Pose estimation parameters + * + * @param pattern Defines center this system and axes direction (default PatternPositionType::ARUCO_CCW_CENTER). + * @param useExtrinsicGuess Parameter used for SOLVEPNP_ITERATIVE. If true (1), the function uses the provided + * rvec and tvec values as initial approximations of the rotation and translation vectors, respectively, and further + * optimizes them (default false). + * @param solvePnPMethod Method for solving a PnP problem: see @ref calib3d_solvePnP_flags (default SOLVEPNP_ITERATIVE). + * + * @deprecated Use Board::matchImagePoints and cv::solvePnP + * + * @sa PatternPositionType, solvePnP() + */ +struct CV_EXPORTS_W_SIMPLE EstimateParameters { + CV_PROP_RW aruco::PatternPositionType pattern; + CV_PROP_RW bool useExtrinsicGuess; + CV_PROP_RW int solvePnPMethod; + + CV_WRAP EstimateParameters(); +}; + +/** + * @brief Calibrate a camera using aruco markers + * + * @param corners vector of detected marker corners in all frames. + * The corners should have the same format returned by detectMarkers (see #detectMarkers). + * @param ids list of identifiers for each marker in corners + * @param counter number of markers in each frame so that corners and ids can be split + * @param board Marker Board layout + * @param imageSize Size of the image used only to initialize the intrinsic camera matrix. + * @param cameraMatrix Output 3x3 floating-point camera matrix + * \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ . If CV\_CALIB\_USE\_INTRINSIC\_GUESS + * and/or CV_CALIB_FIX_ASPECT_RATIO are specified, some or all of fx, fy, cx, cy must be + * initialized before calling the function. + * @param distCoeffs Output vector of distortion coefficients + * \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6],[s_1, s_2, s_3, s_4]])\f$ of 4, 5, 8 or 12 elements + * @param rvecs Output vector of rotation vectors (see Rodrigues ) estimated for each board view + * (e.g. std::vector>). That is, each k-th rotation vector together with the corresponding + * k-th translation vector (see the next output parameter description) brings the board pattern + * from the model coordinate space (in which object points are specified) to the world coordinate + * space, that is, a real position of the board pattern in the k-th pattern view (k=0.. *M* -1). + * @param tvecs Output vector of translation vectors estimated for each pattern view. + * @param stdDeviationsIntrinsics Output vector of standard deviations estimated for intrinsic parameters. + * Order of deviations values: + * \f$(f_x, f_y, c_x, c_y, k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6 , s_1, s_2, s_3, + * s_4, \tau_x, \tau_y)\f$ If one of parameters is not estimated, it's deviation is equals to zero. + * @param stdDeviationsExtrinsics Output vector of standard deviations estimated for extrinsic parameters. + * Order of deviations values: \f$(R_1, T_1, \dotsc , R_M, T_M)\f$ where M is number of pattern views, + * \f$R_i, T_i\f$ are concatenated 1x3 vectors. + * @param perViewErrors Output vector of average re-projection errors estimated for each pattern view. + * @param flags flags Different flags for the calibration process (see #calibrateCamera for details). + * @param criteria Termination criteria for the iterative optimization algorithm. + * + * This function calibrates a camera using an Aruco Board. The function receives a list of + * detected markers from several views of the Board. The process is similar to the chessboard + * calibration in calibrateCamera(). The function returns the final re-projection error. + * + * @deprecated Use Board::matchImagePoints and cv::solvePnP + */ +CV_EXPORTS_AS(calibrateCameraArucoExtended) +double calibrateCameraAruco(InputArrayOfArrays corners, InputArray ids, InputArray counter, const Ptr &board, + Size imageSize, InputOutputArray cameraMatrix, InputOutputArray distCoeffs, + OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, OutputArray stdDeviationsIntrinsics, + OutputArray stdDeviationsExtrinsics, OutputArray perViewErrors, int flags = 0, + const TermCriteria& criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON)); + +/** @overload + * @brief It's the same function as #calibrateCameraAruco but without calibration error estimation. + * @deprecated Use Board::matchImagePoints and cv::solvePnP + */ +CV_EXPORTS_W double calibrateCameraAruco(InputArrayOfArrays corners, InputArray ids, InputArray counter, + const Ptr &board, Size imageSize, InputOutputArray cameraMatrix, + InputOutputArray distCoeffs, OutputArrayOfArrays rvecs = noArray(), + OutputArrayOfArrays tvecs = noArray(), int flags = 0, + const TermCriteria& criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, + 30, DBL_EPSILON)); + + +/** + * @brief Calibrate a camera using Charuco corners + * + * @param charucoCorners vector of detected charuco corners per frame + * @param charucoIds list of identifiers for each corner in charucoCorners per frame + * @param board Marker Board layout + * @param imageSize input image size + * @param cameraMatrix Output 3x3 floating-point camera matrix + * \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ . If CV\_CALIB\_USE\_INTRINSIC\_GUESS + * and/or CV_CALIB_FIX_ASPECT_RATIO are specified, some or all of fx, fy, cx, cy must be + * initialized before calling the function. + * @param distCoeffs Output vector of distortion coefficients + * \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6],[s_1, s_2, s_3, s_4]])\f$ of 4, 5, 8 or 12 elements + * @param rvecs Output vector of rotation vectors (see Rodrigues ) estimated for each board view + * (e.g. std::vector>). That is, each k-th rotation vector together with the corresponding + * k-th translation vector (see the next output parameter description) brings the board pattern + * from the model coordinate space (in which object points are specified) to the world coordinate + * space, that is, a real position of the board pattern in the k-th pattern view (k=0.. *M* -1). + * @param tvecs Output vector of translation vectors estimated for each pattern view. + * @param stdDeviationsIntrinsics Output vector of standard deviations estimated for intrinsic parameters. + * Order of deviations values: + * \f$(f_x, f_y, c_x, c_y, k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6 , s_1, s_2, s_3, + * s_4, \tau_x, \tau_y)\f$ If one of parameters is not estimated, it's deviation is equals to zero. + * @param stdDeviationsExtrinsics Output vector of standard deviations estimated for extrinsic parameters. + * Order of deviations values: \f$(R_1, T_1, \dotsc , R_M, T_M)\f$ where M is number of pattern views, + * \f$R_i, T_i\f$ are concatenated 1x3 vectors. + * @param perViewErrors Output vector of average re-projection errors estimated for each pattern view. + * @param flags flags Different flags for the calibration process (see #calibrateCamera for details). + * @param criteria Termination criteria for the iterative optimization algorithm. + * + * This function calibrates a camera using a set of corners of a Charuco Board. The function + * receives a list of detected corners and its identifiers from several views of the Board. + * The function returns the final re-projection error. + * + * @deprecated Use CharucoBoard::matchImagePoints and cv::solvePnP + */ +CV_EXPORTS_AS(calibrateCameraCharucoExtended) +double calibrateCameraCharuco(InputArrayOfArrays charucoCorners, InputArrayOfArrays charucoIds, + const Ptr &board, Size imageSize, InputOutputArray cameraMatrix, + InputOutputArray distCoeffs, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, + OutputArray stdDeviationsIntrinsics, OutputArray stdDeviationsExtrinsics, + OutputArray perViewErrors, int flags = 0, const TermCriteria& criteria = TermCriteria( + TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON)); + +/** + * @brief It's the same function as #calibrateCameraCharuco but without calibration error estimation. + * + * @deprecated Use CharucoBoard::matchImagePoints and cv::solvePnP + */ +CV_EXPORTS_W double calibrateCameraCharuco(InputArrayOfArrays charucoCorners, InputArrayOfArrays charucoIds, + const Ptr &board, Size imageSize, + InputOutputArray cameraMatrix, InputOutputArray distCoeffs, + OutputArrayOfArrays rvecs = noArray(), + OutputArrayOfArrays tvecs = noArray(), int flags = 0, + const TermCriteria& criteria=TermCriteria(TermCriteria::COUNT + + TermCriteria::EPS, 30, DBL_EPSILON)); +//! @} + +} +} +#endif diff --git a/sourceCode/aruco/charuco.cpp b/sourceCode/aruco/charuco.cpp new file mode 100644 index 0000000..3abd8b0 --- /dev/null +++ b/sourceCode/aruco/charuco.cpp @@ -0,0 +1,62 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +#include "precomp.hpp" +#include +#include "charuco.hpp" +#include + +namespace cv { +namespace aruco { + +using namespace std; + +int interpolateCornersCharuco(InputArrayOfArrays _markerCorners, InputArray _markerIds, + InputArray _image, const Ptr &_board, + OutputArray _charucoCorners, OutputArray _charucoIds, + InputArray _cameraMatrix, InputArray _distCoeffs, int minMarkers) { + CharucoParameters params; + params.minMarkers = minMarkers; + params.cameraMatrix = _cameraMatrix.getMat(); + params.distCoeffs = _distCoeffs.getMat(); + CharucoDetector detector(*_board, params); + vector markerCorners; + _markerCorners.getMatVector(markerCorners); + detector.detectBoard(_image, _charucoCorners, _charucoIds, markerCorners, _markerIds.getMat()); + return (int)_charucoIds.total(); +} + + +void detectCharucoDiamond(InputArray _image, InputArrayOfArrays _markerCorners, InputArray _markerIds, + float squareMarkerLengthRate, OutputArrayOfArrays _diamondCorners, OutputArray _diamondIds, + InputArray _cameraMatrix, InputArray _distCoeffs, Ptr dictionary) { + CharucoParameters params; + params.cameraMatrix = _cameraMatrix.getMat(); + params.distCoeffs = _distCoeffs.getMat(); + CharucoBoard board({3, 3}, squareMarkerLengthRate, 1.f, *dictionary); + CharucoDetector detector(board, params); + vector markerCorners; + _markerCorners.getMatVector(markerCorners); + + detector.detectDiamonds(_image, _diamondCorners, _diamondIds, markerCorners, _markerIds.getMat()); +} + + +void drawCharucoDiamond(const Ptr &dictionary, Vec4i ids, int squareLength, int markerLength, + OutputArray _img, int marginSize, int borderBits) { + CV_Assert(squareLength > 0 && markerLength > 0 && squareLength > markerLength); + CV_Assert(marginSize >= 0 && borderBits > 0); + + // assign the charuco marker ids + vector tmpIds(4); + for(int i = 0; i < 4; i++) + tmpIds[i] = ids[i]; + // create a charuco board similar to a charuco marker and print it + CharucoBoard board(Size(3, 3), (float)squareLength, (float)markerLength, *dictionary, tmpIds); + Size outSize(3 * squareLength + 2 * marginSize, 3 * squareLength + 2 * marginSize); + board.generateImage(outSize, _img, marginSize, borderBits); +} + +} +} diff --git a/sourceCode/aruco/charuco.hpp b/sourceCode/aruco/charuco.hpp new file mode 100644 index 0000000..60ba818 --- /dev/null +++ b/sourceCode/aruco/charuco.hpp @@ -0,0 +1,110 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html +#ifndef OPENCV_CHARUCO_HPP +#define OPENCV_CHARUCO_HPP + +#include +#include +#include "../aruco.hpp" +#include +#include "aruco_calib.hpp" + + +namespace cv { +namespace aruco { + +//! @addtogroup aruco +//! @{ + +/** + * @brief Interpolate position of ChArUco board corners + * @param markerCorners vector of already detected markers corners. For each marker, its four + * corners are provided, (e.g std::vector > ). For N detected markers, the + * dimensions of this array should be Nx4. The order of the corners should be clockwise. + * @param markerIds list of identifiers for each marker in corners + * @param image input image necesary for corner refinement. Note that markers are not detected and + * should be sent in corners and ids parameters. + * @param board layout of ChArUco board. + * @param charucoCorners interpolated chessboard corners + * @param charucoIds interpolated chessboard corners identifiers + * @param cameraMatrix optional 3x3 floating-point camera matrix + * \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ + * @param distCoeffs optional vector of distortion coefficients + * \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6],[s_1, s_2, s_3, s_4]])\f$ of 4, 5, 8 or 12 elements + * @param minMarkers number of adjacent markers that must be detected to return a charuco corner + * + * This function receives the detected markers and returns the 2D position of the chessboard corners + * from a ChArUco board using the detected Aruco markers. If camera parameters are provided, + * the process is based in an approximated pose estimation, else it is based on local homography. + * Only visible corners are returned. For each corner, its corresponding identifier is + * also returned in charucoIds. + * The function returns the number of interpolated corners. + * + * @deprecated Use CharucoDetector::detectBoard + */ +CV_EXPORTS_W int interpolateCornersCharuco(InputArrayOfArrays markerCorners, InputArray markerIds, + InputArray image, const Ptr &board, + OutputArray charucoCorners, OutputArray charucoIds, + InputArray cameraMatrix = noArray(), + InputArray distCoeffs = noArray(), int minMarkers = 2); + +/** + * @brief Detect ChArUco Diamond markers + * + * @param image input image necessary for corner subpixel. + * @param markerCorners list of detected marker corners from detectMarkers function. + * @param markerIds list of marker ids in markerCorners. + * @param squareMarkerLengthRate rate between square and marker length: + * squareMarkerLengthRate = squareLength/markerLength. The real units are not necessary. + * @param diamondCorners output list of detected diamond corners (4 corners per diamond). The order + * is the same than in marker corners: top left, top right, bottom right and bottom left. Similar + * format than the corners returned by detectMarkers (e.g std::vector > ). + * @param diamondIds ids of the diamonds in diamondCorners. The id of each diamond is in fact of + * type Vec4i, so each diamond has 4 ids, which are the ids of the aruco markers composing the + * diamond. + * @param cameraMatrix Optional camera calibration matrix. + * @param distCoeffs Optional camera distortion coefficients. + * @param dictionary dictionary of markers indicating the type of markers. + * + * This function detects Diamond markers from the previous detected ArUco markers. The diamonds + * are returned in the diamondCorners and diamondIds parameters. If camera calibration parameters + * are provided, the diamond search is based on reprojection. If not, diamond search is based on + * homography. Homography is faster than reprojection, but less accurate. + * + * @deprecated Use CharucoDetector::detectDiamonds + */ +CV_EXPORTS_W void detectCharucoDiamond(InputArray image, InputArrayOfArrays markerCorners, + InputArray markerIds, float squareMarkerLengthRate, + OutputArrayOfArrays diamondCorners, OutputArray diamondIds, + InputArray cameraMatrix = noArray(), + InputArray distCoeffs = noArray(), + Ptr dictionary = makePtr + (getPredefinedDictionary(PredefinedDictionaryType::DICT_4X4_50))); + + +/** + * @brief Draw a ChArUco Diamond marker + * + * @param dictionary dictionary of markers indicating the type of markers. + * @param ids list of 4 ids for each ArUco marker in the ChArUco marker. + * @param squareLength size of the chessboard squares in pixels. + * @param markerLength size of the markers in pixels. + * @param img output image with the marker. The size of this image will be + * 3*squareLength + 2*marginSize,. + * @param marginSize minimum margins (in pixels) of the marker in the output image + * @param borderBits width of the marker borders. + * + * This function return the image of a ChArUco marker, ready to be printed. + * + * @deprecated Use CharucoBoard::generateImage() + */ +CV_EXPORTS_W void drawCharucoDiamond(const Ptr &dictionary, Vec4i ids, int squareLength, + int markerLength, OutputArray img, int marginSize = 0, + int borderBits = 1); + +//! @} +} +} + +#endif diff --git a/sourceCode/aruco/precomp.hpp b/sourceCode/aruco/precomp.hpp new file mode 100644 index 0000000..e49b075 --- /dev/null +++ b/sourceCode/aruco/precomp.hpp @@ -0,0 +1,11 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +#ifndef __OPENCV_CCALIB_PRECOMP__ +#define __OPENCV_CCALIB_PRECOMP__ + +#include +#include + +#endif diff --git a/sourceCode/binocularMarkCam.cpp b/sourceCode/binocularMarkCam.cpp new file mode 100644 index 0000000..a54500a --- /dev/null +++ b/sourceCode/binocularMarkCam.cpp @@ -0,0 +1,238 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "binocularMarkCam_Export.h" +#include +#include "aruco.hpp" +#include "aruco/charuco.hpp" +#include + +#define _OUTPUT_DEBUG_DATA 0 + +//version 1.0.0 : ʼ汾 +std::string m_strVersion = "1.0.0"; +//汾 +const char* wd_charuco3DMarkVersion(void) +{ + return m_strVersion.c_str(); +} + +/*Breifά궨ǵ㺯*/ +void detectCharucoCorners(const cv::Mat& img, + const SWD_BQ_CharucoMarkInfo markInfo, + const SWD_BQ_MarkBoardInfo boardInfo, + std::vector& markerIdsAll, + std::vector >& markerCornersAll, + std::vector& charucoIds, + std::vector>& charucoCorners) +{ + cv::Mat gray; + if (img.channels() == 3) + cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY); + else + gray = img.clone(); + + enum cv::aruco::PredefinedDictionaryType dictType; + if (1 == markInfo.dictType) + dictType = cv::aruco::DICT_6X6_1000; + else + dictType = cv::aruco::DICT_6X6_1000; //Ŀǰֻ֧DICT_6X6_1000 + + cv::aruco::Dictionary aruco_dict = cv::aruco::getPredefinedDictionary(dictType); + cv::aruco::DetectorParameters params = cv::aruco::DetectorParameters(); + params.cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE; + + cv::Ptr ptrDictionary = cv::makePtr(aruco_dict); + cv::Ptr ptrParams = cv::makePtr(params); +#if 0 + // Żȶԣ + ptrParams->adaptiveThreshWinSizeMin = 3; + ptrParams->adaptiveThreshWinSizeMax = 23; + ptrParams->adaptiveThreshWinSizeStep = 10; + ptrParams->minMarkerPerimeterRate = 0.01; // Сı +#endif + //нǵ + cv::aruco::detectMarkers(gray, ptrDictionary, markerCornersAll, markerIdsAll, ptrParams); + + //Mark(ͬı궨壩 + for (int board = 0; board < boardInfo.totalBoardNum; board++) + { + int startId = board * boardInfo.boardIdInterval; + int endId = startId + boardInfo.boardChaucoIDNum - 1; + + std::vector ids; + for (int i = 0; i < boardInfo.boardChaucoIDNum; i++) + ids.push_back(startId + i); + + // + std::vector markerIds; + std::vector > markerCorners; + for (int i = 0; i < markerIdsAll.size(); i++) + { + if ((markerIdsAll[i] >= startId) && (markerIdsAll[i] <= endId)) + { + markerIds.push_back(markerIdsAll[i]); + markerCorners.push_back(markerCornersAll[i]); + } + } + if (markerIds.size() > 0) + { + std::vector board_charucoIds; + std::vector board_charucoCorners; + + cv::aruco::CharucoBoard board = cv::aruco::CharucoBoard(markInfo.patternSize, markInfo.checkerSize, markInfo.markerSize, aruco_dict, ids); + cv::Ptr ptrBoard = cv::makePtr(board); + cv::aruco::interpolateCornersCharuco( + markerCorners, + markerIds, + gray, + ptrBoard, + board_charucoCorners, + board_charucoIds); + if (board_charucoIds.size() > 0) + { + // ؾȷ + cv::cornerSubPix(gray, board_charucoCorners, cv::Size(11, 11), cv::Size(-1, -1), + cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 50, 0.1)); + + charucoIds.push_back(startId); + charucoCorners.push_back(board_charucoCorners); + } + } + } + return; +} + +SVzNL3DPoint _getStereoPoints3d(cv::Point2f left, cv::Point2f right, const cv::Mat& Q, double offset = 0) +{ + const double cx = Q.at(0, 3); + const double cy = Q.at(1, 3); + const double f = Q.at(2, 3); + const double t = 1.0 / Q.at(3, 2); + //std::cout << "Cx: " << cx << ", Cy: " << cy << ", f: " << f << ", t: " << t << std::endl; + SVzNL3DPoint result; + double dis = left.x - right.x + offset; + result.x = (left.x + cx) * t / dis; + result.y = (left.y + cy) * t / dis; + result.z = f * t / dis; + //std::cout << "L: " << left[_i] << ", R: " << right[_i] << ", dis: " << dis << std::endl; + return result; +} + +typedef struct +{ + int markID; + std::vector left; + std::vector right; +}_MarkerPairing; + +//ȡMark3DϢ +void wd_BQ_getCharuco3DMark( + cv::Mat& leftImage, + cv::Mat& rightImage, + cv::Mat& cameraMatrixL, + cv::Mat& distCoeffsL, + cv::Mat& cameraMatrixR, + cv::Mat& distCoeffsR, + cv::Mat& R1, + cv::Mat& R2, + cv::Mat& P1, + cv::Mat& P2, + cv::Mat& Q, + SWD_BQ_CharucoMarkInfo markInfo, + SWD_BQ_MarkBoardInfo boardInfo, + double disparityOffset, + std::vector& marks) +{ + std::vector markerIdsLeft; + std::vector > markerCornersLeft; + std::vector charucoIdsLeft; + std::vector> charucoCornersLeft;//ÿMark4ǵӦ4ά + detectCharucoCorners(leftImage, + markInfo, + boardInfo, + markerIdsLeft, + markerCornersLeft, + charucoIdsLeft, + charucoCornersLeft); + + std::vector markerIdsRight; + std::vector > markerCornersRight; + std::vector charucoIdsRight; + std::vector> charucoCornersRight; + detectCharucoCorners(rightImage, + markInfo, + boardInfo, + markerIdsRight, + markerCornersRight, + charucoIdsRight, + charucoCornersRight); + +#if _OUTPUT_DEBUG_DATA + cv::Mat imageCopyLeft = leftImage.clone(); + if (markerIdsLeft.size() > 0) + cv::aruco::drawDetectedMarkers(imageCopyLeft, markerCornersLeft, markerIdsLeft); + if (charucoIdsLeft.size() > 0) + cv::aruco::drawDetectedCornersCharuco(imageCopyLeft, charucoCornersLeft, charucoIdsLeft, cv::Scalar(0, 255, 255)); + char markFilenameL[256]; + sprintf_s(markFilenameL, "markersLeft.bmp"); + cv::imwrite(markFilenameL, imageCopyLeft); + + cv::Mat imageCopyRight = leftImage.clone(); + if (markerIdsRight.size() > 0) + cv::aruco::drawDetectedMarkers(imageCopyRight, markerCornersRight, markerIdsRight); + if (charucoIdsRight.size() > 0) + cv::aruco::drawDetectedCornersCharuco(imageCopyRight, charucoCornersRight, charucoIdsRight, cv::Scalar(0, 255, 255)); + char markFilenameR[256]; + sprintf_s(markFilenameR, "markersRight.bmp"); + cv::imwrite(markFilenameR, imageCopyRight); +#endif + + //Marker + std::vector<_MarkerPairing> stereoMarks; + for (int i = 0, i_max = (int)charucoIdsLeft.size(); i < i_max; i++) + { + for (int j = 0, j_max = (int)charucoIdsRight.size(); j < j_max; j++) + { + if ( (charucoIdsLeft[i] == charucoIdsRight[j]) && (charucoCornersLeft[i].size() == charucoCornersRight[j].size())) + { + _MarkerPairing a_pair; + a_pair.markID = charucoIdsLeft[i]; + a_pair.left.insert(a_pair.left.end(), charucoCornersLeft[i].begin(), charucoCornersLeft[i].end()); + a_pair.right.insert(a_pair.right.end(), charucoCornersRight[j].begin(), charucoCornersRight[j].end()); + stereoMarks.push_back(a_pair); + break; + } + } + } + + for (int i = 0, i_max = (int)stereoMarks.size(); i < i_max; i++) + { + //˫ĿУ + std::vector rectCornersLeft; + cv::undistortPoints(stereoMarks[i].left, rectCornersLeft, cameraMatrixL, distCoeffsL, R1, P1); + std::vector rectCornersRight; + cv::undistortPoints(stereoMarks[i].right, rectCornersRight, cameraMatrixR, distCoeffsR, R2, P2); + + std::vector corners; + for (int m = 0; m < rectCornersLeft.size(); m++) + { + double abs_diff = abs(rectCornersLeft[m].y - rectCornersRight[m].y); + if (abs_diff < 1.0) //С1 + { + SVzNL3DPoint a_pt3d = _getStereoPoints3d(rectCornersLeft[m], rectCornersRight[m], Q, disparityOffset); + corners.push_back(a_pt3d); + } + } + if (corners.size() == 4) + { + SWD_charuco3DMark a_3dMark; + a_3dMark.markID = stereoMarks[i].markID; + a_3dMark.mark3D.x = (corners[0].x + corners[1].x + corners[2].x + corners[3].x) / 4; + a_3dMark.mark3D.y = (corners[0].y + corners[1].y + corners[2].y + corners[3].y) / 4; + a_3dMark.mark3D.z = (corners[0].z + corners[1].z + corners[2].z + corners[3].z) / 4; + marks.push_back(a_3dMark); + } + } + return; +} \ No newline at end of file diff --git a/sourceCode/binocularMarkCam_Export.h b/sourceCode/binocularMarkCam_Export.h new file mode 100644 index 0000000..cb80557 --- /dev/null +++ b/sourceCode/binocularMarkCam_Export.h @@ -0,0 +1,47 @@ +#pragma once + +#if defined(SG_API_LIBRARY) +# define WD_APISHARED_EXPORT __declspec(dllexport) +#else +# define WD_APISHARED_EXPORT __declspec(dllimport) +#endif + +#include "SG_baseDataType.h" +#include +#include + + +typedef struct +{ + cv::Size patternSize; //3x3 mark + float checkerSize; //60mm + float markerSize; //45mm + int dictType; //1: DICT_6x6 +}SWD_BQ_CharucoMarkInfo; + +typedef struct +{ + int totalBoardNum; //mark + int boardIdInterval; //ÿMarkcharucoļһ0ʼڶ8ʼΪ8 + int boardChaucoIDNum; //ÿMarkcharuco3x3charuco, άĸΪ4 +}SWD_BQ_MarkBoardInfo; +//汾 +WD_APISHARED_EXPORT const char* wd_charuco3DMarkVersion(void); + +//ȡMark3DϢ +WD_APISHARED_EXPORT void wd_BQ_getCharuco3DMark( + cv::Mat& leftImage, + cv::Mat& rightImage, + cv::Mat& cameraMatrixL, + cv::Mat& distCoeffsL, + cv::Mat& cameraMatrixR, + cv::Mat& distCoeffsR, + cv::Mat& R1, + cv::Mat& R2, + cv::Mat& P1, + cv::Mat& P2, + cv::Mat& Q, + SWD_BQ_CharucoMarkInfo markInfo, + SWD_BQ_MarkBoardInfo boardInfo, + double disparityOffset, + std::vector& marks); \ No newline at end of file From e0a8333bf29a6f8f3ca5a8322f7efb0d11d0e554 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Fri, 5 Dec 2025 23:04:40 +0800 Subject: [PATCH 11/12] -add noiseFilter --- sourceCode/WD_noiseFilter.cpp | 62 +++++++++++++++++++++++++++++++++++ 1 file changed, 62 insertions(+) create mode 100644 sourceCode/WD_noiseFilter.cpp diff --git a/sourceCode/WD_noiseFilter.cpp b/sourceCode/WD_noiseFilter.cpp new file mode 100644 index 0000000..269ea27 --- /dev/null +++ b/sourceCode/WD_noiseFilter.cpp @@ -0,0 +1,62 @@ +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include + +void wd_noiseFilter( + std::vector< std::vector>& scanLines, + const SSG_outlierFilterParam filterParam, + int* errCode) +{ + *errCode = 0; + int lineNum = (int)scanLines.size(); + int nPointCnt = (int)scanLines[0].size(); + bool vldGrid = true; + //ֱ + for (int i = 0; i < lineNum; i++) + { + if (nPointCnt != (int)scanLines[i].size()) + vldGrid = false; + wd_vectorDataRemoveOutlier_overwrite( + scanLines[i], + filterParam); + } + if (false == vldGrid) + { + *errCode = SG_ERR_3D_DATA_INVLD; + return; + } + //ˮƽ + int hLineNum = nPointCnt; //Gridʽɨߵĵһ + //ˮƽɨ + std::vector> filterHLines; + filterHLines.resize(hLineNum); + for (int i = 0; i < hLineNum; i++) + filterHLines[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < hLineNum; j++) + { + filterHLines[j][line] = scanLines[line][j]; + filterHLines[j][line].pt3D.x = scanLines[line][j].pt3D.y; + filterHLines[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + for (int hLine = 0; hLine < hLineNum; hLine++) + { + //˲˳쳣 + std::vector filterData; + std::vector lineNoise; + sg_lineDataRemoveOutlier( + (SVzNL3DPosition*)filterHLines[hLine].data(), + (int)filterHLines[hLine].size(), + filterParam, + filterData, + lineNoise); + for (int j = 0; j < lineNoise.size(); j++) + { + int lineIdx = lineNoise[j]; + scanLines[lineIdx][hLine].pt3D.z = 0; + } + } + return; +} From c968264e05b5a8f71575a02e43858112a587de36 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Fri, 5 Dec 2025 23:05:48 +0800 Subject: [PATCH 12/12] missed file --- sourceCode/aruco.hpp | 103 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 103 insertions(+) create mode 100644 sourceCode/aruco.hpp diff --git a/sourceCode/aruco.hpp b/sourceCode/aruco.hpp new file mode 100644 index 0000000..3693c38 --- /dev/null +++ b/sourceCode/aruco.hpp @@ -0,0 +1,103 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html +#ifndef OPENCV_ARUCO_HPP +#define OPENCV_ARUCO_HPP + +#include "opencv2/objdetect/aruco_detector.hpp" +#include "aruco/aruco_calib.hpp" + +namespace cv { +namespace aruco { + +/** + * @defgroup aruco Aruco markers, module functionality was moved to objdetect module + * @{ + * ArUco Marker Detection, module functionality was moved to objdetect module + * @sa ArucoDetector, CharucoDetector, Board, GridBoard, CharucoBoard + * @} + */ + +//! @addtogroup aruco +//! @{ + +/** @brief detect markers +@deprecated Use class ArucoDetector::detectMarkers +*/ +CV_EXPORTS_W void detectMarkers(InputArray image, const Ptr &dictionary, OutputArrayOfArrays corners, + OutputArray ids, const Ptr ¶meters = makePtr(), + OutputArrayOfArrays rejectedImgPoints = noArray()); + +/** @brief refine detected markers +@deprecated Use class ArucoDetector::refineDetectedMarkers +*/ +CV_EXPORTS_W void refineDetectedMarkers(InputArray image,const Ptr &board, + InputOutputArrayOfArrays detectedCorners, + InputOutputArray detectedIds, InputOutputArrayOfArrays rejectedCorners, + InputArray cameraMatrix = noArray(), InputArray distCoeffs = noArray(), + float minRepDistance = 10.f, float errorCorrectionRate = 3.f, + bool checkAllOrders = true, OutputArray recoveredIdxs = noArray(), + const Ptr ¶meters = makePtr()); + +/** @brief draw planar board +@deprecated Use Board::generateImage +*/ +CV_EXPORTS_W void drawPlanarBoard(const Ptr &board, Size outSize, OutputArray img, int marginSize, + int borderBits); + +/** @brief get board object and image points +@deprecated Use Board::matchImagePoints +*/ +CV_EXPORTS_W void getBoardObjectAndImagePoints(const Ptr &board, InputArrayOfArrays detectedCorners, + InputArray detectedIds, OutputArray objPoints, OutputArray imgPoints); + + +/** @deprecated Use Board::matchImagePoints and cv::solvePnP + */ +CV_EXPORTS_W int estimatePoseBoard(InputArrayOfArrays corners, InputArray ids, const Ptr &board, + InputArray cameraMatrix, InputArray distCoeffs, InputOutputArray rvec, + InputOutputArray tvec, bool useExtrinsicGuess = false); + +/** + * @brief Pose estimation for a ChArUco board given some of their corners + * @param charucoCorners vector of detected charuco corners + * @param charucoIds list of identifiers for each corner in charucoCorners + * @param board layout of ChArUco board. + * @param cameraMatrix input 3x3 floating-point camera matrix + * \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ + * @param distCoeffs vector of distortion coefficients + * \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6],[s_1, s_2, s_3, s_4]])\f$ of 4, 5, 8 or 12 elements + * @param rvec Output vector (e.g. cv::Mat) corresponding to the rotation vector of the board + * (see cv::Rodrigues). + * @param tvec Output vector (e.g. cv::Mat) corresponding to the translation vector of the board. + * @param useExtrinsicGuess defines whether initial guess for \b rvec and \b tvec will be used or not. + * + * This function estimates a Charuco board pose from some detected corners. + * The function checks if the input corners are enough and valid to perform pose estimation. + * If pose estimation is valid, returns true, else returns false. + * @deprecated Use CharucoBoard::matchImagePoints and cv::solvePnP + * @sa use cv::drawFrameAxes to get world coordinate system axis for object points + */ +CV_EXPORTS_W bool estimatePoseCharucoBoard(InputArray charucoCorners, InputArray charucoIds, + const Ptr &board, InputArray cameraMatrix, + InputArray distCoeffs, InputOutputArray rvec, + InputOutputArray tvec, bool useExtrinsicGuess = false); + +/** @deprecated Use cv::solvePnP + */ +CV_EXPORTS_W void estimatePoseSingleMarkers(InputArrayOfArrays corners, float markerLength, + InputArray cameraMatrix, InputArray distCoeffs, + OutputArray rvecs, OutputArray tvecs, OutputArray objPoints = noArray(), + const Ptr& estimateParameters = makePtr()); + + +/** @deprecated Use CharucoBoard::checkCharucoCornersCollinear + */ +CV_EXPORTS_W bool testCharucoCornersCollinear(const Ptr &board, InputArray charucoIds); + +//! @} + +} +} + +#endif