diff --git a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp index 47272a9..0f51daf 100644 --- a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp +++ b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp @@ -607,13 +607,19 @@ void _outputRGBDScanLapWeld_RGBD( 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; + if (debugData.size() > 0) + { + if (debugData[0].edge != NULL) + { + //加一个点，用于跳过显示工具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 }; @@ -743,7 +749,7 @@ int main() const char* ver = wd_BQWorkpieceCornerVersion(); printf("ver:%s\n", ver); - for (int grp = 7; grp <= 7; grp++) + for (int grp = 6; grp <= 8; grp++) { SSG_planeCalibPara poseCalibPara; //初始化成单位阵 @@ -765,7 +771,7 @@ int main() for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx =2; + //fidx =8; char _scan_file[256]; if(0 == grp) sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx); @@ -821,8 +827,8 @@ int main() filterParam.outlierTh = 5; SSG_treeGrowParam growParam; growParam.maxLineSkipNum = 10; - growParam.yDeviation_max = 5.0; - growParam.maxSkipDistance = 5.0; + growParam.yDeviation_max = 10.0; + growParam.maxSkipDistance = 10.0; growParam.zDeviation_max = 5.0;// growParam.minLTypeTreeLen = 100; //mm growParam.minVTypeTreeLen = 100; //mm @@ -831,8 +837,8 @@ int main() workpieceParam.lineLen = 120; // 120.0; //直线段长度 else workpieceParam.lineLen = 160.0; //直线段长度 - workpieceParam.dirAngleScale = 20.0; //计算方向角的尺度 - workpieceParam.lineDeviation = 10.0;//直线偏离度。工作直线边缘由于扫描和切割导致波动，此参数用于描述此波动。 + workpieceParam.dirAngleScale = 25.0; //计算方向角的尺度 + workpieceParam.lineDeviation = 20.0;//直线偏离度。工作直线边缘由于扫描和切割导致波动，此参数用于描述此波动。 workpieceParam.minCutAngleTh = 15; //最小截角。由于扫描不全导致的截角，当截角过小时无法区分真下的截角和由于点波动导致的角度波动 int errCode = 0; diff --git a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.vcxproj b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.vcxproj index 3fe390e..fe320cf 100644 --- a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.vcxproj +++ b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.vcxproj @@ -24,6 +24,7 @@ {cf563709-0402-447e-bfcc-7701cc90d0af} BQworkpieceCornerExtracttest 10.0 + BQ_workpieceCornerExtraction_test diff --git a/BQ_workpieceCornerExtraction/BQ_workpieceCornerExtraction.vcxproj b/BQ_workpieceCornerExtraction/BQ_workpieceCornerExtraction.vcxproj index f989737..5714048 100644 --- a/BQ_workpieceCornerExtraction/BQ_workpieceCornerExtraction.vcxproj +++ b/BQ_workpieceCornerExtraction/BQ_workpieceCornerExtraction.vcxproj @@ -48,7 +48,7 @@ DynamicLibrary false - v142 + v143 true Unicode diff --git a/beltTearingDetection_test/beltTearingDetection_test.cpp b/beltTearingDetection_test/beltTearingDetection_test.cpp index 4f0e4bb..722ac5b 100644 --- a/beltTearingDetection_test/beltTearingDetection_test.cpp +++ b/beltTearingDetection_test/beltTearingDetection_test.cpp @@ -942,7 +942,7 @@ void _outputObjInfo(char* fileName, std::vector& total_tear sw.close(); } -#define TEST_GROUP 3 +#define TEST_GROUP 4 int main() { #if 0 @@ -1043,13 +1043,14 @@ int main() #else const char* dataPath[TEST_GROUP] = { - "F:\\ShangGu\\皮带撕裂点云\\1-500-2000\\", //0 - "F:\\ShangGu\\皮带撕裂点云\\2-1700-2000\\", //1 - "F:\\ShangGu\\皮带撕裂点云\\SaveData\\" + "F:/ShangGu/皮带撕裂点云/1-500-2000/", //0 + "F:/ShangGu/皮带撕裂点云/2-1700-2000/", //1 + "F:/ShangGu/皮带撕裂点云/SaveData/", //2 + "F:/ShangGu/皮带撕裂点云/20260316_中信重工/", //3 }; SVzNLRange fileIdx[TEST_GROUP] = { - {10,24}, {2, 18}, {16,16} }; + {10,24}, {2, 18}, {16,16}, {1,2} }; double camPoseR[9] = { 1.0, 0.0, 0.0, @@ -1067,7 +1068,7 @@ int main() char _scan_file[256]; int endGroup = TEST_GROUP - 1; - for (int grp = 2; grp <= endGroup; grp++) + for (int grp = 3; grp <= endGroup; grp++) { for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { diff --git a/sourceCode/BQ_workpieceCornerExtraction.cpp b/sourceCode/BQ_workpieceCornerExtraction.cpp index 2c9b0b4..6c35ddf 100644 --- a/sourceCode/BQ_workpieceCornerExtraction.cpp +++ b/sourceCode/BQ_workpieceCornerExtraction.cpp @@ -10,7 +10,8 @@ //version 1.2.1 : fix bugs for ver1.2.0 //version 1.3.0 : 㷨ͬһbrachж˾ж //version 1.4.0 : ʹսжĿ -std::string m_strVersion = "1.4.0"; +//version 1.4.1 : (1)ʹþ෽Ѱ׼ȷ㣨2ʹùսǺRֵжĿ +std::string m_strVersion = "1.4.1"; const char* wd_BQWorkpieceCornerVersion(void) { return m_strVersion.c_str(); @@ -34,7 +35,7 @@ void sx_BQ_lineDataR( lineDataRT_vector(a_line, camPoseR, groundH); } -SVzNL3DPoint _translatePoint(SVzNL3DPoint point, double rMatrix[9]) +SVzNL3DPoint _translatePoint(SVzNL3DPoint point, const double rMatrix[9]) { SVzNL3DPoint result; double x = point.x * rMatrix[0] + point.y * rMatrix[1] + point.z * rMatrix[2]; @@ -594,6 +595,8 @@ void _computeClosedPntListDirCorners(std::vector& polarPoints, doub dirCornerAngles.resize(pntSize); for (int i = 0; i < pntSize; i++) { + if (i == 68) + int kkk = 1; memset(&dirCornerAngles[i], 0, sizeof(SSG_dirCornerAngle)); SWD_polarPt& a_polarPt = polarPoints[i]; //ǰѰ @@ -635,6 +638,24 @@ void _computeClosedPntListDirCorners(std::vector& polarPoints, doub corner += 360; else if (corner > 180) corner = corner - 360; + + dirCornerAngles[i].flag = 0; + //˵ڼǺͷλµcorner + double polarAngle = dirCornerAngles[i].point.angle; + double angleDiff1 = abs(polarAngle - dirCornerAngles[i].forwardAngle); + if (angleDiff1 > 180) + angleDiff1 = 360 - angleDiff1; + double angleDiff2 = abs(polarAngle - dirCornerAngles[i].backwardAngle); + if (angleDiff2 > 180) + angleDiff2 = 360 - angleDiff2; + if ((angleDiff1 < 10) || (angleDiff2 < 10)) //˵뼫߷µIJ + { + if (dirCornerAngles[i].corner > 160) //Ƕȴ + { + dirCornerAngles[i].flag = -1; + } + } + dirCornerAngles[i].corner = corner; //ͼϵϵy෴С- dirCornerAngles[i].pntIdx = i; dirCornerAngles[i].forward_pntIdx = plus_i; @@ -647,6 +668,7 @@ void _computeClosedPntListDirCorners(std::vector& polarPoints, doub //ȡcornerֵʵֺʹô˺дŻ void _searchPlusCornerPeaks( std::vector< SSG_dirCornerAngle>& corners, + double cutAngleTh, std::vector< SSG_dirCornerAngle>& cornerPlusPeaks ) { @@ -660,9 +682,12 @@ void _searchPlusCornerPeaks( SSG_dirCornerAngle* pre_data = NULL; for (int i = 0, i_max = cornerSize; i < i_max; i++) { - if (i == 275) + if (i == 451) int kkk = 1; SSG_dirCornerAngle* curr_data = &corners[i]; + if (curr_data->flag < 0) + continue; + if (NULL == pre_data) { sEdgePtIdx = i; @@ -689,7 +714,7 @@ void _searchPlusCornerPeaks( case 1: // if (cornerDiff < 0) //½ { - if (pre_data->corner > 10) //ʹ10Ϊޣ˳㲨µĽǶȱ仯 + if (pre_data->corner > cutAngleTh) //ؽޣ˳㲨µĽǶȱ仯 peakCorners.push_back(*pre_data); _state = 2; } @@ -767,8 +792,29 @@ void _searchPlusCornerPeaks( double corner_2 = corners[a_dirAngle.backward_pntIdx].corner; double diff_1 = corner_curr - corner_1; double diff_2 = corner_curr - corner_2; - if ((diff_1 > corner_curr / 2) && (diff_2 > corner_curr / 2)) - cornerPlusPeaks.push_back(a_dirAngle); + if ((diff_1 > corner_curr / 4) && (diff_2 > corner_curr / 4)) + { + double polarAngle = a_dirAngle.point.angle; + double angleDiff1 = abs(polarAngle - a_dirAngle.forwardAngle); + if (angleDiff1 > 180) + angleDiff1 = 360 - angleDiff1; + double angleDiff2 = abs(polarAngle - a_dirAngle.backwardAngle); + if(angleDiff2 > 180) + angleDiff2 = 360 - angleDiff2; + if ((angleDiff1 < 10) || (angleDiff2 < 10)) //˵뼫߷µIJ + { + if (a_dirAngle.corner < 160) //Ƕȴ + { + a_dirAngle.flag = 0; + cornerPlusPeaks.push_back(a_dirAngle); + } + } + else + { + a_dirAngle.flag = 0; + cornerPlusPeaks.push_back(a_dirAngle); + } + } } } } @@ -777,14 +823,32 @@ void _searchPlusCornerPeaks( bool checkIsLine( SSG_dirCornerAngle& pt1, SSG_dirCornerAngle& pt2, std::vector& polarPoints, - double angleTh, double cutLineDeviationTh) + double cutLineDeviationTh, + double* maxDevaion) { int ringBuffSize = (int)polarPoints.size(); + + int dist1 = pt2.pntIdx - pt1.pntIdx; + if(dist1 <0) + dist1 = dist1 + ringBuffSize; + + int dist2 = pt2.backward_pntIdx - pt1.forward_pntIdx; + if (dist2 < 0) + dist2 = dist2 + ringBuffSize; + int idx1, idx2; + if ( dist2 > dist1) //޷ܿһ߶ + { + idx1 = pt1.pntIdx; + idx2 = pt2.pntIdx; + } + else + { + idx1 = pt1.forward_pntIdx; //ܿһ߶ + idx2 = pt2.backward_pntIdx; //ܿһ߶ + } //ƫж -// + // double a, b, c; - int idx1 = pt1.forward_pntIdx; //ܿһ߶ - int idx2 = pt2.backward_pntIdx; //ܿһ߶ compute2ptLine_2( polarPoints[idx1].x, polarPoints[idx1].y, polarPoints[idx2].x, polarPoints[idx2].y, @@ -805,28 +869,773 @@ bool checkIsLine( if (maxDist < dist) maxDist = dist; } + *maxDevaion = maxDist; if (maxDist < cutLineDeviationTh) //жΪֱ return true; else return false; } +//ݸյ㣬BranchInfo +void computeBranchInfo( + SSG_dirCornerAngle& corner_1, SSG_dirCornerAngle& corner_2, + std::vector& polarPoints, + std::vector< SSG_dirCornerAngle>& cornerPlusPeaks, + const SSX_BQworkpiecePara& workpieceParam, +#if _OUTPUT_DEBUG_DATA + SSX_debugInfo& a_branchDebug, +#endif + SWD_branchInfo& a_branchInfo +) +{ + int ringBuffSize = (int)polarPoints.size(); + //ֱ + // ֱ0ǵֱ֮ + std::vector linePts_0; + int ptSize = corner_2.backward_pntIdx - corner_1.forward_pntIdx; + if (ptSize < 0) + ptSize += ringBuffSize; + for (int m = 0; m <= ptSize; m++) + { + int idx = (corner_1.forward_pntIdx + m) % ringBuffSize; + SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z }; + linePts_0.push_back(a_pt); + } + double _a0 = 0, _b0 = 0, _c0 = 0; + lineFitting_abc(linePts_0, &_a0, &_b0, &_c0); + //ֱ1 + std::vector linePts_1; + int startIdx = corner_1.backward_pntIdx; + SWD_polarPt startPt = polarPoints[startIdx]; + for (int m = 0; m < ringBuffSize; m++) + { + int idx = startIdx - m; + if (idx < 0) + idx += ringBuffSize; + + double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2)); + if (dist > workpieceParam.lineLen) + break; + SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z }; + linePts_1.push_back(a_pt); + } + double _a1 = 0, _b1 = 0, _c1 = 0; + lineFitting_abc(linePts_1, &_a1, &_b1, &_c1); + //ֱ2 + std::vector linePts_2; + startIdx = corner_2.forward_pntIdx; + startPt = polarPoints[startIdx]; + for (int m = 0; m < ringBuffSize; m++) + { + int idx = (startIdx + m) % ringBuffSize; + double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2)); + if (dist > workpieceParam.lineLen) + break; + SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z }; + linePts_2.push_back(a_pt); + } + double _a2 = 0, _b2 = 0, _c2 = 0; + lineFitting_abc(linePts_2, &_a2, &_b2, &_c2); + +#if _OUTPUT_DEBUG_DATA + a_branchDebug.rgnIdx = 0; + a_branchDebug.edge_size = (int)linePts_0.size(); + a_branchDebug.edgeLink1_size = (int)linePts_1.size(); + a_branchDebug.edgeLink2_size = (int)linePts_2.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); + + for (int m = 0; m < a_branchDebug.edge_size; m++) + a_branchDebug.edge[m] = linePts_0[m]; + for (int m = 0; m < a_branchDebug.edgeLink1_size; m++) + a_branchDebug.edgeLink_1[m] = linePts_1[m]; + for (int m = 0; m < a_branchDebug.edgeLink2_size; m++) + a_branchDebug.edgeLink_2[m] = linePts_2[m]; +#endif + + linePts_1.insert(linePts_1.end(), linePts_0.begin(), linePts_0.end()); + linePts_2.insert(linePts_2.end(), linePts_0.begin(), linePts_0.end()); + //branchϢ + a_branchInfo.corner[0] = computeLineCrossPt_abs(_a0, _b0, _c0, _a1, _b1, _c1); + a_branchInfo.corner[0].z = computeMeanZ(linePts_1); + a_branchInfo.corner[2] = computeLineCrossPt_abs(_a0, _b0, _c0, _a2, _b2, _c2); + a_branchInfo.corner[2].z = computeMeanZ(linePts_2); + 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 = computeMeanZ(linePts_0); + // + a_branchInfo.angle = corner_1.point.angle; + a_branchInfo.line_a = _b0; + a_branchInfo.line_b = -_a0; + a_branchInfo.line_c = -(a_branchInfo.line_a * a_branchInfo.corner[1].x + a_branchInfo.line_b * a_branchInfo.corner[1].y); + + return; +} + +//нDzȫȫĹսǵ +SVzNL3DPoint _fittingCutAngle( + SSG_dirCornerAngle& corner_1, SSG_dirCornerAngle& corner_2, + std::vector& polarPoints, + const SSX_BQworkpiecePara& workpieceParam +) +{ + int ringBuffSize = (int)polarPoints.size(); + //ϼ㽻 + //ֱ1 + std::vector linePts_1; + int startIdx = corner_1.backward_pntIdx; + SWD_polarPt startPt = polarPoints[startIdx]; + for (int m = 0; m < ringBuffSize; m++) + { + int idx = startIdx - m; + if (idx < 0) + idx += ringBuffSize; + + double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2)); + if (dist > workpieceParam.lineLen) + break; + SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z }; + linePts_1.push_back(a_pt); + } + double _a1 = 0, _b1 = 0, _c1 = 0; + lineFitting_abc(linePts_1, &_a1, &_b1, &_c1); + //ֱ2 + std::vector linePts_2; + startIdx = corner_2.forward_pntIdx; + startPt = polarPoints[startIdx]; + for (int m = 0; m < ringBuffSize; m++) + { + int idx = (startIdx + m) % ringBuffSize; + double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2)); + if (dist > workpieceParam.lineLen) + break; + SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z }; + linePts_2.push_back(a_pt); + } + double _a2 = 0, _b2 = 0, _c2 = 0; + lineFitting_abc(linePts_2, &_a2, &_b2, &_c2); + //󽻵 + SVzNL3DPoint crossPt = computeLineCrossPt_abs(_a1, _b1, _c1, _a2, _b2, _c2); + linePts_1.insert(linePts_1.end(), linePts_2.begin(), linePts_2.end()); + crossPt.z = computeMeanZ(linePts_1); + return crossPt; +} + +int _searchClosestCorner(int searchSeed, std::vector< SSG_dirCornerAngle>& cornerPlusPeaks) +{ + int minDist = -1; + int minIdx = -1; + for (int i = 0; i < (int)cornerPlusPeaks.size(); i++) + { + if (cornerPlusPeaks[i].flag > 0) + continue; + + int dist = abs(cornerPlusPeaks[i].pntIdx - searchSeed); + if (minDist < 0) + { + minDist = dist; + minIdx = i; + } + else + { + if (minDist > dist) + { + minDist = dist; + minIdx = i; + } + } + } + return minIdx; +} + +//curr_corner +bool _fittingCutAngleBranch_1( + std::vector& polarPoints, + std::vector< SSG_dirCornerAngle>& cornerPlusPeaks, + SSG_dirCornerAngle& curr_corner, + SSG_dirCornerAngle& corner_1, + SSG_dirCornerAngle& corner_2, + const SSX_BQworkpiecePara& workpieceParam, + const double branchAngle, + const double angleTh, double cutLineDeviationTh, +#if _OUTPUT_DEBUG_DATA + SSX_debugInfo& a_branchDebug, +#endif + SWD_branchInfo& a_branchInfo) +{ + int ringBuffSize = (int)polarPoints.size(); + //ж3нbranchǰһcornerforwardһcornerbackwardȣĹսǺһ + double angle_diff1 = abs(curr_corner.forwardAngle - corner_1.backwardAngle); + if (angle_diff1 > 180) + angle_diff1 = 360 - angle_diff1; + double angle_diff2 = abs(corner_1.forwardAngle - corner_2.backwardAngle); + if (angle_diff2 > 180) + angle_diff2 = 360 - angle_diff2; + double angle_diff3 = abs(branchAngle - corner_1.corner - corner_2.corner); + if ((angle_diff1 < angleTh) && (angle_diff2 < angleTh) && (angle_diff3 < angleTh*2.0) && + (corner_1.flag != 1) && (corner_2.flag != 1)&&(corner_1.pntIdx>=0) &&(corner_2.pntIdx>=0)) + { + //ƫж + //ж֮Ƿһֱ + double maxDeviation = 0; + bool isLine = checkIsLine(corner_1, corner_2, polarPoints, cutLineDeviationTh, &maxDeviation); + if (true == isLine) //жΪؽ + { + SVzNL3DPoint cutCornerPt = _fittingCutAngle(corner_1, corner_2, polarPoints, workpieceParam); + corner_1.point.x = cutCornerPt.x; + corner_1.point.y = cutCornerPt.y; + corner_1.point.z = cutCornerPt.z; + corner_2.pntIdx = -1; + //corner + corner_1.corner = corner_1.corner + corner_2.corner; + corner_1.forward_pntIdx = corner_2.forward_pntIdx; + + //branchInfo + //ݸյ㣬BranchInfo + computeBranchInfo( + curr_corner, corner_1, + polarPoints, + cornerPlusPeaks, + workpieceParam, +#if _OUTPUT_DEBUG_DATA + a_branchDebug, +#endif + a_branchInfo + ); + return true; + } + } + else if (corner_1.flag != 1)//жнbranchʱںܽȱһcorner + { + double anti_dir = curr_corner.backwardAngle; + if (anti_dir > 0) + anti_dir = anti_dir - 180; + else if (anti_dir < 0) + anti_dir = anti_dir + 180; + + angle_diff1 = abs(anti_dir - corner_1.forwardAngle); + if (angle_diff1 > 180) + angle_diff1 = 360 - angle_diff1; + + angle_diff2 = abs(curr_corner.forwardAngle - corner_1.backwardAngle); + if (angle_diff2 > 180) + angle_diff2 = 360 - angle_diff2; + if ( ((angle_diff1 < angleTh * 2)|| (angle_diff2 < angleTh)) && (corner_1.pntIdx>=0)) + { + //ж֮Ƿһֱ + double maxDeviation = 0; + bool isLine = checkIsLine( curr_corner, corner_1, polarPoints, cutLineDeviationTh*2, &maxDeviation); + if (true == isLine) //жΪؽ + { + //ֱӼbrachInfocurr_cornerһΪ׼ + // ֱ1ӦcurrCornerbackward + std::vector linePts_1; + int startIdx = curr_corner.backward_pntIdx; + SWD_polarPt startPt = polarPoints[startIdx]; + for (int m = 0; m < ringBuffSize; m++) + { + int idx = startIdx - m; + if (idx < 0) + idx += ringBuffSize; + + double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2)); + if (dist > workpieceParam.lineLen) + break; + SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z }; + linePts_1.push_back(a_pt); + } + double _a1 = 0, _b1 = 0, _c1 = 0; + lineFitting_abc(linePts_1, &_a1, &_b1, &_c1); + //ֱ2: ӦcurrCornerfordward + std::vector linePts_2; + startIdx = corner_1.forward_pntIdx; + startPt = polarPoints[startIdx]; + for (int m = 0; m < ringBuffSize; m++) + { + int idx = (startIdx + m)% ringBuffSize; + + double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2)); + if (dist > workpieceParam.lineLen) + break; + SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z }; + linePts_2.push_back(a_pt); + } + double _a2 = 0, _b2 = 0, _c2 = 0; + lineFitting_abc(linePts_2, &_a2, &_b2, &_c2); + //ֱ1 curr_cornerĹյֱ0ֱ + double _a0 = 0, _b0 = 0, _c0 = 0; + _a0 = _b1; + _b0 = -_a1; + _c0 = -(_a0 * curr_corner.point.x + _b0 * curr_corner.point.y); + // +#if _OUTPUT_DEBUG_DATA + a_branchDebug.rgnIdx = 0; + a_branchDebug.edgeLink1_size = (int)linePts_1.size(); + a_branchDebug.edgeLink2_size = (int)linePts_2.size(); + a_branchDebug.edge_size = 0; + a_branchDebug.edge = NULL; + a_branchDebug.edgeLink_1 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink1_size); + a_branchDebug.edgeLink_2 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink2_size); + for (int m = 0; m < a_branchDebug.edgeLink1_size; m++) + a_branchDebug.edgeLink_1[m] = linePts_1[m]; + for (int m = 0; m < a_branchDebug.edgeLink2_size; m++) + a_branchDebug.edgeLink_2[m] = linePts_2[m]; +#endif + //branchϢ + a_branchInfo.corner[0] = { curr_corner.point.x, curr_corner.point.y, curr_corner.point.z }; + a_branchInfo.corner[2] = computeLineCrossPt_abs(_a0, _b0, _c0, _a2, _b2, _c2); + a_branchInfo.corner[2].z = computeMeanZ(linePts_2); + 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.corner[2].z)/2; + // + a_branchInfo.angle = curr_corner.point.angle; + a_branchInfo.line_a = _b0; + a_branchInfo.line_b = -_a0; + a_branchInfo.line_c = -(a_branchInfo.line_a * a_branchInfo.corner[1].x + a_branchInfo.line_b * a_branchInfo.corner[1].y); + return true; + } + } + } + return false; +} + +//curr_cornerķ +bool _fittingCutAngleBranch_2( + std::vector& polarPoints, + std::vector< SSG_dirCornerAngle>& cornerPlusPeaks, + SSG_dirCornerAngle& curr_corner, + SSG_dirCornerAngle& corner_1, + SSG_dirCornerAngle& corner_2, + const SSX_BQworkpiecePara& workpieceParam, + const double branchAngle, + const double angleTh, double cutLineDeviationTh, +#if _OUTPUT_DEBUG_DATA + SSX_debugInfo& a_branchDebug, +#endif + SWD_branchInfo& a_branchInfo) +{ + int ringBuffSize = (int)polarPoints.size(); + //ж3нbranchǰһcornerforwardһcornerbackwardȣĹսǺһ + double angle_diff1 = abs(curr_corner.backwardAngle - corner_1.forwardAngle); + if (angle_diff1 > 180) + angle_diff1 = 360 - angle_diff1; + double angle_diff2 = abs(corner_1.backwardAngle - corner_2.forwardAngle); + if (angle_diff2 > 180) + angle_diff2 = 360 - angle_diff2; + double angle_diff3 = abs(branchAngle - corner_1.corner - corner_2.corner); + if ((angle_diff1 < angleTh) && (angle_diff2 < angleTh) && (angle_diff3 < angleTh * 2) && + (corner_1.flag != 1) && (corner_2.flag != 1)&&(corner_1.pntIdx>=0) &&(corner_2.pntIdx>=0)) + { + //ƫж + //ж֮Ƿһֱ + double maxDeviation = 0; + bool isLine = checkIsLine(corner_2, corner_1, polarPoints, cutLineDeviationTh, &maxDeviation); + if (true == isLine) //жΪؽ + { + SVzNL3DPoint cutCornerPt = _fittingCutAngle(corner_2, corner_1, polarPoints, workpieceParam); + corner_1.point.x = cutCornerPt.x; + corner_1.point.y = cutCornerPt.y; + corner_1.point.z = cutCornerPt.z; + corner_2.pntIdx = -1; + //corner + corner_1.corner = corner_1.corner + corner_2.corner; + corner_1.backward_pntIdx = corner_2.backward_pntIdx; + + //branchInfo + //ݸյ㣬BranchInfo + computeBranchInfo( + corner_1, curr_corner, + polarPoints, + cornerPlusPeaks, + workpieceParam, +#if _OUTPUT_DEBUG_DATA + a_branchDebug, +#endif + a_branchInfo + ); + return true; + } + } + else if (corner_1.flag != 1)//жнbranchʱںܽȱһcorner + { + double anti_dir = curr_corner.forwardAngle; + if (anti_dir > 0) + anti_dir = anti_dir - 180; + else if (anti_dir < 0) + anti_dir = anti_dir + 180; + + angle_diff1 = abs(anti_dir - corner_1.backwardAngle); + if (angle_diff1 > 180) + angle_diff1 = 360 - angle_diff1; + + angle_diff2 = abs(curr_corner.backwardAngle - corner_1.forwardAngle); + if (angle_diff2 > 180) + angle_diff2 = 360 - angle_diff2; + if ( ((angle_diff1 < angleTh * 2)||(angle_diff2 < angleTh)) && (corner_1.pntIdx >= 0)) + { + //ж֮Ƿһֱ + double maxDeviation = 0; + bool isLine = checkIsLine(corner_1, curr_corner, polarPoints, cutLineDeviationTh * 2, &maxDeviation); + if (true == isLine) //жΪؽ + { + //ֱӼbrachInfocurr_cornerһΪ׼ + // ֱ1ӦcurrCornerbackward + std::vector linePts_1; + int startIdx = curr_corner.forward_pntIdx; + SWD_polarPt startPt = polarPoints[startIdx]; + for (int m = 0; m < ringBuffSize; m++) + { + int idx = (startIdx + m) % ringBuffSize; + double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2)); + if (dist > workpieceParam.lineLen) + break; + SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z }; + linePts_1.push_back(a_pt); + } + double _a1 = 0, _b1 = 0, _c1 = 0; + lineFitting_abc(linePts_1, &_a1, &_b1, &_c1); + //ֱ2: ӦcurrCornerfordward + std::vector linePts_2; + startIdx = corner_1.backward_pntIdx; + startPt = polarPoints[startIdx]; + for (int m = 0; m < ringBuffSize; m++) + { + int idx = startIdx - m; + if (idx < 0) + idx += ringBuffSize; + double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2)); + if (dist > workpieceParam.lineLen) + break; + SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z }; + linePts_2.push_back(a_pt); + } + double _a2 = 0, _b2 = 0, _c2 = 0; + lineFitting_abc(linePts_2, &_a2, &_b2, &_c2); + //ֱ1 curr_cornerĹյֱ0ֱ + double _a0 = 0, _b0 = 0, _c0 = 0; + _a0 = _b1; + _b0 = -_a1; + _c0 = -(_a0 * curr_corner.point.x + _b0 * curr_corner.point.y); + // +#if _OUTPUT_DEBUG_DATA + a_branchDebug.rgnIdx = 0; + a_branchDebug.edgeLink1_size = (int)linePts_1.size(); + a_branchDebug.edgeLink2_size = (int)linePts_2.size(); + a_branchDebug.edge_size = 0; + a_branchDebug.edge = NULL; + a_branchDebug.edgeLink_1 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink1_size); + a_branchDebug.edgeLink_2 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink2_size); + for (int m = 0; m < a_branchDebug.edgeLink1_size; m++) + a_branchDebug.edgeLink_1[m] = linePts_1[m]; + for (int m = 0; m < a_branchDebug.edgeLink2_size; m++) + a_branchDebug.edgeLink_2[m] = linePts_2[m]; +#endif + //branchϢ + a_branchInfo.corner[2] = { curr_corner.point.x, curr_corner.point.y, curr_corner.point.z }; + a_branchInfo.corner[0] = computeLineCrossPt_abs(_a0, _b0, _c0, _a2, _b2, _c2); + a_branchInfo.corner[0].z = computeMeanZ(linePts_2); + 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.corner[2].z) / 2; + // + a_branchInfo.angle = curr_corner.point.angle; + a_branchInfo.line_a = _b0; + a_branchInfo.line_b = -_a0; + a_branchInfo.line_c = -(a_branchInfo.line_a * a_branchInfo.corner[1].x + a_branchInfo.line_b * a_branchInfo.corner[1].y); + return true; + } + } + } + return false; +} + +bool compareByBranchAngle(const SWD_branchInfo& a, const SWD_branchInfo& b) { + return a.angle < b.angle; +} + //ĽǵBranch жǷȱǡȱʱͨϷʽȫ // std::vector& polarPoints: ѭ +// polarRPeakInfo: Rֵ +// cornerPlusPeaks cornerֵ㡣RֵcornerֵͬһλʱΪЧĹյ //branchCornerAngle: ֧ѰҲͬǶȵBranchbranchCornerAngleָ90Ϊֱǡ //angleTh: ǶΧΪ90ֱǣʵʼӹ //cutLineDeviationTh:Խؽߣؽϵ㵽ֱߵƫ롣֤ؽֱ void _computeBranchesFromCornerPeaks( std::vector& polarPoints, + std::vector& polarRPeakInfo, std::vector< SSG_dirCornerAngle>& cornerPlusPeaks, + const SSX_BQworkpiecePara& workpieceParam, std::vector& branches, - SSX_BQworkpiecePara& workpieceParam, #if _OUTPUT_DEBUG_DATA std::vector& branchDebugData, #endif - double branchCornerAngle, double angleTh, double cutLineDeviationTh) + double branchCornerAngle, double angleTh, double cutLineDeviationTh +) { int ringBuffSize = (int)polarPoints.size(); + + //RֵΪжϹյǷȱ + //ȽRֵCornerֵӦļֵѡֵΪŶߵĹյ + std::vector realCorners; + double samePosDistTh = 20.0; //Rֵcornerֵλͬһľ + int samePosIdxDistTh = 15; + for (int j = 0; j < (int)polarRPeakInfo.size(); j++) + { + if (polarRPeakInfo[j].flag > 0) + continue; + + double minDist = -1; + int minIdx = -1; + + for (int i = 0; i < (int)cornerPlusPeaks.size(); i++) + { + if (cornerPlusPeaks[i].flag > 0) + continue; + + //ֵ롣С5mmΪյ + double dist = sqrt(pow(cornerPlusPeaks[i].point.x - polarRPeakInfo[j].point.x, 2) + + pow(cornerPlusPeaks[i].point.y - polarRPeakInfo[j].point.y, 2)); + if (minDist < 0) + { + minDist = dist; + minIdx = i; + } + else if (minDist > dist) + { + minDist = dist; + minIdx = i; + } + } + int idx_diff = abs(cornerPlusPeaks[minIdx].pntIdx - polarRPeakInfo[j].cptIndex); + if ( (minDist < samePosDistTh) || (idx_diff < samePosIdxDistTh)) + { + double angleDiff = abs(cornerPlusPeaks[minIdx].corner - branchCornerAngle); + if (angleDiff < angleTh) + { + cornerPlusPeaks[minIdx].flag = 1; + polarRPeakInfo[j].flag = 1; + } + else + { + if (cornerPlusPeaks[minIdx].corner > branchCornerAngle) + { + cornerPlusPeaks[minIdx].flag = 1; + polarRPeakInfo[j].flag = 1; + } + else + { + cornerPlusPeaks[minIdx].flag = 2; + polarRPeakInfo[j].flag = 2; + } + } + SSG_intPair a_pair; + a_pair.data_0 = minIdx; + a_pair.data_1 = j; + a_pair.idx = 0; + realCorners.push_back(a_pair); + } + } + + //жǷн + for (int i = 0; i < (int)realCorners.size(); i++) + { + int idx_corner = realCorners[i].data_0; + if (cornerPlusPeaks[idx_corner].flag != 2) + continue; + + SSG_dirCornerAngle& curr_corner = cornerPlusPeaks[idx_corner]; + //жǷн + //cornerȶԣǷΪн + int idx_1 = idx_corner - 1; + if (idx_1 < 0) + idx_1 = (int)cornerPlusPeaks.size() - 1; + SSG_dirCornerAngle& corner_1 = cornerPlusPeaks[idx_1]; + + int idx_2 = idx_corner + 1; + if (idx_2 >= (int)cornerPlusPeaks.size()) + idx_2 = 0; + SSG_dirCornerAngle& corner_2 = cornerPlusPeaks[idx_2]; + + double sum1 = curr_corner.corner + corner_1.corner; + double sum2 = curr_corner.corner + corner_2.corner; + double angleDiff_1 = abs(sum1 - branchCornerAngle); + double angleDiff_2 = abs(sum2 - branchCornerAngle); + + bool foundCutAngle = false; + if ( (angleDiff_1 < angleTh) &&(corner_1.flag ==0)&&(corner_1.pntIdx >=0)) + { + //ƫж + //ж֮Ƿһֱ + double maxDeviation = 0; + bool isLine = checkIsLine(corner_1, curr_corner, polarPoints, cutLineDeviationTh, &maxDeviation); + if (true == isLine) //жΪؽ + { + SVzNL3DPoint cutCornerPt = _fittingCutAngle(corner_1, curr_corner, polarPoints, workpieceParam); + curr_corner.point.x = cutCornerPt.x; + curr_corner.point.y = cutCornerPt.y; + curr_corner.point.z = cutCornerPt.z; + corner_1.pntIdx = -1; + //corner + curr_corner.corner = sum1; + curr_corner.backward_pntIdx = corner_1.backward_pntIdx; + foundCutAngle = true; + cornerPlusPeaks[idx_corner].flag = 1; + polarRPeakInfo[realCorners[i].data_1].flag = 1; + } + } + else if ( (angleDiff_2 < angleTh) &&(false == foundCutAngle) && (corner_2.flag == 0) && (corner_2.pntIdx >= 0)) + { + //ƫж + //ж֮Ƿһֱ + double maxDeviation = 0; + bool isLine = checkIsLine(curr_corner, corner_2, polarPoints, cutLineDeviationTh, &maxDeviation); + if (true == isLine) //жΪؽ + { + SVzNL3DPoint cutCornerPt = _fittingCutAngle(curr_corner, corner_2, polarPoints, workpieceParam); + //ϼ㽻 + curr_corner.point.x = cutCornerPt.x; + curr_corner.point.y = cutCornerPt.y; + curr_corner.point.z = cutCornerPt.z; + corner_2.pntIdx = -1; + //corner + curr_corner.corner = sum2; + curr_corner.forward_pntIdx = corner_2.forward_pntIdx; + cornerPlusPeaks[idx_corner].flag = 1; + polarRPeakInfo[realCorners[i].data_1].flag = 1; + } + } + else + { + double angleDiff = abs(curr_corner.corner - branchCornerAngle); + if (angleDiff < angleTh * 1.5) + { + cornerPlusPeaks[idx_corner].flag = 1; + polarRPeakInfo[realCorners[i].data_1].flag = 1; + } + } + } + + //ûжϵRֵдRֵΪ׼λcornerΪ׼ + int size_peaks = (int)polarRPeakInfo.size(); + for (int i = 0; i < size_peaks; i++) + { + if (polarRPeakInfo[i].flag > 0) + continue; + + //corner + int nearestCorner = _searchClosestCorner(polarRPeakInfo[i].cptIndex, cornerPlusPeaks); + if (nearestCorner < 0) + continue; + + //ûн + SSG_dirCornerAngle& curr_corner = cornerPlusPeaks[nearestCorner]; + int tmpIdx_1 = nearestCorner - 1; + if (tmpIdx_1 < 0) + tmpIdx_1 = size_peaks - 1; + SSG_dirCornerAngle& corner_1 = cornerPlusPeaks[tmpIdx_1]; + + int tmpIdx_2 = (nearestCorner + 1) % size_peaks; + SSG_dirCornerAngle& corner_2 = cornerPlusPeaks[tmpIdx_2]; + + double sumCorner_1 = curr_corner.corner + corner_1.corner; + double angleDiff_1 = abs(sumCorner_1 - branchCornerAngle); + + double sumCorner_2 = curr_corner.corner + corner_2.corner; + double angleDiff_2 = abs(sumCorner_2 - branchCornerAngle); + + bool foundCutAngle = false; + if ((angleDiff_1 < angleTh) && (corner_1.flag == 0)&&(corner_1.pntIdx >=0)) + { + //ƫж + //ж֮Ƿһֱ + double maxDeviation = 0; + bool isLine = checkIsLine(corner_1, curr_corner, polarPoints, cutLineDeviationTh, &maxDeviation); + if (true == isLine) //жΪؽ + { + SVzNL3DPoint cutCornerPt = _fittingCutAngle(corner_1, curr_corner, polarPoints, workpieceParam); + curr_corner.point.x = cutCornerPt.x; + curr_corner.point.y = cutCornerPt.y; + curr_corner.point.z = cutCornerPt.z; + corner_1.pntIdx = -1; + //corner + curr_corner.corner = sumCorner_1; + curr_corner.backward_pntIdx = corner_1.backward_pntIdx; + foundCutAngle = true; + curr_corner.flag = 1; + } + + } + if ((angleDiff_2 < angleTh) && (corner_2.flag == 0)&&(corner_2.pntIdx>=0) && (false == foundCutAngle)) + { + //ƫж + //ж֮Ƿһֱ + double maxDeviation = 0; + bool isLine = checkIsLine(curr_corner, corner_2, polarPoints, cutLineDeviationTh, &maxDeviation); + if (true == isLine) //жΪؽ + { + SVzNL3DPoint cutCornerPt = _fittingCutAngle(curr_corner, corner_2, polarPoints, workpieceParam); + //ϼ㽻 + curr_corner.point.x = cutCornerPt.x; + curr_corner.point.y = cutCornerPt.y; + curr_corner.point.z = cutCornerPt.z; + corner_2.pntIdx = -1; + //corner + curr_corner.corner = sumCorner_2; + curr_corner.forward_pntIdx = corner_2.forward_pntIdx; + curr_corner.flag = 1; + } + } + } + +#if 0 + //branch, + int pairSize = (int)realCorners.size(); + for (int i = 0; i < (int)pairSize; i++) + { + int cornerPeakIdx_1 = realCorners[i].data_0; + if (cornerPeakIdx_1 < 0) + continue; + + SSG_dirCornerAngle& corner_1 = cornerPlusPeaks[cornerPeakIdx_1]; + int tmpIdx = (i + 1) % pairSize; + if (realCorners[tmpIdx].data_0) + continue; + + int cornerPeakIdx_2 = realCorners[tmpIdx].data_0; + SSG_dirCornerAngle& corner_2 = cornerPlusPeaks[cornerPeakIdx_2]; + //ж֮Ƿһֱ + bool isBranch = checkIsLine(corner_1, corner_2, polarPoints, cutLineDeviationTh); + if (true == isBranch) //жΪbranch + { + //ݸյ㣬BranchInfo +#if _OUTPUT_DEBUG_DATA + SSX_debugInfo a_branchDebug; +#endif + SWD_branchInfo a_branchInfo; + computeBranchInfo( + corner_1, corner_2, + polarPoints, + cornerPlusPeaks, + workpieceParam, +#if _OUTPUT_DEBUG_DATA + a_branchDebug, +#endif + a_branchInfo + ); + branches.push_back(a_branchInfo); +#if _OUTPUT_DEBUG_DATA + branchDebugData.push_back(a_branchDebug); +#endif + realCorners[i].data_0 = -1; + realCorners[tmpIdx].data_0 = -1; + } + } + + //ʣµûԵĹյд + + //жǷȱ int size_peaks = (int)cornerPlusPeaks.size(); for (int i = 0; i < size_peaks; i++) @@ -846,7 +1655,7 @@ void _computeBranchesFromCornerPeaks( { //ƫж //ж֮Ƿһֱ - bool isLine = checkIsLine( corner_1, corner_2, polarPoints, angleTh, cutLineDeviationTh); + bool isLine = checkIsLine( corner_1, corner_2, polarPoints, cutLineDeviationTh); if (true == isLine) //жΪؽ { //ϼ㽻 @@ -915,121 +1724,144 @@ void _computeBranchesFromCornerPeaks( } } } - +#endif //Ѱbranch: branch֮Ϊֱ size_peaks = (int)cornerPlusPeaks.size(); - int branchID = 1; + std::vector validCornerPeaks; for (int i = 0; i < size_peaks; i++) { - if (cornerPlusPeaks[i].pntIdx < 0) + if (cornerPlusPeaks[i].flag == 1) + validCornerPeaks.push_back(i); + } + int validPeak_size = (int)validCornerPeaks.size(); + int branchID = 1; + for (int i = 0; i < validPeak_size; i++) + { + int cornerIdx = validCornerPeaks[i]; + if (cornerPlusPeaks[cornerIdx].flag == 0) continue; - SSG_dirCornerAngle& corner_1 = cornerPlusPeaks[i]; - int tmpIdx = (i + 1) % size_peaks; - SSG_dirCornerAngle& corner_2 = cornerPlusPeaks[tmpIdx]; + SSG_dirCornerAngle& corner_1 = cornerPlusPeaks[cornerIdx]; + int tmpIdx = (i + 1) % validPeak_size; + int nxtCornerIdx = validCornerPeaks[tmpIdx]; + SSG_dirCornerAngle& corner_2 = cornerPlusPeaks[nxtCornerIdx]; //ж֮Ƿһֱ - bool isBranch = checkIsLine(corner_1, corner_2, polarPoints, angleTh, cutLineDeviationTh); + double maxDevaion = 0; + bool isBranch = false; + if( (corner_1.pntIdx >=0) && (corner_2.pntIdx >=0)) + isBranch = checkIsLine(corner_1, corner_2, polarPoints, cutLineDeviationTh, &maxDevaion); if (true == isBranch) //жΪbranch { - //ֱ - // ֱ0ǵֱ֮ - std::vector linePts_0; - int ptSize = corner_2.backward_pntIdx - corner_1.forward_pntIdx; - if (ptSize < 0) - ptSize += ringBuffSize; - for (int m = 0; m <= ptSize; m++) - { - int idx = (corner_1.forward_pntIdx + m) % ringBuffSize; - SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z }; - linePts_0.push_back(a_pt); - } - double _a0 = 0, _b0 = 0, _c0 = 0; - lineFitting_abc(linePts_0, &_a0, &_b0, &_c0); - //ֱ1 - std::vector linePts_1; - int startIdx = corner_1.backward_pntIdx; - SWD_polarPt startPt = polarPoints[startIdx]; - for (int m = 0; m < ringBuffSize; m++) - { - int idx = startIdx - m; - if (idx < 0) - idx += ringBuffSize; - - double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2)); - if (dist > workpieceParam.lineLen) - break; - SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z }; - linePts_1.push_back(a_pt); - } - double _a1 = 0, _b1 = 0, _c1 = 0; - lineFitting_abc(linePts_1, &_a1, &_b1, &_c1); - //ֱ2 - std::vector linePts_2; - startIdx = corner_2.forward_pntIdx; - startPt = polarPoints[startIdx]; - for (int m = 0; m < ringBuffSize; m++) - { - int idx = (startIdx + m) % ringBuffSize; - double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2)); - if (dist > workpieceParam.lineLen) - break; - SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z }; - linePts_2.push_back(a_pt); - } - double _a2 = 0, _b2 = 0, _c2 = 0; - lineFitting_abc(linePts_2, &_a2, &_b2, &_c2); - + //ݸյ㣬BranchInfo #if _OUTPUT_DEBUG_DATA SSX_debugInfo a_branchDebug; - a_branchDebug.rgnIdx = branchID++; - a_branchDebug.edge_size = (int)linePts_0.size(); - a_branchDebug.edgeLink1_size = (int)linePts_1.size(); - a_branchDebug.edgeLink2_size = (int)linePts_2.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); - - for (int m = 0; m < a_branchDebug.edge_size; m++) - a_branchDebug.edge[m] = linePts_0[m]; - for (int m = 0; m < a_branchDebug.edgeLink1_size; m++) - a_branchDebug.edgeLink_1[m] = linePts_1[m]; - for (int m = 0; m < a_branchDebug.edgeLink2_size; m++) - a_branchDebug.edgeLink_2[m] = linePts_2[m]; - +#endif + SWD_branchInfo a_branchInfo; + computeBranchInfo( + corner_1, corner_2, + polarPoints, + cornerPlusPeaks, + workpieceParam, +#if _OUTPUT_DEBUG_DATA + a_branchDebug, +#endif + a_branchInfo + ); + branches.push_back(a_branchInfo); +#if _OUTPUT_DEBUG_DATA branchDebugData.push_back(a_branchDebug); #endif - - linePts_1.insert(linePts_1.end(), linePts_0.begin(), linePts_0.end()); - linePts_2.insert(linePts_2.end(), linePts_0.begin(), linePts_0.end()); - //branchϢ - SWD_branchInfo a_branchInfo; - a_branchInfo.corner[0] = computeLineCrossPt_abs(_a0, _b0, _c0, _a1, _b1, _c1); - a_branchInfo.corner[0].z = computeMeanZ(linePts_1); - a_branchInfo.corner[2] = computeLineCrossPt_abs(_a0, _b0, _c0, _a2, _b2, _c2); - a_branchInfo.corner[2].z = computeMeanZ(linePts_2); - 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 = computeMeanZ(linePts_0); - // - a_branchInfo.angle = corner_1.point.angle; - a_branchInfo.line_a = _b0; - a_branchInfo.line_b = -_a0; - a_branchInfo.line_c = -(a_branchInfo.line_a * a_branchInfo.corner[1].x + a_branchInfo.line_b * a_branchInfo.corner[1].y); - branches.push_back(a_branchInfo); - - corner_1.pntIdx = -1; - corner_2.pntIdx = -1; + corner_1.flag = 0; + corner_2.flag = 0; } } + //ûγbranchInfoļֵ + //ڲԵRֵ㣬ԼֵΪ׼Ѱŵƽбߣcornerĺ෴ֵȣ + for (int i = 0; i < validPeak_size; i++) + { + int cornerIdx = validCornerPeaks[i]; + if (cornerPlusPeaks[cornerIdx].flag == 0) + continue; + + SSG_dirCornerAngle& curr_corner = cornerPlusPeaks[cornerIdx]; + //˳ + int tmpIdx = (cornerIdx + 1)% size_peaks; + SSG_dirCornerAngle corner_1 = cornerPlusPeaks[tmpIdx]; + tmpIdx = (cornerIdx + 2) % size_peaks; + SSG_dirCornerAngle corner_2 = cornerPlusPeaks[tmpIdx]; + //нbranch +#if _OUTPUT_DEBUG_DATA + SSX_debugInfo a_branchDebug; +#endif + SWD_branchInfo a_branchInfo; + bool foundBranch = _fittingCutAngleBranch_1( + polarPoints, + cornerPlusPeaks, + curr_corner, + corner_1, + corner_2, + workpieceParam, + branchCornerAngle, + angleTh, cutLineDeviationTh, +#if _OUTPUT_DEBUG_DATA + a_branchDebug, +#endif + a_branchInfo); + if (true == foundBranch) + { + branches.push_back(a_branchInfo); +#if _OUTPUT_DEBUG_DATA + branchDebugData.push_back(a_branchDebug); +#endif + } + else + { + // + tmpIdx = cornerIdx - 1; + if (tmpIdx < 0) + tmpIdx += size_peaks; + corner_1 = cornerPlusPeaks[tmpIdx]; + tmpIdx = cornerIdx - 2; + if(tmpIdx < 0) + tmpIdx += size_peaks; + corner_2 = cornerPlusPeaks[tmpIdx]; + //нbranch + + foundBranch = _fittingCutAngleBranch_2( + polarPoints, + cornerPlusPeaks, + curr_corner, + corner_1, + corner_2, + workpieceParam, + branchCornerAngle, + angleTh, cutLineDeviationTh, +#if _OUTPUT_DEBUG_DATA + a_branchDebug, +#endif + a_branchInfo); + if (true == foundBranch) + { + branches.push_back(a_branchInfo); +#if _OUTPUT_DEBUG_DATA + branchDebugData.push_back(a_branchDebug); +#endif + } + } + } + + // + std::sort(branches.begin(), branches.end(), compareByBranchAngle); + return; } SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( - std::vector< std::vector>& scanLines, + std::vector< std::vector>& scanLines_src, const SSG_cornerParam cornerPara, const SSG_outlierFilterParam filterParam, - SSG_treeGrowParam growParam, - SSG_planeCalibPara groundCalibPara, - SSX_BQworkpiecePara workpieceParam, + const SSG_treeGrowParam growParam, + const SSG_planeCalibPara groundCalibPara, + const SSX_BQworkpiecePara workpieceParam, #if _OUTPUT_DEBUG_DATA std::vector& debug_contours, #endif @@ -1038,13 +1870,18 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( *errCode = 0; SSX_BQworkpieceResult workpieceCorners; memset(&workpieceCorners, 0, sizeof(SSX_BQworkpieceResult)); - int lineNum = (int)scanLines.size(); + int lineNum = (int)scanLines_src.size(); if (lineNum == 0) { *errCode = SG_ERR_3D_DATA_NULL; return workpieceCorners; } + //ݽƽף,СԵϵIJµĽǶȼ㲨 + std::vector< std::vector> scanLines; + int fillingHoleSize = 3;//״С:С3Ŀ׶ + wd_pointCloudHoleFilling(scanLines_src, scanLines, fillingHoleSize); //״С + //ʼͽĿհɨȥɨ߽ int validStartLine = -1; for (int i = 0; i < lineNum; i++) @@ -1077,6 +1914,16 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( //ӦתǶ { + std::vector featurePoints; + //ȫFlag + std::vector> globalFlags; + globalFlags.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + globalFlags[i].resize(linePtNum); + std::fill(globalFlags[i].begin(), globalFlags[i].end(), 0); + } + //ֱȡ std::vector> jumpFeatures_v_raw; for (int line = 0; line < lineNum; line++) @@ -1101,6 +1948,21 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( cornerPara, //scaleͨȡbagH1/4 line_features); jumpFeatures_v_raw.push_back(line_features); + + int featureNum = (int)line_features.size(); + for (int j = 0; j < featureNum; j++) + { + int idx_line = line_features[j].jumpPos2D.x; + int idx_pt = line_features[j].jumpPos2D.y; + + SVzNL3DPosition a_featurePt = lineData[idx_pt]; + if (globalFlags[idx_line][idx_pt] == 0) + { + globalFlags[idx_line][idx_pt] = 1; + a_featurePt.nPointIdx = (idx_line << 16) | (idx_pt & 0x0000FFFF); + featurePoints.push_back(a_featurePt); + } + } } if (false == isGridData)//ݲʽ @@ -1124,6 +1986,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( 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(); @@ -1146,8 +2009,23 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( cornerPara, //scaleͨȡbagH1/4 line_features); jumpFeatures_h_raw.push_back(line_features); - } + int featureNum = (int)line_features.size(); + for (int j = 0; j < featureNum; j++) + { + int idx_line = line_features[j].jumpPos2D.y; + int idx_pt = line_features[j].jumpPos2D.x; + + SVzNL3DPosition a_featurePt = scanLines[idx_line][idx_pt]; + if (globalFlags[idx_line][idx_pt] == 0) + { + globalFlags[idx_line][idx_pt] = 1; + a_featurePt.nPointIdx = (idx_line << 16) | (idx_pt & 0x0000FFFF); + featurePoints.push_back(a_featurePt); + } + } + } +#if 0 //˳ //ֱ߷ std::vector v_trees; @@ -1188,52 +2066,6 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( } 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]; @@ -1281,7 +2113,44 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( } } } - +#else + //࣬ȡ + int clusterCheckWin = 8; // + std::vector> objClusters; + double clusterDist = sqrt(pow(growParam.yDeviation_max,2) + pow(growParam.maxSkipDistance, 2)); + wd_pointClustering_speedUp( + featurePoints, + lineNum, linePtNum, clusterCheckWin, // + clusterDist, + objClusters //result + ); + //ȡľΪ + int contourID = 0; + int contourPtNum = 0; + for (int i = 0; i < (int)objClusters.size(); i++) + { + if (contourPtNum < objClusters[i].size()) + { + contourPtNum = (int)objClusters[i].size(); + contourID = i; + } + } + std::vector polarPoints; + for (int i = 0, i_max = (int)objClusters[contourID].size(); i < i_max; i++) + { + int lineIdx = objClusters[contourID][i].nPointIdx >> 16; + int ptIdx = objClusters[contourID][i].nPointIdx & 0x0000FFFF; + 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(); @@ -1290,8 +2159,66 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( *errCode = SX_ERR_ZERO_CONTOUR_PT; return workpieceCorners; } + double center_x = 0; double center_y = 0; + double center_z = 0; +#if 1 //ļ + //ǣڽļ + for (int line = 0; line < lineNum; line++) + { + int start = -1; + for (int j = 0; j < linePtNum; j++) + { + if (scanLines[line][j].nPointIdx == -1) + { + if (start < 0) + start = j; + else + { + for (int m = start+1; m < j; m++) + scanLines[line][m].nPointIdx = -2; + start = -1; + } + } + } + } + for (int j = 0; j < linePtNum; j++) + { + int start = -1; + for (int line = 0; line < lineNum; line++) + { + if (scanLines[line][j].nPointIdx == -1) + { + if (start < 0) + start = line; + else + { + for (int m = start+1; m < line; m++) + scanLines[m][j].nPointIdx = -2; + start = -1; + } + } + } + } + int testNum = 0; + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if ( (scanLines[line][j].pt3D.z > 1e-4) && (scanLines[line][j].nPointIdx == -2)) + { + center_x += scanLines[line][j].pt3D.x; + center_y += scanLines[line][j].pt3D.y; + center_z += scanLines[line][j].pt3D.z; + testNum++; + } + } + } + center_x = center_x / (double)testNum; + center_y = center_y / (double)testNum; + center_z = center_z / (double)testNum; +#else // for (int pi = 0; pi < contourPtSize; pi++) { center_x += polarPoints[pi].x; @@ -1299,6 +2226,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( } center_x = center_x / (double)contourPtSize; center_y = center_y / (double)contourPtSize; +#endif //㼫RTheta for (int pi = 0; pi < contourPtSize; pi++) { @@ -1312,28 +2240,13 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( std::sort(polarPoints.begin(), polarPoints.end(), compareByAngle); for (int pi = 0; pi < contourPtSize; pi++) polarPoints[pi].cptIndex = pi; // index - + //ԵĽǶȱ仯 2026.03.06 std::vector< SSG_dirCornerAngle> dirCornerAngles; - double scale = 20.0; - _computeClosedPntListDirCorners(polarPoints, scale, dirCornerAngles); + _computeClosedPntListDirCorners(polarPoints, workpieceParam.dirAngleScale, dirCornerAngles); //ȡǹյ(ֵ std::vector< SSG_dirCornerAngle> cornerPlusPeaks; - _searchPlusCornerPeaks(dirCornerAngles, cornerPlusPeaks); - //ʹRˣжǷȱ - double branchCornerAngle = 90; - double angleTh = 10; - double cutLineDeviationTh = 5.0; - std::vector branches; - _computeBranchesFromCornerPeaks( - polarPoints, - cornerPlusPeaks, - branches, - workpieceParam, -#if _OUTPUT_DEBUG_DATA - debug_contours, -#endif - branchCornerAngle, angleTh, cutLineDeviationTh); + _searchPlusCornerPeaks(dirCornerAngles, workpieceParam.minCutAngleTh, cornerPlusPeaks); //ȡRֵ double minR = -1, maxR = -1; //СRûз֧minRmaxRСʱΪԲλ8Σûз֧ @@ -1376,12 +2289,17 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( int pkId = 0; //ԲεļֵƫƣԲҲγɼֵݼֵL=ֱ߶γȹɵŽж double arcAngleChkLen = 100; //ԲŽǵij߶ + double validPolarPeakZRange = 50; for (int i = 0, i_max = (int)polarRPeakPts.size(); i < i_max; i++) { int ptidx = polarRPeakPts[i].cptIndex; double px, py, pz; px = polarRPeakPts[i].x; py = polarRPeakPts[i].y; + pz = polarRPeakPts[i].z; + double z_diff = abs(center_z - pz); + if (z_diff > validPolarPeakZRange) //50mmжΪЧ + continue; int LL1 = -1; int halfLL1 = -1; @@ -1433,6 +2351,8 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( a_pkInfo.L2_ptIndex = LL2; a_pkInfo.cornerAngle = theta; a_pkInfo.cornerDir = 0; + a_pkInfo.point = polarRPeakPts[i]; + a_pkInfo.flag = 0; polarRPeakPts[i].cptIndex = ptidx; polarRPeakPts[i].pkId = pkId; pkId++; @@ -1442,6 +2362,21 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( } } + //branch + double branchCornerAngle = 90; + double angleTh = 10; + std::vector branches; + _computeBranchesFromCornerPeaks( + polarPoints, + polarPeakInfo, + cornerPlusPeaks, + workpieceParam, + branches, +#if _OUTPUT_DEBUG_DATA + debug_contours, +#endif + branchCornerAngle, angleTh, workpieceParam.lineDeviation); + int workpieceType = -1; bool partialScan = false; //ҰܸǹȫʱpartialScanΪtrue std::vector< SWD_branchInfo> branchInfo; @@ -1777,7 +2712,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( #if 1 //ͶԭϵԱ۱궨ȷ for (int i = 0; i < lineNum; i++) - sx_BQ_lineDataR(scanLines[i], groundCalibPara.invRMatrix, -1); + sx_BQ_lineDataR(scanLines_src[i], groundCalibPara.invRMatrix, -1); //Ͷԭϵ SVzNL3DPoint rawObj; for (int i = 0; i < 3; i++) diff --git a/sourceCode/BQ_workpieceCornerExtraction_Export.h b/sourceCode/BQ_workpieceCornerExtraction_Export.h index 4cce85f..6b6e143 100644 --- a/sourceCode/BQ_workpieceCornerExtraction_Export.h +++ b/sourceCode/BQ_workpieceCornerExtraction_Export.h @@ -8,6 +8,9 @@ typedef struct { double lineLen; //ֱ߶γ + double dirAngleScale; //㷽ǵij߶ + double lineDeviation;//ֱƫȡֱ߱Եɨи²˲˲ + double minCutAngleTh; //Сؽǡɨ費ȫµĽؽǣؽǹСʱ޷µĽؽǺڵ㲨µĽǶȲ }SSX_BQworkpiecePara; typedef struct @@ -53,12 +56,12 @@ SG_APISHARED_EXPORT void sx_BQ_lineDataR( //ȡǵ㼰λϢ SG_APISHARED_EXPORT SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( - std::vector< std::vector>& scanLines, + std::vector< std::vector>& scanLines_src, const SSG_cornerParam cornerPara, const SSG_outlierFilterParam filterParam, - SSG_treeGrowParam growParam, - SSG_planeCalibPara groundCalibPara, - SSX_BQworkpiecePara workpieceParam, + const SSG_treeGrowParam growParam, + const SSG_planeCalibPara groundCalibPara, + const SSX_BQworkpiecePara workpieceParam, #if _OUTPUT_DEBUG_DATA std::vector& debug_contours, #endif diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index a58de4f..a710163 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -677,6 +677,14 @@ SG_APISHARED_EXPORT void sg_pointClustering( double clusterDist, std::vector>& objClusters //result ); +//ʹSVzNL3DPositionnPointIdxʾ2DϢ16λLine 16λptIdx +//ʱԼ +SG_APISHARED_EXPORT void wd_pointClustering_speedUp( + std::vector< SVzNL3DPosition>& pts, + int lineNum, int ptSize, int clusterCheckWin, // + double clusterDist, + std::vector>& objClusters //result +); //դϵĴڵڵľ࣬3D //ʹvector2άṹ @@ -760,6 +768,13 @@ SG_APISHARED_EXPORT void wd_noiseFilter( const SSG_outlierFilterParam filterParam, int* errCode); +//ƽ +SG_APISHARED_EXPORT void wd_pointCloudHoleFilling( + std::vector< std::vector>& scanLines_src, + std::vector< std::vector>& scanLines_dst, + const int fillingHoleSize //״С +); + // Eigenʵ֣pts1pts2תƽƾ //Ҫ SG_APISHARED_EXPORT void caculateRT( diff --git a/sourceCode/SG_clustering.cpp b/sourceCode/SG_clustering.cpp index ba0a133..e562a37 100644 --- a/sourceCode/SG_clustering.cpp +++ b/sourceCode/SG_clustering.cpp @@ -9,6 +9,7 @@ void _seedClustering( std::vector< SVzNL3DPosition>& a_cluster, std::vector< SVzNL3DPosition>& pts, + std::vector flags, double clusterDist) { int i = 0; @@ -18,15 +19,61 @@ void _seedClustering( break; SVzNL3DPosition a_seed = a_cluster[i]; - for (int i = 0, i_max = (int)pts.size(); i < i_max; i++) + for (int j = 0, j_max = (int)pts.size(); j < j_max; j++) { - if (pts[i].nPointIdx < 0) + if (flags[j] < 0) continue; - double dist = sqrt(pow(a_seed.pt3D.x - pts[i].pt3D.x, 2) + pow(a_seed.pt3D.y - pts[i].pt3D.y, 2)); - if (dist < clusterDist) + double dist_x = abs(a_seed.pt3D.x - pts[j].pt3D.x); + double dist_y = abs(a_seed.pt3D.y - pts[j].pt3D.y); + if ( (dist_x < clusterDist) && (dist_y < clusterDist)) { - a_cluster.push_back(pts[i]); - pts[i].nPointIdx = -1; + a_cluster.push_back(pts[j]); + flags[j] = -1; + } + } + i++; + } + return; +} + +void _seedClustering_speedUp( + std::vector< SVzNL3DPosition>& a_cluster, + std::vector< SVzNL3DPosition>& pts, + std::vector& flags, + double clusterDist, + std::vector>& indexing2D, + int lineNum, int ptSize, int clusterCheckWin) // +{ + int i = 0; + while (1) + { + if (i >= a_cluster.size()) + break; + + SVzNL3DPosition a_seed = a_cluster[i]; + int seed_line = a_seed.nPointIdx >> 16; + int seed_ptIdx =a_seed.nPointIdx & 0x0000FFFF; + for (int line = seed_line - clusterCheckWin; line <= seed_line + clusterCheckWin; line++) + { + if ((line >= 0) && (line < lineNum)) + { + for (int ptIdx = seed_ptIdx - clusterCheckWin; ptIdx <= seed_ptIdx + clusterCheckWin; ptIdx++) + { + if ((ptIdx >= 0) && (ptIdx < ptSize)) + { + int backIdx = indexing2D[line][ptIdx]; + if (backIdx < 0) + continue; + if (flags[backIdx] < 0) + continue; + double dist = sqrt(pow(a_seed.pt3D.x - pts[backIdx].pt3D.x,2) + pow(a_seed.pt3D.y - pts[backIdx].pt3D.y,2)); + if (dist < clusterDist) + { + a_cluster.push_back(pts[backIdx]); + flags[backIdx] = -1; + } + } + } } } i++; @@ -43,30 +90,95 @@ void sg_pointClustering( int ptSize = (int)pts.size(); if (ptSize == 0) return; + + std::vector flags; + flags.resize(ptSize); + std::fill(flags.begin(), flags.end(), 0); + while(pts.size() > 0) { SVzNL3DPosition a_pt = pts[0]; - + flags[0] = -1;//ֹظ //½һcluster std::vector< SVzNL3DPosition> a_cluster; a_cluster.push_back(a_pt); - pts[0].nPointIdx = -1; //ֹظ + //pts[0].nPointIdx = -1; _seedClustering( a_cluster, pts, + flags, clusterDist); objClusters.push_back(a_cluster); //һ //ptsдĵȥ int m_max = (int)pts.size(); for (int m = m_max - 1; m >= 0; m--) //ӺǰɾӰѭ { - if(pts[m].nPointIdx < 0) + if (flags[m] < 0) + { pts.erase(pts.begin() + m); + flags.erase(flags.begin() + m); + } } } return; } +//ʹSVzNL3DPositionnPointIdxʾ2DϢ16λLine 16λptIdx +//ʱԼ +void wd_pointClustering_speedUp( + std::vector< SVzNL3DPosition>& pts, + int lineNum, int linePtSize, int clusterCheckWin, // + double clusterDist, + std::vector>& objClusters //result +) +{ + std::vector> indexing2D; + indexing2D.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + indexing2D[i].resize(linePtSize); + std::fill(indexing2D[i].begin(), indexing2D[i].end(), -1); + } + for (int i = 0; i < (int)pts.size(); i++) + { + int line = pts[i].nPointIdx >> 16; + int ptIdx = pts[i].nPointIdx & 0x0000FFFF; + indexing2D[line][ptIdx] = i; + } + + int ptSize = (int)pts.size(); + if (ptSize == 0) + return; + + std::vector flags; + flags.resize(ptSize); + std::fill(flags.begin(), flags.end(), 0); + + int idx = 0; + while (idx < (int)pts.size()) + { + SVzNL3DPosition a_pt = pts[idx]; + if (flags[idx] >= 0) + { + flags[idx] = -1;//ֹظ + //½һcluster + std::vector< SVzNL3DPosition> a_cluster; + a_cluster.push_back(a_pt); + //pts[0].nPointIdx = -1; + _seedClustering_speedUp( + a_cluster, + pts, + flags, + clusterDist, + indexing2D, + lineNum, linePtSize, clusterCheckWin); + objClusters.push_back(a_cluster); //һ + } + idx++; + } + return; +} + //ľ void wd_gridPointClustering( std::vector>& featureMask, diff --git a/sourceCode/WD_noiseFilter.cpp b/sourceCode/WD_noiseFilter.cpp index 269ea27..c02e576 100644 --- a/sourceCode/WD_noiseFilter.cpp +++ b/sourceCode/WD_noiseFilter.cpp @@ -60,3 +60,115 @@ void wd_noiseFilter( } return; } + +void wd_pointCloudHoleFilling( + std::vector< std::vector>& scanLines_src, + std::vector< std::vector>& scanLines_dst, + const int fillingHoleSize //״С +) +{ + int lineNum = (int)scanLines_src.size(); + scanLines_dst.resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + int ptNum = (int)scanLines_src[line].size(); + scanLines_dst[line].resize(ptNum); + for (int i = 0; i < ptNum; i++) + { + scanLines_dst[line][i] = scanLines_src[line][i]; + scanLines_src[line][i].nPointIdx = 0; //δ + } + } + + for (int line = 0; line < lineNum; line++) + { + int ptNum = (int)scanLines_src[line].size(); + for (int i = 0; i < ptNum; i++) + { + SVzNL3DPosition& a_pt = scanLines_src[line][i]; + if ( (a_pt.pt3D.z < 1e-4) && (a_pt.nPointIdx == 0)) //δĿհ׵ + { + //ǷҪ + //ɨ߷ + int j_top = -1; + for (int j = i - 1; j >= i - fillingHoleSize; j--) + { + if (j < 0) + break; + else if (scanLines_src[line][j].pt3D.z > 1e-4) + { + j_top = j; + break; + } + } + int j_btm = -1; + for (int j = i + 1; j <= i + fillingHoleSize; j++) + { + if (j >= ptNum) + break; + else if (scanLines_src[line][j].pt3D.z > 1e-4) + { + j_btm = j; + break; + } + } + if ((j_top >= 0) && (j_btm >= 0)) + { + SVzNL3DPoint& top_pt = scanLines_src[line][j_top].pt3D; + SVzNL3DPoint& btm_pt = scanLines_src[line][j_btm].pt3D; + //ֵ + double dist = (double)(j_btm - j_top); + for (int j = j_top + 1; j <= j_btm - 1; j++) + { + double d = (double)(j - j_top); + double k2 = d / dist; + double k1 = 1.0 - k2; + scanLines_dst[line][j].pt3D = { top_pt.x * k1 + btm_pt.x * k2, top_pt.y * k1 + btm_pt.y * k2 , top_pt.z * k1 + btm_pt.z * k2 }; + scanLines_src[line][j].nPointIdx = 1; + } + } + else + { + //ˮƽ + int line_left = -1; + for (int j = line - 1; j >= line - fillingHoleSize; j--) + { + if (j < 0) + break; + else if (scanLines_src[j][i].pt3D.z > 1e-4) + { + line_left = j; + break; + } + } + int line_right = -1; + for (int j = line + 1; j <= line + fillingHoleSize; j++) + { + if (j >= lineNum) + break; + else if (scanLines_src[j][i].pt3D.z > 1e-4) + { + line_right = j; + break; + } + } + if ((line_left >= 0) && (line_right >= 0)) + { + SVzNL3DPoint& left_pt = scanLines_src[line_left][i].pt3D; + SVzNL3DPoint& right_pt = scanLines_src[line_right][i].pt3D; + //ֵ + double dist = (double)(line_right - line_left); + for (int j = line_left + 1; j <= line_right - 1; j++) + { + double d = (double)(j - line_left); + double k2 = d / dist; + double k1 = 1.0 - k2; + scanLines_dst[j][i].pt3D = { left_pt.x * k1 + right_pt.x * k2, left_pt.y * k1 + right_pt.y * k2 , left_pt.z * k1 + right_pt.z * k2 }; + scanLines_src[j][i].nPointIdx = 1; + } + } + } + } + } + } +} \ No newline at end of file