From 6c5adfc7d538423a5e8a799523efbca0353da0bb Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Wed, 10 Dec 2025 22:37:28 +0800 Subject: [PATCH] sx_BQ_getWorkpieceCorners version 1.2.0 --- .../BQ_workpieceCornerExtract_test.cpp | 213 ++- .../BQ_workpieceCornerExtraction.vcxproj | 2 +- baseAlgorithm/baseAlgorithm.vcxproj | 2 +- sourceCode/BQ_workpieceCornerExtraction.cpp | 1539 +++++++++-------- .../BQ_workpieceCornerExtraction_Export.h | 20 +- sourceCode/SG_baseAlgo_Export.h | 30 + sourceCode/SG_baseDataType.h | 6 + sourceCode/SG_baseFunc.cpp | 132 ++ 8 files changed, 1159 insertions(+), 785 deletions(-) diff --git a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp index a502b36..fe2c1c4 100644 --- a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp +++ b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp @@ -232,59 +232,77 @@ void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara) void _outputCornerInfo(char* fileName, SSX_BQworkpieceResult workpieceCorner) { std::ofstream sw(fileName); + + + sw << "节点" << workpieceCorner.workpieceType << std::endl << std::endl; 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 << "A:" << std::endl; + sprintf_s(dataStr, 250, " corner_0: (%g, %g, %g)", workpieceCorner.corner_1[0].x, workpieceCorner.corner_1[0].y, workpieceCorner.corner_1[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); + sprintf_s(dataStr, 250, " corner_1: (%g, %g, %g)", workpieceCorner.corner_1[1].x, workpieceCorner.corner_1[1].y, workpieceCorner.corner_1[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); + sprintf_s(dataStr, 250, " corner_2: (%g, %g, %g)", workpieceCorner.corner_1[2].x, workpieceCorner.corner_1[2].y, workpieceCorner.corner_1[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)); + double dist = sqrt(pow(workpieceCorner.corner_1[0].x - workpieceCorner.corner_1[2].x, 2) + + pow(workpieceCorner.corner_1[0].y - workpieceCorner.corner_1[2].y, 2) + + pow(workpieceCorner.corner_1[0].z - workpieceCorner.corner_1[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); + sprintf_s(dataStr, 250, " corner_0: (%g, %g, %g)", workpieceCorner.corner_2[0].x, workpieceCorner.corner_2[0].y, workpieceCorner.corner_3[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); + sprintf_s(dataStr, 250, " corner_1: (%g, %g, %g)", workpieceCorner.corner_2[1].x, workpieceCorner.corner_2[1].y, workpieceCorner.corner_3[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); + sprintf_s(dataStr, 250, " corner_2: (%g, %g, %g)", workpieceCorner.corner_2[2].x, workpieceCorner.corner_2[2].y, workpieceCorner.corner_3[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)); + dist = sqrt(pow(workpieceCorner.corner_2[0].x - workpieceCorner.corner_2[2].x, 2) + + pow(workpieceCorner.corner_2[0].y - workpieceCorner.corner_2[2].y, 2) + + pow(workpieceCorner.corner_2[0].z - workpieceCorner.corner_2[2].z, 2)); sprintf_s(dataStr, 250, " Len: %g", dist); sw << dataStr << std::endl; + if (workpieceCorner.workpieceType != 3) + { + sw << "C:" << std::endl; + sprintf_s(dataStr, 250, " corner_0: (%g, %g, %g)", workpieceCorner.corner_3[0].x, workpieceCorner.corner_3[0].y, workpieceCorner.corner_3[0].z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, " corner_1: (%g, %g, %g)", workpieceCorner.corner_3[1].x, workpieceCorner.corner_3[1].y, workpieceCorner.corner_3[1].z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, " corner_2: (%g, %g, %g)", workpieceCorner.corner_3[2].x, workpieceCorner.corner_3[2].y, workpieceCorner.corner_3[2].z); + sw << dataStr << std::endl; + dist = sqrt(pow(workpieceCorner.corner_3[0].x - workpieceCorner.corner_3[2].x, 2) + + pow(workpieceCorner.corner_3[0].y - workpieceCorner.corner_3[2].y, 2) + + pow(workpieceCorner.corner_3[0].z - workpieceCorner.corner_3[2].z, 2)); + sprintf_s(dataStr, 250, " Len: %g", dist); + sw << dataStr << std::endl; + } + + if ((workpieceCorner.workpieceType == 1) || (workpieceCorner.workpieceType == 4)) + { + sw << "D:" << std::endl; + sprintf_s(dataStr, 250, " corner_0: (%g, %g, %g)", workpieceCorner.corner_4[0].x, workpieceCorner.corner_4[0].y, workpieceCorner.corner_4[0].z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, " corner_1: (%g, %g, %g)", workpieceCorner.corner_4[1].x, workpieceCorner.corner_4[1].y, workpieceCorner.corner_4[1].z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, " corner_2: (%g, %g, %g)", workpieceCorner.corner_4[2].x, workpieceCorner.corner_4[2].y, workpieceCorner.corner_4[2].z); + sw << dataStr << std::endl; + dist = sqrt(pow(workpieceCorner.corner_4[0].x - workpieceCorner.corner_4[2].x, 2) + + pow(workpieceCorner.corner_4[0].y - workpieceCorner.corner_4[2].y, 2) + + pow(workpieceCorner.corner_4[0].z - workpieceCorner.corner_4[2].z, 2)); + sprintf_s(dataStr, 250, " Len: %g", dist); + sw << dataStr << std::endl; + } + sw << std::endl; + + sprintf_s(dataStr, 250, "len_A1 : %g", workpieceCorner.len135_A1); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "len_A2 : %g", workpieceCorner.len225_A2); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "len_B2 : %g", workpieceCorner.len315_B2); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "len_B1 : %g", workpieceCorner.len45_B1); + sw << dataStr << std::endl; sw.close(); } @@ -395,14 +413,14 @@ void _outputRGBDScanLapWeld_RGBD( std::vector>& scanLines, SSX_BQworkpieceResult workpieceCorner, bool outDebugInfo, - SSX_debugInfo* debugData) + std::vector& debugData) { int lineNum = (int)scanLines.size(); std::ofstream sw(fileName); int realLines = lineNum; if (workpieceCorner.workpieceType > 0) realLines++; - if(debugData) + if(debugData.size() > 0) realLines++; sw << "LineNum:" << realLines << std::endl; sw << "DataType: 0" << std::endl; @@ -472,24 +490,29 @@ void _outputRGBDScanLapWeld_RGBD( if (workpieceCorner.workpieceType > 0) { - int linePtNum = 12; + std::vector ptBuffer; + for (int i = 0; i < 3; i++) + ptBuffer.push_back(workpieceCorner.corner_1[i]); + for (int i = 0; i < 3; i++) + ptBuffer.push_back(workpieceCorner.corner_2[i]); + if (workpieceCorner.workpieceType != 3) + { + for (int i = 0; i < 3; i++) + ptBuffer.push_back(workpieceCorner.corner_3[i]); + } + if ((workpieceCorner.workpieceType == 1) || (workpieceCorner.workpieceType == 4)) + { + for (int i = 0; i < 3; i++) + ptBuffer.push_back(workpieceCorner.corner_4[i]); + } + ptBuffer.push_back(workpieceCorner.center); + int linePtNum = (int)ptBuffer.size(); 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++) + for (int j = 0; j < linePtNum; j++) { float x = (float)ptBuffer[j].x; float y = (float)ptBuffer[j].y; @@ -507,18 +530,52 @@ void _outputRGBDScanLapWeld_RGBD( sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; } - if ((debugData) && (workpieceCorner.workpieceType > 0)) + if ((debugData.size() > 0) && (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; - 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; + std::vector< SWD_3DPointPair> lines; + SWD_3DPointPair a_line; + a_line.pt1 = workpieceCorner.corner_1[0]; + a_line.pt2 = workpieceCorner.corner_1[2]; + lines.push_back(a_line); + a_line.pt1 = workpieceCorner.corner_1[1]; + a_line.pt2 = workpieceCorner.center; + lines.push_back(a_line); + + a_line.pt1 = workpieceCorner.corner_2[0]; + a_line.pt2 = workpieceCorner.corner_2[2]; + lines.push_back(a_line); + a_line.pt1 = workpieceCorner.corner_2[1]; + a_line.pt2 = workpieceCorner.center; + lines.push_back(a_line); + + if (workpieceCorner.workpieceType != 3) + { + a_line.pt1 = workpieceCorner.corner_3[0]; + a_line.pt2 = workpieceCorner.corner_3[2]; + lines.push_back(a_line); + a_line.pt1 = workpieceCorner.corner_3[1]; + a_line.pt2 = workpieceCorner.center; + lines.push_back(a_line); + } + if ((workpieceCorner.workpieceType == 1) || (workpieceCorner.workpieceType == 4)) + { + a_line.pt1 = workpieceCorner.corner_4[0]; + a_line.pt2 = workpieceCorner.corner_4[2]; + lines.push_back(a_line); + a_line.pt1 = workpieceCorner.corner_4[1]; + a_line.pt2 = workpieceCorner.center; + lines.push_back(a_line); + } + + int linePtNum = 0; + for (int i = 0; i < (int)debugData.size(); i++) + linePtNum += debugData[i].edge_size + debugData[i].edgeLink1_size + debugData[i].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 i = 0; i < (int)debugData.size(); i++) { for (int j = 0; j < debugData[i].edge_size; j++) { @@ -560,32 +617,10 @@ void _outputRGBDScanLapWeld_RGBD( rgb = { 255, 0, 0 }; size = 3; int lineIdx = 0; - for (int i = 0; i < 4; i++) + for (int i = 0; i < (int)lines.size(); 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]; + SVzNL3DPoint pt0 = lines[i].pt1; + SVzNL3DPoint pt1 = lines[i].pt2; sw << "Poly_" << lineIdx << "_2" << std::endl; sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-"; sw << "{0,0}-{0,0}-"; @@ -596,8 +631,8 @@ void _outputRGBDScanLapWeld_RGBD( lineIdx++; } //加一个直线，用于跳过显示工具bug - SVzNL3DPoint pt0 = debugData[0].edge_ends[0]; - SVzNL3DPoint pt1 = debugData[0].edge_ends[1]; + SVzNL3DPoint pt0 = lines[0].pt1; + SVzNL3DPoint pt1 = lines[0].pt2; sw << "Poly_" << lineIdx << "_2" << std::endl; sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-"; sw << "{0,0}-{0,0}-"; @@ -783,7 +818,7 @@ int main() SSX_BQworkpiecePara workpieceParam; workpieceParam.lineLen = 180.0; //直线段长度 int errCode = 0; - SSX_debugInfo debug_conturs[4]; + std::vector debug_conturs; SSX_BQworkpieceResult workpieceCorner = sx_BQ_getWorkpieceCorners( scanLines, cornerParam, @@ -800,7 +835,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, true, debug_conturs); + _outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, false, debug_conturs); #else _outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, true, NULL); #endif diff --git a/BQ_workpieceCornerExtraction/BQ_workpieceCornerExtraction.vcxproj b/BQ_workpieceCornerExtraction/BQ_workpieceCornerExtraction.vcxproj index c9c8465..c519166 100644 --- a/BQ_workpieceCornerExtraction/BQ_workpieceCornerExtraction.vcxproj +++ b/BQ_workpieceCornerExtraction/BQ_workpieceCornerExtraction.vcxproj @@ -145,7 +145,7 @@ true true true - NDEBUG;BQWORKPIECECORNEREXTRACTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + NDEBUG;BQWORKPIECECORNEREXTRACTION_EXPORTS;_WINDOWS;_USRDLL;SG_API_LIBRARY;%(PreprocessorDefinitions) true NotUsing pch.h diff --git a/baseAlgorithm/baseAlgorithm.vcxproj b/baseAlgorithm/baseAlgorithm.vcxproj index 11a0b7f..21bd19d 100644 --- a/baseAlgorithm/baseAlgorithm.vcxproj +++ b/baseAlgorithm/baseAlgorithm.vcxproj @@ -160,10 +160,10 @@ false ..\..\thirdParty\opencv320\build\x64\vc14\lib;%(AdditionalLibraryDirectories) opencv_world320.lib;%(AdditionalDependencies) + /bigobj %(AdditionalOptions) - diff --git a/sourceCode/BQ_workpieceCornerExtraction.cpp b/sourceCode/BQ_workpieceCornerExtraction.cpp index 83f5dd5..0d6e9ea 100644 --- a/sourceCode/BQ_workpieceCornerExtraction.cpp +++ b/sourceCode/BQ_workpieceCornerExtraction.cpp @@ -164,8 +164,11 @@ typedef struct typedef struct { + int pkId; int lineIdx; int ptIdx; + int cptIndex; //Ô²ÖÜɨÃèÉϵĵãÐò + //double cornerAngle; //ÒÔµãΪÖÐÐĵÄÁ½²àÏÒµÄ¼Ð½Ç double R; double angle; double x; @@ -173,6 +176,22 @@ typedef struct double z; }SWD_polarPt; +typedef struct +{ + int cptIndex; + int L1_ptIndex; + int L2_ptIndex; + double cornerAngle; + int cornerDir; +}SWD_polarPeakInfo; + +typedef struct +{ + double angle; + SVzNL3DPoint corner[3]; + double line_a, line_b, line_c; //±ßµÄ´¹Ïß·½³Ì ax+by+c = 0 +}SWD_branchInfo; + //ÄæʱÕëÐýתʱ ¦È > 0 £»Ë³Ê±ÕëÐýתʱ ¦È < 0 cv::Point2f _rotate2D(cv::Point2f pt, double sinTheta, double cosTheta) { @@ -194,6 +213,330 @@ int _counterLinePtNum(std::vector& lineData) return ptNum; } +//¼ÆËã·ÖÖ§ÐÅÏ¢ +int _getBranchInfo( + int validStartLine, //¿ªÊ¼É¨Ãè±ß½ç + int validEndLine, //½áÊøɨÃè±ß½ç + std::vector& polarPoints, + SWD_polarPt branchCorner, + SWD_polarPeakInfo branchCornerInfo, + SWD_branchInfo* resultBranchInfo, + std::vector& LinePts, + std::vector< SVzNL3DPoint>& edgePt1, + std::vector< SVzNL3DPoint>& edgePt2 + ) +{ + int contourPtSize = (int)polarPoints.size(); + std::vector branchContourPts; + + int cornerWin = 2; + int LineDir, contourDir, startIdx, endingIdx; + if (branchCornerInfo.cornerDir == 2)//ÄæʱÕë + { + startIdx = branchCorner.cptIndex - cornerWin; //´Ë´¦½«cornerÖÜΧµÄµã²»¼ÆËãÔÚÄÚ + if (startIdx < 0) + startIdx += contourPtSize; + LineDir = -1; + contourDir = 1; + endingIdx = branchCornerInfo.L1_ptIndex; + } + else //˳ʱÕë + { + startIdx = (branchCorner.cptIndex + cornerWin) % contourPtSize; + LineDir = 1; + contourDir = -1; + endingIdx = branchCornerInfo.L2_ptIndex; + } + + int ptIdx = startIdx; + while (1) + { + SVzNL3DPoint a_pt = { polarPoints[ptIdx].x, polarPoints[ptIdx].y, polarPoints[ptIdx].z }; + LinePts.push_back(a_pt); + if (ptIdx == endingIdx) + break; + ptIdx += LineDir; + if (ptIdx < 0) + ptIdx += contourPtSize; + else + ptIdx = ptIdx % contourPtSize; + } + if (LinePts.size() < 5) + return -1; + + //µ±Ö±½ÇÓÉÓÚɨÃè±ß½ç±»ÇгÉÒ»¸öÉϱߺÜСµÄÌÝÐÎʱ£¬¼ì²é³öµÄ½Çµã¿ÉÄܲ»ÊÇÕæÕýµÄ½Çµã£¬ÐèÒª½øÐмì²é + //µü´ú¼ÆËãÕæÕýµÄ½Çµã£ºÈ¡cornerÁ½±ß5¸öµã£¬ÈôÓб߽磬¼ì²é + int chkWin = 5; + bool toRefine = false; + for (int i = -chkWin; i <= chkWin; i++) + { + int idx = branchCorner.cptIndex + i; + if (idx < 0) + idx += contourPtSize; + else + idx = idx % contourPtSize; + if ((polarPoints[idx].lineIdx == validStartLine) || (polarPoints[idx].lineIdx == validEndLine)) + { + toRefine = true; + break; + } + } + if (true == toRefine) + { + SVzNL3DPoint pt1 = LinePts[0]; + SVzNL3DPoint pt2 = LinePts.back(); + double aa, bb, cc; + compute2ptLine( pt1, pt2, &aa, &bb, &cc); + //¼ÆËãÕæÕýµÄ½Çµã + double maxH = 0; + int maxHPos = 0; + for (int i = 0; i < (int)LinePts.size(); i++) + { + double H = computePtDistToLine(LinePts[i].x, LinePts[i].y, aa, bb, cc); + if (maxH < H) + { + maxH = H; + maxHPos = i; + } + } + int cptIdx = startIdx + LineDir * maxHPos; + if (cptIdx < 0) + cptIdx += contourPtSize; + else + cptIdx = cptIdx % contourPtSize; + + branchCorner = polarPoints[cptIdx]; + if (branchCornerInfo.cornerDir == 2)//ÄæʱÕë + { + startIdx = branchCorner.cptIndex - cornerWin; //´Ë´¦½«cornerÖÜΧµÄµã²»¼ÆËãÔÚÄÚ + if (startIdx < 0) + startIdx += contourPtSize; + } + else //˳ʱÕë + startIdx = (branchCorner.cptIndex + cornerWin) % contourPtSize; + + LinePts.clear(); + ptIdx = startIdx; + while (1) + { + SVzNL3DPoint a_pt = { polarPoints[ptIdx].x, polarPoints[ptIdx].y, polarPoints[ptIdx].z }; + LinePts.push_back(a_pt); + if (ptIdx == endingIdx) + break; + ptIdx += LineDir; + if (ptIdx < 0) + ptIdx += contourPtSize; + else + ptIdx = ptIdx % contourPtSize; + } + if (LinePts.size() < 5) + return -1; + } + + //ÄâºÏÖ±Ïß: ax+by+c = 0 + double _a = 0, _b = 0, _c = 0; + lineFitting_abc(LinePts, &_a, &_b, &_c); + SVzNL3DPoint endingPt0 = LinePts.back(); + //¼ÆËã¶Ëµã´¹×ã + SVzNL2DPointD footPt = sx_getFootPoint_abc(endingPt0.x, endingPt0.y, _a, _b, _c); + //ÌáÈ¡ÆäËüÂÖÀªµã + for (int i = cornerWin; i < contourPtSize; i++) //±Ü¿ª½ÇµãÖÜΧ + { + int ptIdx = i * contourDir + branchCorner.cptIndex; + if (ptIdx < 0) + ptIdx += contourPtSize; + else + ptIdx = ptIdx % contourPtSize; + SWD_polarPt a_pt = polarPoints[ptIdx]; + SVzNL2DPointD contourFoot = sx_getFootPoint_abc(a_pt.x, a_pt.y, _a, _b, _c); + double dist = sqrt(pow(footPt.x - contourFoot.x, 2) + pow(footPt.y - contourFoot.y, 2)); + if (dist < 1.0) + break; + branchContourPts.push_back(a_pt); + } + //¼ì²éÂÖÀªµãÖÐÓÐûÓб߽çµã¡£Èç¹ûÓб߽çµã£¬ÐèÒª½øÐÐÍÆÀí + int contourPtNum = (int)branchContourPts.size(); + bool hasSidePt = false; + for (int m = 0; m < contourPtNum; m++) + { + if ((branchContourPts[m].lineIdx == validStartLine) || + (branchContourPts[m].lineIdx == validEndLine)) + { + branchContourPts[m].z = -1.0; //label + hasSidePt = true; + } + } + //·Ö¿ªbranchµÄÁíÁ½¶Î + if (false == hasSidePt) + { + double maxDist = -1; + int maxPos = -1; + //ÕÒµ½¹Õµã:¼ÆËãËùÓеãÓë¶ËµãµÄ¾àÀ룬ÒÔ×î´óÖµ×÷Ϊ¹Õµã + for (int m = 0; m < contourPtNum; m++) + { + double dist = sqrt(pow(branchContourPts[m].x - endingPt0.x, 2) + + pow(branchContourPts[m].y - endingPt0.y, 2)); + if (maxDist < 0) + { + maxDist = dist; + maxPos = m; + } + else + { + if (maxDist < dist) + { + maxDist = dist; + maxPos = m; + } + } + } + //·Ö¸ôÁ½¶Î + int start0 = 0; + int end0 = maxPos - cornerWin; + + for (int m = start0; m <= end0; m++) + { + SVzNL3DPoint a_pt = { branchContourPts[m].x, branchContourPts[m].y, branchContourPts[m].z }; + edgePt1.push_back(a_pt); + } + + int start1 = maxPos + cornerWin; + int end1 = contourPtNum - 1; + + for (int m = start1; m <= end1; m++) + { + SVzNL3DPoint a_pt = { branchContourPts[m].x, branchContourPts[m].y, branchContourPts[m].z }; + edgePt2.push_back(a_pt); + } + + //ÄâºÏÇó½»µã + double edge1_a, edge1_b, edge1_c; + lineFitting_abc(edgePt1, &edge1_a, &edge1_b, &edge1_c); + double edge2_a, edge2_b, edge2_c; + lineFitting_abc(edgePt2, &edge2_a, &edge2_b, &edge2_c); + + //¼ÆËã½»µã + SWD_branchInfo a_branchInfo; + a_branchInfo.corner[0] = computeLineCrossPt_abs(_a, _b, _c, edge1_a, edge1_b, edge1_c); + a_branchInfo.corner[0].z = computeMeanZ(edgePt1); + a_branchInfo.corner[2] = computeLineCrossPt_abs(edge1_a, edge1_b, edge1_c, edge2_a, edge2_b, edge2_c); + a_branchInfo.corner[2].z = a_branchInfo.corner[0].z; + a_branchInfo.corner[1].x = (a_branchInfo.corner[0].x + a_branchInfo.corner[2].x) / 2; + a_branchInfo.corner[1].y = (a_branchInfo.corner[0].y + a_branchInfo.corner[2].y) / 2; + a_branchInfo.corner[1].z = a_branchInfo.corner[0].z; + // + a_branchInfo.angle = branchCorner.angle; + a_branchInfo.line_a = _a; + a_branchInfo.line_a = _b; + a_branchInfo.line_a = _c; + *resultBranchInfo = a_branchInfo; + } + else + { + //½«±ß½çÈ¥³ý£¬Ê£ÏµÄÁ½¶Î½øÐд¦Àí + std::vector< SVzNL3DPoint> edgePt1; + for (int i = 0; i < contourPtNum; i++) + { + SVzNL3DPoint a_pt = { branchContourPts[i].x, branchContourPts[i].y, branchContourPts[i].z }; + if ((branchContourPts[i].lineIdx == validStartLine)|| (branchContourPts[i].lineIdx == validEndLine)) + { + break; + } + edgePt1.push_back(a_pt); + } + std::vector< SVzNL3DPoint> edgePt2; + for (int i = contourPtNum - 1; i >= 0; i--) + { + SVzNL3DPoint a_pt = { branchContourPts[i].x, branchContourPts[i].y, branchContourPts[i].z }; + if ((branchContourPts[i].lineIdx == validStartLine) || (branchContourPts[i].lineIdx == validEndLine)) + break; + edgePt2.insert(edgePt2.begin(), a_pt); + } + //¼ÆËãµÚÒ»¶ÎÔÚÖ±ÏßÉϵĴ¹×㣬ȡ´¹×ãµÄÖÊÐÄ¡£¹ýÖÊÐĵĴ¹Ïß + int edge1PtNum = (int)edgePt1.size(); + int edge2PtNum = (int)edgePt2.size(); + if ((edge1PtNum == 0) || (edge2PtNum == 0)) + { + return -1; + } + SVzNL2DPointD foot = { 0.0,0.0 }; + for (int i = 0; i < edge1PtNum; i++) + { + SVzNL2DPointD a_foot = sx_getFootPoint_abc(edgePt1[i].x, edgePt1[i].y, _a, _b, _c); + foot.x += a_foot.x; + foot.y += a_foot.y; + } + foot.x = foot.x / edge1PtNum; + foot.y = foot.y / edge1PtNum; + double edge1_a, edge1_b, edge1_c; + edge1_a = _b; + edge1_b = -_a; + edge1_c = _a * foot.y - _b * foot.x; + //¼ÆËãµÚ¶þ¶ÎÔÚÖ±ÏßÉϵĴ¹×㣬ȡ´¹×ãµÄÖÊÐÄ¡£¹ýÖÊÐĵĴ¹Ïß + foot.x = 0; + foot.y = 0; + for (int i = 0; i < edge2PtNum; i++) + { + SVzNL2DPointD a_foot = sx_getFootPoint_abc(edgePt2[i].x, edgePt2[i].y, edge1_a, edge1_b, edge1_c); + foot.x += a_foot.x; + foot.y += a_foot.y; + } + foot.x = foot.x / edge2PtNum; + foot.y = foot.y / edge2PtNum; + double edge2_a, edge2_b, edge2_c; + edge2_a = _a; + edge2_b = _b; + edge2_c = -_a * foot.x - _b * foot.y; + //¼ÆËã½»µã + SWD_branchInfo a_branchInfo; + a_branchInfo.corner[0] = computeLineCrossPt_abs(_a, _b, _c, edge1_a, edge1_b, edge1_c); + a_branchInfo.corner[0].z = computeMeanZ(edgePt1); + a_branchInfo.corner[2] = computeLineCrossPt_abs(edge1_a, edge1_b, edge1_c, edge2_a, edge2_b, edge2_c); + a_branchInfo.corner[2].z = a_branchInfo.corner[0].z; + a_branchInfo.corner[1].x = (a_branchInfo.corner[0].x + a_branchInfo.corner[2].x) / 2; + a_branchInfo.corner[1].y = (a_branchInfo.corner[0].y + a_branchInfo.corner[2].y) / 2; + a_branchInfo.corner[1].z = a_branchInfo.corner[0].z; + // + a_branchInfo.angle = branchCorner.angle; + a_branchInfo.line_a = _a; + a_branchInfo.line_b = _b; + a_branchInfo.line_c = -(_a * a_branchInfo.corner[1].x + _b * a_branchInfo.corner[1].y); + *resultBranchInfo = a_branchInfo; + } + return 0; +} + + +SVzNL3DPoint computeEdgeCross(double angle, std::vector& polarPoints, double a, double b, double c) +{ + double minDist = -1; + int angleIdx = -1; + int ptSize = (int)polarPoints.size(); + for (int i = 0; i < ptSize; i++) + { + double diff = computeAngleDiff(angle, polarPoints[i].angle); + if (diff < 30) //ËÑË÷60¶È·¶Î§ + { + double H = computePtDistToLine(polarPoints[i].x, polarPoints[i].y, a, b, c); + if (minDist < 0) + { + minDist = H; + angleIdx = i; + } + else + { + if (minDist > H) + { + minDist = H; + angleIdx = i; + } + } + } + } + SVzNL3DPoint a_pt = { polarPoints[angleIdx].x, polarPoints[angleIdx].y, polarPoints[angleIdx].z }; + return a_pt; +} + SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( std::vector< std::vector>& scanLines, const SSG_cornerParam cornerPara, @@ -202,7 +545,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( SSG_planeCalibPara groundCalibPara, SSX_BQworkpiecePara workpieceParam, #if _OUTPUT_DEBUG_DATA - SSX_debugInfo* debug_conturs, + std::vector& debug_contours, #endif int* errCode) { @@ -246,7 +589,6 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( int linePtNum = (int)scanLines[0].size(); bool isGridData = true; - SSX_featureContour region[4]; //×ÔÊÊÓ¦¸÷ÖÖÐýת½Ç¶È { //´¹Ö±Ìø±äÌØÕ÷ÌáÈ¡ @@ -442,6 +784,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( SWD_polarPt a_polarPt; a_polarPt.lineIdx = lineIdx; a_polarPt.ptIdx = ptIdx; + a_polarPt.cptIndex = -1; a_polarPt.R = 0; a_polarPt.angle = 0; a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x; @@ -481,8 +824,11 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( } //°´½Ç¶È´óСÅÅÐò std::sort(polarPoints.begin(), polarPoints.end(), compareByAngle); + for (int pi = 0; pi < contourPtSize; pi++) + polarPoints[pi].cptIndex = pi; // index + //ÌáÈ¡R¼«Öµµã - std::vector rPeaks; + double minR = -1, maxR = -1; //¼ÆËã×îСºÍ×î´óµÄR£¬ÓÃÒÔÇø·ÖÓÐûÓзÖÖ§¡£minRºÍmaxRÏà²îСʱ£¬ÎªÔ²Ðλò8½ÇÐΣ¬Ã»ÓзÖÖ§ std::vector polarRPeakPts; int winSize = contourPtSize / 36; //+-10¶È·¶Î§ if (winSize < 5) @@ -490,6 +836,16 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( for (int pi = 0; pi < contourPtSize; pi++) { double currR = polarPoints[pi].R; + if (minR < 0) + { + minR = currR; + maxR = currR; + } + else + { + minR = minR > currR ? currR : minR; + maxR = maxR < currR ? currR : maxR; + } bool isPeak = true; for (int k = -winSize; k <= winSize; k++) { @@ -501,654 +857,509 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( } } if (true == isPeak) - { - rPeaks.push_back(pi); polarRPeakPts.push_back(polarPoints[pi]); - } } - if (rPeaks.size() != 8) + + double ratio_MaxMin = maxR / minR; + bool hasBranch = ratio_MaxMin < 1.25 ? false : true; + + std::vector validPolarRPeakPts; + std::vector polarPeakInfo; + int pkId = 0; + //¹ýÂËÔ²»¡¶ÎµÄ¼«Öµ£ºÓÉÓÚÖØÐÄÆ«ÒÆ£¬Ô²»¡¶ÎÒ²»áÐγɼ«Öµ¡£¸ù¾Ý¼«ÖµÁ½±ßL=Ö±Ï߶γ¤¶È¹¹³ÉµÄÕŽÇÅÐ¶Ï + for (int i = 0, i_max = (int)polarRPeakPts.size(); i < i_max; i++) { - *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 ptidx = polarRPeakPts[i].cptIndex; + double px, py, pz; + px = polarRPeakPts[i].x; + py = polarRPeakPts[i].y; + + int LL1 = -1; + for (int j = ptidx - 1; j > -contourPtSize; j--) { - 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; + int idx = (j + contourPtSize) % contourPtSize; //ͲÐνṹ + double cx = polarPoints[idx].x; + double cy = polarPoints[idx].y; + double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2)); + if (len > workpieceParam.lineLen) + { + LL1 = idx; + break; + } } - if ((true == pre_isPair) && (false == nxt_isPair)) + int LL2 = -1; + for (int j = ptidx + 1; j < contourPtSize*2; j++) { - 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; + int idx = j % contourPtSize; //ͲÐνṹ + double cx = polarPoints[idx].x; + double cy = polarPoints[idx].y; + double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2)); + if (len > workpieceParam.lineLen) + { + LL2 = idx; + break; + } } - else if ((false == pre_isPair) && (true == nxt_isPair)) + + if ((LL1 >= 0) && (LL2 >= 0)) { - 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; + double len1 = sqrt(pow(px - polarPoints[LL1].x, 2) + pow(py - polarPoints[LL1].y, 2)); + double len2 = sqrt(pow(px - polarPoints[LL2].x, 2) + pow(py - polarPoints[LL2].y, 2)); + double len3 = sqrt(pow(polarPoints[LL1].x - polarPoints[LL2].x, 2) + + pow(polarPoints[LL1].y - polarPoints[LL2].y, 2)); + double cosTheta = (len1 * len1 + len2 * len2 - len3 * len3) / (2 * len1 * len2); + double theta = acos(cosTheta) * 180.0 / PI; + if (theta < 150) + { + double L1_angle = polarPoints[ptidx].angle - polarPoints[LL1].angle; + if (L1_angle < 0) + L1_angle += 360; + double L2_angle = polarPoints[LL2].angle - polarPoints[ptidx].angle; + if (L2_angle < 0) + L2_angle += 360; + + SWD_polarPeakInfo a_pkInfo; + a_pkInfo.cptIndex = ptidx; + a_pkInfo.L1_ptIndex = LL1; + a_pkInfo.L2_ptIndex = LL2; + a_pkInfo.cornerAngle = theta; + if(L1_angle < L2_angle) + a_pkInfo.cornerDir = 2; //ÄæʱÕë + else + a_pkInfo.cornerDir = 1; //˳ʱÕë + + polarRPeakPts[i].cptIndex = ptidx; + polarRPeakPts[i].pkId = pkId; + pkId++; + validPolarRPeakPts.push_back(polarRPeakPts[i]); + polarPeakInfo.push_back(a_pkInfo); + } + } + } + + int workpieceType = -1; + std::vector< SWD_branchInfo> branchInfo; + if (true == hasBranch) + { + //¼ÆËã·ÖÖ§£¬90¶È·¶Î§ÄÚΪͬһ·ÖÖ§¡£¸ù¾Ý·ÖÖ§ÊýÁ¿È·¶¨¹¤¼þÀàÐÍ + int pkSize = (int)validPolarRPeakPts.size(); + for (int m = 0; m < pkSize; m++) + { + if (validPolarRPeakPts[m].cptIndex < 0) + continue; + + if (polarPeakInfo[m].cornerDir == 2) //ÄæʱÕë¡£ + { + int nxtIdx = (m + 1)%pkSize; + double angleDiff = validPolarRPeakPts[nxtIdx].angle - validPolarRPeakPts[m].angle; + if (angleDiff < 0) + angleDiff += 360; + if (angleDiff < 90) //Ϊͬһbranch£¬ºÏ²¢£¬Ö»±£ÁôÒ»¸ö + { + validPolarRPeakPts[nxtIdx].pkId = validPolarRPeakPts[m].pkId; //pair + validPolarRPeakPts[nxtIdx].cptIndex = -1; + } + } + } + + std::vector> branchPeaks; //ÿ¸öbranch×î¶àÁ½¸ö¼«Öµµã + branchPeaks.resize(pkId); + std::vector> branchPeakInfo; + branchPeakInfo.resize(pkId); + for (int m = 0; m < pkSize; m++) + { + if (validPolarRPeakPts[m].cptIndex < 0) + validPolarRPeakPts[m].cptIndex = polarPeakInfo[m].cptIndex; //»Ö¸´ + int pkId = validPolarRPeakPts[m].pkId; + branchPeaks[pkId].push_back(validPolarRPeakPts[m]); + branchPeakInfo[pkId].push_back(polarPeakInfo[m]); + } + for (int m = pkSize - 1; m >= 0; m--) + { + if (branchPeaks[m].size() == 0) + { + branchPeaks.erase(branchPeaks.begin() + m); + branchPeakInfo.erase(branchPeakInfo.begin() + m); + } + } + int branchNum = (int)branchPeaks.size(); + if (branchNum == 2) + workpieceType = 3; //½Úµã3 + else if (branchNum == 3) + workpieceType = 2; //½Úµã2 + else if (branchNum == 4) + workpieceType = 1; //½Úµã1 + else + workpieceType = 0; + + //¼ÆËã¸÷¸öbranchµÄÐÅÏ¢ + for (int branchIdx = 0; branchIdx < branchNum; branchIdx++) + { + std::vector& a_branch = branchPeaks[branchIdx]; + std::vector& a_branchInfo = branchPeakInfo[branchIdx]; + //È¡¹Ì¶¨³¤¶È´¹Ö±±ß + SWD_polarPt branchCorner; + SWD_polarPeakInfo branchCornerInfo; + if (a_branch.size() == 2) //È¡½Ç¶È×î½Ó½ü90¶ÈµÄ + { + double diff1 = abs(a_branchInfo[0].cornerAngle - 90); + double diff2 = abs(a_branchInfo[1].cornerAngle - 90); + branchCorner = diff1 < diff2 ? a_branch[0]: a_branch[1]; + branchCornerInfo = diff1 < diff2 ? a_branchInfo[0] : a_branchInfo[1]; + } + else + { + branchCorner = a_branch[0]; + branchCornerInfo = a_branchInfo[0]; + } + + SWD_branchInfo resultBranchInfo; + std::vector branchLinePts; //ÓÃÓÚÏÔʾºÍdebug + std::vector< SVzNL3DPoint> branchEdgePt1; + std::vector< SVzNL3DPoint> branchEdgePt2; + int opOK = _getBranchInfo( + validStartLine, //¿ªÊ¼É¨Ãè±ß½ç + validEndLine, //½áÊøɨÃè±ß½ç + polarPoints, + branchCorner, + branchCornerInfo, + &resultBranchInfo, + branchLinePts, + branchEdgePt1, + branchEdgePt2); + if(opOK < 0) + { + *errCode = SX_ERR_ZERO_CONTOUR_PT; + return workpieceCorners; + } + branchInfo.push_back(resultBranchInfo); +#if _OUTPUT_DEBUG_DATA + SSX_debugInfo a_branchDebug; + a_branchDebug.rgnIdx = (int)branchInfo.size(); + a_branchDebug.edge_size = (int)branchEdgePt1.size(); + a_branchDebug.edgeLink1_size = (int)branchLinePts.size(); + a_branchDebug.edgeLink2_size = (int)branchEdgePt2.size(); + a_branchDebug.edge = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edge_size); + a_branchDebug.edgeLink_1 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink1_size); + a_branchDebug.edgeLink_2 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink2_size); +#endif + for(int m = 0; m < a_branchDebug.edge_size; m ++) + a_branchDebug.edge[m] = branchEdgePt1[m]; + for (int m = 0; m < a_branchDebug.edgeLink1_size; m++) + a_branchDebug.edgeLink_1[m] = branchLinePts[m]; + for (int m = 0; m < a_branchDebug.edgeLink2_size; m++) + a_branchDebug.edgeLink_2[m] = branchEdgePt2[m]; + debug_contours.push_back(a_branchDebug); + } + workpieceCorners.workpieceType = workpieceType; + if (workpieceType == 1) //4¸öbranch + { + for (int m = 0; m < 3; m++) + workpieceCorners.corner_1[m] = branchInfo[0].corner[m]; + for (int m = 0; m < 3; m++) + workpieceCorners.corner_2[m] = branchInfo[1].corner[m]; + for (int m = 0; m < 3; m++) + workpieceCorners.corner_3[m] = branchInfo[2].corner[m]; + for (int m = 0; m < 3; m++) + workpieceCorners.corner_4[m] = branchInfo[3].corner[m]; + //¼ÆËãÊ£ÓàÐÅÏ¢ + double line1_a, line1_b, line1_c; + compute2ptLine( + workpieceCorners.corner_1[1], workpieceCorners.corner_3[1], + &line1_a, &line1_b, &line1_c); + double line2_a, line2_b, line2_c; + compute2ptLine( + workpieceCorners.corner_2[1], workpieceCorners.corner_4[1], + &line2_a, &line2_b, &line2_c); + workpieceCorners.center = computeLineCrossPt_abs( + line1_a, line1_b, line1_c, + line2_a, line2_b, line2_c); + workpieceCorners.center.z = (workpieceCorners.corner_1[1].z + workpieceCorners.corner_2[1].z + + workpieceCorners.corner_3[1].z + workpieceCorners.corner_4[1].z) / 4; + //line1Ðýת45¶È·½Ïò + double r45_line1_a, r45_line1_b, r45_line1_c; + rotateLine45Deg( + line1_a, line1_b, line1_c, + workpieceCorners.center.x, workpieceCorners.center.y, + &r45_line1_a, &r45_line1_b, &r45_line1_c); + + double r45_line2_a, r45_line2_b, r45_line2_c; + rotateLine45Deg( + line2_a, line2_b, line2_c, + workpieceCorners.center.x, workpieceCorners.center.y, + &r45_line2_a, &r45_line2_b, &r45_line2_c); + + double angle1 = getLineAngle(r45_line1_a, r45_line1_b, r45_line1_c); + double angle2 = getLineAngle(r45_line2_a, r45_line2_b, r45_line2_c); + if (angle1 < angle2) + { + SVzNL3DPoint a_cross = computeEdgeCross(angle1, polarPoints, r45_line1_a, r45_line1_b, r45_line1_c); + workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.center, a_cross); + a_cross = computeEdgeCross((angle1+180), polarPoints, r45_line1_a, r45_line1_b, r45_line1_c); + workpieceCorners.len225_A2 = compute2DLen(workpieceCorners.center, a_cross); + a_cross = computeEdgeCross(angle2, polarPoints, r45_line2_a, r45_line2_b, r45_line2_c); + workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.center, a_cross); + a_cross = computeEdgeCross((angle2+180), polarPoints, r45_line2_a, r45_line2_b, r45_line2_c); + workpieceCorners.len315_B2 = compute2DLen(workpieceCorners.center, a_cross); + } + else + { + SVzNL3DPoint a_cross = computeEdgeCross(angle2, polarPoints, r45_line2_a, r45_line2_b, r45_line2_c); + workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.center, a_cross); + a_cross = computeEdgeCross((angle2 + 180), polarPoints, r45_line2_a, r45_line2_b, r45_line2_c); + workpieceCorners.len225_A2 = compute2DLen(workpieceCorners.center, a_cross); + a_cross = computeEdgeCross(angle1, polarPoints, r45_line1_a, r45_line1_b, r45_line1_c); + workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.center, a_cross); + a_cross = computeEdgeCross((angle1 + 180), polarPoints, r45_line1_a, r45_line1_b, r45_line1_c); + workpieceCorners.len315_B2 = compute2DLen(workpieceCorners.center, a_cross); + } + } + else if (workpieceType == 2) //3 ¸öbranch + { + int startIdx = 0; + for (int i = 0; i < 3; i++) + { + int nxtIdx = (i + 1) % 3; + double diff = computeAngleDiff(branchInfo[i].angle, branchInfo[nxtIdx].angle); + if (diff > 135) + { + startIdx = nxtIdx; + break; + } + } + + double crossDir; + for (int m = 0; m < 3; m++) + workpieceCorners.corner_1[m] = branchInfo[startIdx].corner[m]; + startIdx = (startIdx + 1) % 3; + crossDir = branchInfo[startIdx].angle; + for (int m = 0; m < 3; m++) + workpieceCorners.corner_2[m] = branchInfo[startIdx].corner[m]; + startIdx = (startIdx + 1) % 3; + for (int m = 0; m < 3; m++) + workpieceCorners.corner_3[m] = branchInfo[startIdx].corner[m]; + + //¼ÆËãÊ£ÓàÐÅÏ¢ + workpieceCorners.center.x = (workpieceCorners.corner_1[1].x + workpieceCorners.corner_3[1].x) / 2; + workpieceCorners.center.y = (workpieceCorners.corner_1[1].y + workpieceCorners.corner_3[1].y) / 2; + workpieceCorners.center.z = (workpieceCorners.corner_1[1].z + workpieceCorners.corner_3[1].z) / 2; + + double line_a, line_b, line_c; + compute2ptLine( + workpieceCorners.corner_2[1], workpieceCorners.center, + &line_a, &line_b, &line_c); + + workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.corner_1[1], workpieceCorners.center); + workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.corner_2[1], workpieceCorners.center); + workpieceCorners.len225_A2 = compute2DLen(workpieceCorners.corner_3[1], workpieceCorners.center); + SVzNL3DPoint a_cross = computeEdgeCross((crossDir+180), polarPoints, line_a, line_b, line_c); + workpieceCorners.len315_B2 = compute2DLen(a_cross, workpieceCorners.center); } else { - *errCode = SX_ERR_INVLID_RPEAK_PAIR; + for (int m = 0; m < 3; m++) + workpieceCorners.corner_1[m] = branchInfo[0].corner[m]; + for (int m = 0; m < 3; m++) + workpieceCorners.corner_2[m] = branchInfo[1].corner[m]; + SVzNL3DPoint center = computeLineCrossPt_abs( + branchInfo[0].line_a, branchInfo[0].line_b, branchInfo[0].line_c, + branchInfo[1].line_a, branchInfo[1].line_b, branchInfo[1].line_c); + workpieceCorners.center.x = center.x; + workpieceCorners.center.y = center.y; + workpieceCorners.center.z = (workpieceCorners.corner_1[1].z + workpieceCorners.corner_2[1].z) / 2; + workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.corner_1[1], workpieceCorners.center); + workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.corner_2[1], workpieceCorners.center); + SVzNL3DPoint a_cross = computeEdgeCross( + (branchInfo[0].angle + 180), polarPoints, + branchInfo[0].line_a, branchInfo[0].line_b, branchInfo[0].line_c); + workpieceCorners.len225_A2 = compute2DLen(a_cross, workpieceCorners.center); + a_cross = computeEdgeCross( + (branchInfo[1].angle + 180), polarPoints, + branchInfo[1].line_a, branchInfo[1].line_b, branchInfo[1].line_c); + workpieceCorners.len315_B2 = compute2DLen(a_cross, workpieceCorners.center); + } + } + else + { + workpieceType = 4; //½Úµã4 + //¼ì²éûÓÐÔڱ߽çµÄCorner + int polarPkNum = (int)validPolarRPeakPts.size(); + for (int pki = 0; pki < polarPkNum; pki++) + { + //¼ì²éÊÇ·ñÔÚɨÃè±ß½ç + bool isSide = false; + int cptIdx = validPolarRPeakPts[pki].cptIndex; + SVzNL3DPoint cpt = { validPolarRPeakPts[pki].x, validPolarRPeakPts[pki].y, validPolarRPeakPts[pki].z }; + std::vector edgePt_1; + for (int i = 0; i < contourPtSize; i++) + { + int idx = (i + cptIdx)%contourPtSize; + SVzNL3DPoint a_pt = { polarPoints[idx].x, polarPoints[idx].y, polarPoints[idx].z }; + double len = sqrt(pow(cpt.x - a_pt.x, 2) + pow(cpt.y - a_pt.y, 2)); + if (len > workpieceParam.lineLen) + break; + else + { + if ((polarPoints[idx].lineIdx == validStartLine) || + (polarPoints[idx].lineIdx == validEndLine)) + isSide = true; + else + edgePt_1.push_back(a_pt); + } + } + + std::vector edgePt_2; + for (int i = 0; i < contourPtSize; i++) + { + int idx = cptIdx - i; + if (idx < 0) + idx += contourPtSize; + + SVzNL3DPoint a_pt = { polarPoints[idx].x, polarPoints[idx].y, polarPoints[idx].z }; + double len = sqrt(pow(cpt.x - a_pt.x, 2) + pow(cpt.y - a_pt.y, 2)); + if (len > workpieceParam.lineLen) + break; + else + { + if ((polarPoints[idx].lineIdx == validStartLine) || + (polarPoints[idx].lineIdx == validEndLine)) + isSide = true; + else + edgePt_2.push_back(a_pt); + } + } + if( (edgePt_1.size() < 10) || (edgePt_2.size() < 10)) + { + *errCode = SX_ERR_ZERO_CONTOUR_PT; + return workpieceCorners; + } + + if (false == isSide) + { + //ÄâºÏ¼ÆËã½»µã£¬ÐÞÕý + double edge1_a = 0, edge1_b = 0, edge1_c = 0; + lineFitting_abc(edgePt_1, &edge1_a, &edge1_b, &edge1_c); + double edge2_a = 0, edge2_b = 0, edge2_c = 0; + lineFitting_abc(edgePt_2, &edge2_a, &edge2_b, &edge2_c); + //¼ÆËã½»µã + SVzNL3DPoint crossPt = computeLineCrossPt_abs( + edge1_a, edge1_b, edge1_c, edge2_a, edge2_b, edge2_c); + validPolarRPeakPts[pki].x = crossPt.x; + validPolarRPeakPts[pki].y = crossPt.y; + } + } + if (polarPkNum != 8) + { + *errCode = SX_ERR_INVLID_RPEAK_NUM; 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) + //È¡»¥Ïà´¹Ö±µÄÁ½¶Ô + //£¨1£©È¡ÏàÁÚcorner¾àÀë×îСµÄÁ½¶Î + std::vector sideLens; + for (int i = 0; i < polarPkNum; i++) { - for (int m = startIdx + 1; m < endIdx; m++) + int nxtIdx = (i + 1) % polarPkNum; + double len = sqrt(pow(validPolarRPeakPts[i].x - validPolarRPeakPts[nxtIdx].x, 2) + + pow(validPolarRPeakPts[i].y - validPolarRPeakPts[nxtIdx].y, 2)); + sideLens.push_back(len); + } + double minLen1 = -1; + int minLen1_idx = -1; + for (int i = 0; i < polarPkNum; i++) + { + if (minLen1 < 0) { - SVzNL3DPoint a_pt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z }; - edge.push_back(a_pt); + minLen1 = sideLens[i]; + minLen1_idx = i; } - } - else //·ÖÁ½¶Î - { - for (int m = startIdx + 1; m < contourPtSize; m++) + else { - 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()); - } - } - -#if 0 //ÀÏËã·¨£¬ÐèҪɨÃèΪˮƽ-ÊúÖ±·½Ïò - { - //´¹Ö±Ìø±äÌØÕ÷ÌáÈ¡ - 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ͨ³£È¡bagHµÄ1/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ͨ³£È¡bagHµÄ1/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) + if(minLen1 > sideLens[i]) { - 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; + minLen1 = sideLens[i]; + minLen1_idx = i; } - 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++) + double minLen2 = -1; + int minLen2_idx = -1; + for (int i = 0; i < polarPkNum; i++) { - SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j]; - if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//Ðé¼ÙÄ¿±ê¹ýÂ˺óµã»áÖÃ0 + if (i == minLen1_idx) + continue; + + if (minLen2 < 0) { - int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16; - if (existEdgeId == 0) + minLen2 = sideLens[i]; + minLen2_idx = i; + } + else + { + if (minLen2 > sideLens[i]) { - 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; + minLen2 = sideLens[i]; + minLen2_idx = i; } - a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D); } } - hvTreeIdx++; - } - int hvTreeSize = hvTreeIdx; + if( (minLen1_idx < 0) || (minLen2_idx < 0)) + { + *errCode = SX_ERR_INVLID_RPEAK_NUM; + return workpieceCorners; + } - if (v_trees.size() < 2) - { - *errCode = SX_ERR_INVLD_VTREE_NUM; - return workpieceCorners; - } - if (h_trees.size() < 2) - { - *errCode = SX_ERR_INVLD_HTREE_NUM; - return workpieceCorners; - } - - //Ñ°ÕÒvTreeµÄ×îÉϺÍ×îÏ - int vTree_T = 0; - int vTree_B = 0; - for (int i = 1, i_max = (int)v_trees.size(); i < i_max; i++) - { - if (v_trees[i].roi.top < v_trees[vTree_T].roi.top) - vTree_T = i; - if (v_trees[i].roi.bottom > v_trees[vTree_B].roi.bottom) - vTree_B = i; - } - - //Ñ°ÕÒhTreeµÄ×î×óºÍ×îÓÒ - int hTree_L = 0; - int hTree_R = 0; - for (int i = 1, i_max = (int)h_trees.size(); i < i_max; i++) - { - //ˮƽɨÃèxyÊǽ»»»µÄ£¬×óÓÒ¶ÔÓ¦ROIµÄtopBottom - if (h_trees[i].roi.top < h_trees[hTree_L].roi.top) - hTree_L = i; - if (h_trees[i].roi.bottom > h_trees[hTree_R].roi.bottom) - hTree_R = i; - } - - 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; + workpieceCorners.workpieceType = workpieceType; + //¼ÆË㹤¼þÐÅÏ¢ + int tmpIdx = (minLen1_idx + 1) % polarPkNum; + workpieceCorners.corner_1[0] = { validPolarRPeakPts[minLen1_idx].x, validPolarRPeakPts[minLen1_idx].y, validPolarRPeakPts[minLen1_idx].z }; + workpieceCorners.corner_1[2] = { validPolarRPeakPts[tmpIdx].x, validPolarRPeakPts[tmpIdx].y, validPolarRPeakPts[tmpIdx].z }; + workpieceCorners.corner_1[1].x = (workpieceCorners.corner_1[0].x + workpieceCorners.corner_1[2].x) / 2; + workpieceCorners.corner_1[1].y = (workpieceCorners.corner_1[0].y + workpieceCorners.corner_1[2].y) / 2; + workpieceCorners.corner_1[1].z = (workpieceCorners.corner_1[0].z + workpieceCorners.corner_1[2].z) / 2; + //˳ÐòµÄÁ½¸öcornerΪÏÂÒ»¶Î + tmpIdx = (tmpIdx +1) % polarPkNum; + int tmpIdx1 = (tmpIdx + 1) % polarPkNum; + workpieceCorners.corner_2[0] = { validPolarRPeakPts[tmpIdx].x, validPolarRPeakPts[tmpIdx].y, validPolarRPeakPts[tmpIdx].z }; + workpieceCorners.corner_2[2] = { validPolarRPeakPts[tmpIdx1].x, validPolarRPeakPts[tmpIdx1].y, validPolarRPeakPts[tmpIdx1].z }; + workpieceCorners.corner_2[1].x = (workpieceCorners.corner_2[0].x + workpieceCorners.corner_2[2].x) / 2; + workpieceCorners.corner_2[1].y = (workpieceCorners.corner_2[0].y + workpieceCorners.corner_2[2].y) / 2; + workpieceCorners.corner_2[1].z = (workpieceCorners.corner_2[0].z + workpieceCorners.corner_2[2].z) / 2; + //µÚ¶þ¸ö×î¶ÌµÄÁ½¸öcornerΪµÚ3¸öcorner + tmpIdx = (minLen2_idx + 1) % polarPkNum; + workpieceCorners.corner_3[0] = { validPolarRPeakPts[minLen2_idx].x, validPolarRPeakPts[minLen2_idx].y, validPolarRPeakPts[minLen2_idx].z }; + workpieceCorners.corner_3[2] = { validPolarRPeakPts[tmpIdx].x, validPolarRPeakPts[tmpIdx].y, validPolarRPeakPts[tmpIdx].z }; + workpieceCorners.corner_3[1].x = (workpieceCorners.corner_3[0].x + workpieceCorners.corner_3[2].x) / 2; + workpieceCorners.corner_3[1].y = (workpieceCorners.corner_3[0].y + workpieceCorners.corner_3[2].y) / 2; + workpieceCorners.corner_3[1].z = (workpieceCorners.corner_3[0].z + workpieceCorners.corner_3[2].z) / 2; + //˳ÐòµÄÁ½¸öcornerΪÏÂÒ»¶Î + tmpIdx = (tmpIdx + 1) % polarPkNum; + tmpIdx1 = (tmpIdx + 1) % polarPkNum; + workpieceCorners.corner_4[0] = { validPolarRPeakPts[tmpIdx].x, validPolarRPeakPts[tmpIdx].y, validPolarRPeakPts[tmpIdx].z }; + workpieceCorners.corner_4[2] = { validPolarRPeakPts[tmpIdx1].x, validPolarRPeakPts[tmpIdx1].y, validPolarRPeakPts[tmpIdx1].z }; + workpieceCorners.corner_4[1].x = (workpieceCorners.corner_4[0].x + workpieceCorners.corner_4[2].x) / 2; + workpieceCorners.corner_4[1].y = (workpieceCorners.corner_4[0].y + workpieceCorners.corner_4[2].y) / 2; + workpieceCorners.corner_4[1].z = (workpieceCorners.corner_4[0].z + workpieceCorners.corner_4[2].z) / 2; + //¼ÆËãÊ£ÓàÐÅÏ¢ + double line1_a, line1_b, line1_c; + compute2ptLine( + workpieceCorners.corner_1[1], workpieceCorners.corner_3[1], + &line1_a, &line1_b, &line1_c); + double line2_a, line2_b, line2_c; + compute2ptLine( + workpieceCorners.corner_2[1], workpieceCorners.corner_4[1], + &line2_a, &line2_b, &line2_c); + workpieceCorners.center = computeLineCrossPt_abs( + line1_a, line1_b, line1_c, + line2_a, line2_b, line2_c); + workpieceCorners.center.z = (workpieceCorners.corner_1[1].z + workpieceCorners.corner_2[1].z + + workpieceCorners.corner_3[1].z + workpieceCorners.corner_4[1].z) / 4; + workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.corner_1[1], workpieceCorners.center); + workpieceCorners.len225_A2 = compute2DLen(workpieceCorners.corner_2[1], workpieceCorners.center); + workpieceCorners.len315_B2 = compute2DLen(workpieceCorners.corner_3[1], workpieceCorners.center); + workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.corner_4[1], workpieceCorners.center); } } -#endif - - 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 //½«Êý¾ÝÖØÐÂͶÉä»ØÔ­À´µÄ×ø±êϵ£¬ÒÔ±£³ÖÊÖÑ۱궨½á¹ûÕýÈ· @@ -1158,77 +1369,39 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( 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; + rawObj = _translatePoint(workpieceCorners.corner_1[i], groundCalibPara.invRMatrix); + workpieceCorners.corner_1[i] = rawObj; + rawObj = _translatePoint(workpieceCorners.corner_2[i], groundCalibPara.invRMatrix); + workpieceCorners.corner_2[i] = rawObj; + rawObj = _translatePoint(workpieceCorners.corner_3[i], groundCalibPara.invRMatrix); + workpieceCorners.corner_3[i] = rawObj; + rawObj = _translatePoint(workpieceCorners.corner_4[i], groundCalibPara.invRMatrix); + workpieceCorners.corner_4[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 + rawObj = _translatePoint(workpieceCorners.center, groundCalibPara.invRMatrix); + workpieceCorners.center = rawObj; #endif #if _OUTPUT_DEBUG_DATA - if (debug_conturs) + for (int i = 0; i < (int)debug_contours.size(); i++) { - for (int i = 0; i < 4; i++) + SSX_debugInfo& a_branchDebug = debug_contours[i]; + for (int m = 0; m < a_branchDebug.edge_size; m++) { - 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]; + rawObj = _translatePoint(a_branchDebug.edge[m], groundCalibPara.invRMatrix); + a_branchDebug.edge[m] = rawObj; + } + for (int m = 0; m < a_branchDebug.edgeLink1_size; m++) + { + rawObj = _translatePoint(a_branchDebug.edgeLink_1[m], groundCalibPara.invRMatrix); + a_branchDebug.edgeLink_1[m] = rawObj; + } + for (int m = 0; m < a_branchDebug.edgeLink2_size; m++) + { + rawObj = _translatePoint(a_branchDebug.edgeLink_2[m], groundCalibPara.invRMatrix); + a_branchDebug.edgeLink_2[m] = rawObj; } } #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 index 9282e23..4cce85f 100644 --- a/sourceCode/BQ_workpieceCornerExtraction_Export.h +++ b/sourceCode/BQ_workpieceCornerExtraction_Export.h @@ -13,10 +13,11 @@ typedef struct typedef struct { int workpieceType; //1-½Úµã1£» 2-½Úµã2£» 3-½Úµã3£»4-½Úµã4£»0-δ֪£» ÆäËü-·Ç·¨ - SVzNL3DPoint corner_L[3]; - SVzNL3DPoint corner_R[3]; - SVzNL3DPoint corner_T[3]; - SVzNL3DPoint corner_B[3]; + //1,2,3,4ΪÄæʱÕë + SVzNL3DPoint corner_1[3]; + SVzNL3DPoint corner_2[3]; + SVzNL3DPoint corner_3[3]; + SVzNL3DPoint corner_4[3]; SVzNL3DPoint center; //¹¤¼þÖÐÐĵã double len135_A1; //A1³¤¶È£¬´Ó¹¤¼þÖÐÐÄ135¶È·½Ïò double len45_B1; //B1³¤¶È£¬´Ó¹¤¼þÖÐÐÄ45¶È·½Ïò @@ -27,15 +28,12 @@ typedef struct 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]; + SVzNL3DPoint* edge; + SVzNL3DPoint* edgeLink_1; + SVzNL3DPoint* edgeLink_2; }SSX_debugInfo; //¶Á°æ±¾ºÅ @@ -62,6 +60,6 @@ SG_APISHARED_EXPORT SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( SSG_planeCalibPara groundCalibPara, SSX_BQworkpiecePara workpieceParam, #if _OUTPUT_DEBUG_DATA - SSX_debugInfo* debug_conturs, + std::vector& debug_contours, #endif int* errCode); diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 77683ed..e79a9f9 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -1,8 +1,10 @@ #pragma once + #include "SG_algo_Export.h" #include #include + //Â˳ýÀëȺµã:zÌø±äÃÅÏÞ·½·¨£¨´óÓÚÃÅÏÞÊÓΪ²»Á¬Ðø£¬¸ù¾ÝÁ¬Ðø¶ÎµãÊýÁ¿ÅжÏÔëÉù£© SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier( SVzNL3DPosition* lineData, @@ -343,10 +345,38 @@ SG_APISHARED_EXPORT void lineFitting_abc( double* _b, double* _c); +//¼ÆËãZ¾ùÖµ +SG_APISHARED_EXPORT double computeMeanZ(std::vector< SVzNL3DPoint>& pts); + +//¼ÆËã½Ç¶È²îÖµ£¬ÔÚ0-180¶È·¶Î§ +SG_APISHARED_EXPORT double computeAngleDiff(double theta1, double theta2); + +//¼ÆËãÖ±Ïß½»µã SG_APISHARED_EXPORT SVzNL3DPoint computeLineCrossPt_abs( double a1, double b1, double c1, double a2, double b2, double c2); +//¼ÆËã¹ý2µãµÄÖ±Ïß·½³Ì +SG_APISHARED_EXPORT void compute2ptLine( + SVzNL3DPoint pt1, + SVzNL3DPoint pt2, + double* _a, double* _b, double* _c); + +//Ðýת45¶ÈºóµÄÖ±Ïß·½³Ì +SG_APISHARED_EXPORT void rotateLine45Deg( + double _a, double _b, double _c, + double x0, double y0, + double* r_a, double* r_b, double* r_c); + +//¼ÆËãÖ±Ïß½Ç¶È +SG_APISHARED_EXPORT double getLineAngle(double _a, double _b, double _c); + +//¼ÆËãÁ½µãµÄ2D¾àÀë +SG_APISHARED_EXPORT double compute2DLen(SVzNL3DPoint pt1, SVzNL3DPoint pt2); + +//¼ÆËãµãµ½Ö±Ïß¾àÀë +SG_APISHARED_EXPORT double computePtDistToLine(double x0, double y0, double a, double b, double c); + //¼ÆËã´¹×ãµã£¬Ö±Ïß·½³Ì£ºy = kx + b SG_APISHARED_EXPORT SVzNL2DPointD sx_getFootPoint( double x0, diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 4fbce8c..e45b0d5 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -478,3 +478,9 @@ typedef struct int markID; SVzNL3DPoint mark3D; }SWD_charuco3DMark; + +typedef struct +{ + SVzNL3DPoint pt1; + SVzNL3DPoint pt2; +}SWD_3DPointPair; \ No newline at end of file diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 2a971d3..4c2c2d5 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -237,6 +237,138 @@ void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b) *_b = (-x_sum * xy_sum + xx_sum * y_sum) / (num * xx_sum - x_sum * x_sum);//¸ù¾Ý¹«Ê½Çó½âb } +//ÄâºÏ³ÉͨÓÃÖ±Ïß·½³Ì£¬°üÀ¨´¹Ö± +void lineFitting_abc(std::vector< SVzNL3DPoint>& inliers, double* _a, double* _b, double* _c) +{ + //ÅжÏÊÇ·ñΪ´¹Ö± + int dataSize = (int)inliers.size(); + if (dataSize <2) + return; + + double deltaX = abs(inliers[0].x - inliers[dataSize - 1].x); + double deltaY = abs(inliers[0].y - inliers[dataSize - 1].y); + std::vector< SVzNL3DPoint> fittingData; + if (deltaX < deltaY) + { + //x=ky+b ÄâºÏ + for (int i = 0; i < dataSize; i++) + { + SVzNL3DPoint a_fitPt; + a_fitPt.x = inliers[i].y; + a_fitPt.y = inliers[i].x; + a_fitPt.z = inliers[i].z; + fittingData.push_back(a_fitPt); + } + double k = 0, b = 0; + lineFitting(fittingData, &k, &b); + //ax+by+c + *_a = 1.0; + *_b = -k; + *_c = -b; + } + else + { + //y = kx+bÄâºÏ + double k = 0, b = 0; + lineFitting(inliers, &k, &b); + //ax+by+c + *_a = k; + *_b = -1; + *_c = b; + } + return; +} + +//¼ÆËãZ¾ùÖµ +double computeMeanZ(std::vector< SVzNL3DPoint>& pts) +{ + int ptNum = (int)pts.size(); + int vldNum = 0; + double sumZ = 0; + for (int i = 0; i < ptNum; i++) + { + if (pts[i].z > 1e-4) + { + sumZ += pts[i].z; + vldNum++; + } + } + if (vldNum > 0) + return (sumZ / vldNum); + else + return 0; +} + +SVzNL3DPoint computeLineCrossPt_abs(double a1, double b1, double c1, double a2, double b2, double c2) +{ + SVzNL3DPoint crossPt; + crossPt.x = (c2 * b1 - c1 * b2) / (a1 * b2 - a2 * b1); + crossPt.y = (c2 * a1 - c1 * a2) / (b1 * a2 - b2 * a1); + crossPt.z = 0; + return crossPt; +} + +//¼ÆËã½Ç¶È²îÖµ£¬ÔÚ0-180¶È·¶Î§ +double computeAngleDiff(double theta1, double theta2) +{ + double diff = theta1 - theta2; + if (diff < 0) + diff += 360; + if (diff > 180) + diff = 360 - diff; + + return diff; +} + +void compute2ptLine(SVzNL3DPoint pt1, SVzNL3DPoint pt2, double* _a, double* _b, double* _c) +{ + *_a = pt2.y - pt1.y; + *_b = pt1.x - pt2.x; + *_c = pt2.x * pt1.y - pt1.x * pt2.y; + return; +} + +//Ðýת45¶ÈºóµÄÖ±Ïß·½³Ì +void rotateLine45Deg( + double _a, double _b, double _c, + double x0, double y0, + double* r_a, double* r_b, double* r_c) +{ + // ÐýתºóÖ±ÏßµÄϵÊý£¨»ùÓÚÊýѧÍƵ¼£© + *r_a = _a + _b; + *r_b = _b - _a; + *r_c = -(*r_a) * x0 - (*r_b) * y0; + return; +} + +double getLineAngle(double _a, double _b, double _c) +{ + if (_a = 0) + return 0; + else if (_b = 0) + return 90; + else + { + double theta = atan(-_a / _b) + PI / 2; + theta = (theta * 180.0) / PI; + return theta; + } +} + +//¼ÆËãÁ½µãµÄ2D¾àÀë +double compute2DLen(SVzNL3DPoint pt1, SVzNL3DPoint pt2) +{ + double len = sqrt(pow(pt1.x - pt2.x, 2) + pow(pt1.y - pt2.y, 2)); + return len; +} + +double computePtDistToLine(double x0, double y0, double a, double b, double c) +{ + double tmp = sqrt(pow(a, 2) + pow(b, 2)); + double dist = abs(a * x0 + b * y0 + c) / tmp; + return dist; +} + //¼ÆËã´¹×ãµã£¬Ö±Ïß·½³Ì£ºy = kx + b SVzNL2DPointD sx_getFootPoint(double x0, double y0, double k, double b) {