From ab785098511419886f1b28e0e7c1b12d7a425a3e Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Mon, 20 Apr 2026 11:32:50 +0800 Subject: [PATCH] =?UTF-8?q?rodAndBarDetection=20version=201.2.4=20:=20?= =?UTF-8?q?=E6=A0=B9=E6=8D=AE=E5=AE=9A=E5=90=91=E7=9B=98=E5=B7=A6=E4=B8=8A?= =?UTF-8?q?=E7=82=B9=E5=92=8C=E5=8F=B3=E4=B8=8B=E7=82=B9=E7=A1=AE=E5=AE=9A?= =?UTF-8?q?=E5=A7=BF=E6=80=81=E5=90=91=E9=87=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- baseAlgorithm/baseAlgorithm.vcxproj | 1 + .../rodAndBarDetection_test.cpp | 83 +++- sourceCode/SG_baseAlgo_Export.h | 15 + sourceCode/SG_baseFunc.cpp | 11 + sourceCode/WD_holeDetection.cpp | 376 ++++++++++++++++++ sourceCode/rodAndBarDetection.cpp | 305 +++++++++++++- sourceCode/rodAndBarDetection_Export.h | 6 +- sourceCode/workpieceHolePositioning.cpp | 123 +++--- 8 files changed, 833 insertions(+), 87 deletions(-) create mode 100644 sourceCode/WD_holeDetection.cpp diff --git a/baseAlgorithm/baseAlgorithm.vcxproj b/baseAlgorithm/baseAlgorithm.vcxproj index 088da2a..026885c 100644 --- a/baseAlgorithm/baseAlgorithm.vcxproj +++ b/baseAlgorithm/baseAlgorithm.vcxproj @@ -175,6 +175,7 @@ + diff --git a/rodAndBarDetection_test/rodAndBarDetection_test.cpp b/rodAndBarDetection_test/rodAndBarDetection_test.cpp index 99f225f..540060b 100644 --- a/rodAndBarDetection_test/rodAndBarDetection_test.cpp +++ b/rodAndBarDetection_test/rodAndBarDetection_test.cpp @@ -224,16 +224,24 @@ void _outputChanneltInfo(char* fileName, std::vector& screwInfo sw.close(); } -void _outputPlatePiseInfo(char* fileName, SSX_pointPoseInfo& centerInfo) +void _outputPlatePiseInfo(char* fileName, SSX_platePoseInfo& centerInfo) { std::ofstream sw(fileName); char dataStr[250]; - sprintf_s(dataStr, 250, "定位盘: center_( %g, %g, %g ), normalDir_( %g, %g, %g ), xDir_( %g, %g, %g ), yDir_( %g, %g, %g )", - centerInfo.center.x, centerInfo.center.y, centerInfo.center.z, - centerInfo.normalDir.x, centerInfo.normalDir.y, centerInfo.normalDir.z, - centerInfo.xDir.x, centerInfo.xDir.y, centerInfo.xDir.z, - centerInfo.yDir.x, centerInfo.yDir.y, centerInfo.yDir.z); - sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "定位盘: \n"); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, " holeLT_(% g, % g, % g)\n", centerInfo.holeLT.x, centerInfo.holeLT.y, centerInfo.holeLT.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, " holeRB_(% g, % g, % g)\n", centerInfo.holeRB.x, centerInfo.holeRB.y, centerInfo.holeRB.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, " center_(% g, % g, % g)\n", centerInfo.center.x, centerInfo.center.y, centerInfo.center.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, " normalDir_(% g, % g, % g)\n", centerInfo.normalDir.x, centerInfo.normalDir.y, centerInfo.normalDir.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, " xDir_(% g, % g, % g)\n", centerInfo.xDir.x, centerInfo.xDir.y, centerInfo.xDir.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, " yDir_(% g, % g, % g)\n", centerInfo.yDir.x, centerInfo.yDir.y, centerInfo.yDir.z); + sw << dataStr << std::endl; sw.close(); } @@ -409,7 +417,7 @@ void _outputRGBDScan_RGBD( void _outputRGBDScan_RGBD_centerPose( char* fileName, std::vector>& scanLines, - SSX_pointPoseInfo& poseInfo + SSX_platePoseInfo& poseInfo ) { int lineNum = (int)scanLines.size(); @@ -477,7 +485,7 @@ void _outputRGBDScan_RGBD_centerPose( } { - sw << "Line_" << lineIdx << "_0_1" << std::endl; + sw << "Line_" << lineIdx << "_0_3" << std::endl; rgb = { 250, 0, 0 }; size = 8; float x = (float)poseInfo.center.x; @@ -487,6 +495,20 @@ void _outputRGBDScan_RGBD_centerPose( sw << "{0,0}-{0,0}-"; sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + x = (float)poseInfo.holeLT.x; + y = (float)poseInfo.holeLT.y; + z = (float)poseInfo.holeLT.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + + x = (float)poseInfo.holeRB.x; + y = (float)poseInfo.holeRB.y; + z = (float)poseInfo.holeRB.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + //输出法向 size = 1; double len = 60; @@ -535,6 +557,37 @@ void _outputRGBDScan_RGBD_centerPose( sw << "{0,0}-{0,0}-"; sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; lineIdx++; + + rgb = { 0, 250, 0 }; + basePt = { poseInfo.holeLT.x - len * poseInfo.xDir.x, + poseInfo.holeLT.y - len * poseInfo.xDir.y, + poseInfo.holeLT.z - len * poseInfo.xDir.z }; + pt2 = { poseInfo.holeLT.x + len * poseInfo.xDir.x, + poseInfo.holeLT.y + len * poseInfo.xDir.y, + poseInfo.holeLT.z + len * poseInfo.xDir.z }; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)basePt.x << "," << (float)basePt.y << "," << (float)basePt.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << pt2.x << "," << pt2.y << "," << pt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + basePt = { poseInfo.holeRB.x - len * poseInfo.xDir.x, + poseInfo.holeRB.y - len * poseInfo.xDir.y, + poseInfo.holeRB.z - len * poseInfo.xDir.z }; + pt2 = { poseInfo.holeRB.x + len * poseInfo.xDir.x, + poseInfo.holeRB.y + len * poseInfo.xDir.y, + poseInfo.holeRB.z + len * poseInfo.xDir.z }; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)basePt.x << "," << (float)basePt.y << "," << (float)basePt.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << pt2.x << "," << pt2.y << "," << pt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; } } sw.close(); @@ -892,7 +945,7 @@ void locatingPlateTest(void) }; SVzNLRange fileIdx[LOCATING_PALTE_TEST_GROUP] = { - {1,16}, + {1,17}, }; const char* ver = wd_rodAndBarDetectionVersion(); @@ -902,7 +955,7 @@ void locatingPlateTest(void) { for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx =4; + //fidx =2; char _scan_file[256]; sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); @@ -926,7 +979,7 @@ void locatingPlateTest(void) cornerParam.jumpCornerTh_2 = 60; int errCode = 0; - SSX_pointPoseInfo centerPose = sx_getLocationPlatePose( + SSX_platePoseInfo centerPose = sx_getLocationPlatePose( scanLines, cornerParam, & errCode); @@ -1109,10 +1162,10 @@ void rodWeldSeamPosition_test(void) SSG_treeGrowParam growParam; growParam.maxLineSkipNum = 5; growParam.yDeviation_max = 10.0; - growParam.maxSkipDistance = 10.0; + growParam.maxSkipDistance = 20.0; growParam.zDeviation_max = 10.0;// - growParam.minLTypeTreeLen = 100; //mm, 螺杆长度 - growParam.minVTypeTreeLen = 100; //mm + growParam.minLTypeTreeLen = 50; //mm, 螺杆长度 + growParam.minVTypeTreeLen = 50; //mm bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道 int errCode = 0; diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 7e8c414..d287e93 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -7,6 +7,9 @@ // SG_APISHARED_EXPORT SVzNL3DPoint vec3_cross(const SVzNL3DPoint& a, const SVzNL3DPoint& b); +//ʱתʱ > 0 ˳ʱתʱ < 0 +SG_APISHARED_EXPORT SVzNL3DPoint wd_rotate2D(const SVzNL3DPoint& pt, const double angle); + //˳Ⱥ:z޷Ϊεж SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier( SVzNL3DPosition* lineData, @@ -874,3 +877,15 @@ SG_APISHARED_EXPORT void scanLinesSmooting3x3( std::vector< std::vector>& gridDataInput, std::vector< std::vector>& smoothingData ); + + +SG_APISHARED_EXPORT void WD_getHoleInfo( + std::vector< std::vector>& scanLines, + const SSG_lineSegParam lineSegPara, + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + const double valieCommonNumRatio, + std::vector& segTrees_v, + std::vector& segTrees_h, + std::vector& validObjects +); diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 09b146f..04582e3 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -19,6 +19,17 @@ SVzNL3DPoint vec3_cross(const SVzNL3DPoint& a, const SVzNL3DPoint& b) c.z = a.x * b.y - a.y * b.x; return c; } +//ʱתʱ > 0 ˳ʱתʱ < 0 +SVzNL3DPoint wd_rotate2D(const SVzNL3DPoint& pt, const double angle) +{ + double sinTheta = sin(PI * angle / 180); + double cosTheta = cos(PI * angle / 180); + SVzNL3DPoint rotatePt; + rotatePt.x = pt.x * cosTheta - pt.y * sinTheta; + rotatePt.y = pt.x * sinTheta + pt.y * cosTheta; + rotatePt.z = pt.z; + return rotatePt; +} SVzNL3DRangeD sg_getScanDataROI( //ɨROI diff --git a/sourceCode/WD_holeDetection.cpp b/sourceCode/WD_holeDetection.cpp new file mode 100644 index 0000000..2ed6219 --- /dev/null +++ b/sourceCode/WD_holeDetection.cpp @@ -0,0 +1,376 @@ +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include + +void _updateROI(SSG_ROIRectD& roi, SVzNL3DPoint& a_pt) +{ + if (a_pt.z > 1E-4) + { + if (roi.left < 0) + { + roi.left = a_pt.x; + roi.right = a_pt.x; + roi.left = a_pt.y; + roi.right = a_pt.y; + } + else + { + if (roi.left > a_pt.x) + roi.left = a_pt.x; + if (roi.right < a_pt.x) + roi.right = a_pt.x; + if (roi.top > a_pt.y) + roi.top = a_pt.y; + if (roi.bottom < a_pt.y) + roi.bottom = a_pt.y; + } + } + return; +} + +void _updateRenge(SVzNLRange& range, int idx) +{ + if (range.nMin < 0) + { + range.nMin = idx; + range.nMax = idx; + } + else + { + if (range.nMin > idx) + range.nMin = idx; + if (range.nMax < idx) + range.nMax = idx; + } + return; +} + +bool _checkExist(int id, std::vector& buff) +{ + for (int i = 0; i < (int)buff.size(); i++) + { + if (id == buff[i]) + return true; + } + return false; +} + +void _searchNeighbours( + int selfId, int chkExtening, + int sLineIdx, int eLineIdx, + SVzNLRange ptIdxRange, + std::vector>& treeMask, + std::vector& neighbours) +{ + int lineNum = (int)treeMask.size(); + int ptNum = treeMask[0].size(); + for (int line = sLineIdx - chkExtening; line <= eLineIdx + chkExtening; line++) + { + if ((line >= 0) && (line < lineNum)) + { + for (int ptIdx = ptIdxRange.nMin - chkExtening; ptIdx <= ptIdxRange.nMax + chkExtening; ptIdx++) + { + if ((ptIdx >= 0) && (ptIdx < ptNum)) + { + if ((treeMask[line][ptIdx] >= 0) && (treeMask[line][ptIdx] != selfId)) + { + bool isExist = _checkExist(treeMask[line][ptIdx], neighbours); + if (false == isExist) + neighbours.push_back(treeMask[line][ptIdx]); + } + } + } + } + } + return; +} + +void WD_getHoleInfo( + std::vector< std::vector>& scanLines, + const SSG_lineSegParam lineSegPara, + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + const double valieCommonNumRatio, + std::vector& segTrees_v, + std::vector& segTrees_h, + std::vector& validObjects +) +{ + int lineNum = (int)scanLines.size(); + int linePtNum = (int)scanLines[0].size(); + + std::vector> pointMask; + pointMask.resize(lineNum); + + //ȡհ߶ + std::vector> holeGaps; + //ȡ߶ζ˵ + for (int line = 0; line < lineNum; line++) + { + if (line == 1047) + int kkk = 1; + + std::vector& lineData = scanLines[line]; + pointMask[line].resize(lineData.size()); + std::fill(pointMask[line].begin(), pointMask[line].end(), 0);//ʼΪ0 + //˲˳쳣 + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); + + std::vector segs; + wd_getLineDataNullIntervals(lineData, lineSegPara, segs); + + //seg˵ΪԵ㡣˵ƽ󣬴ֱ׵ڲXYƽϾΪԵ㡣 + std::vector line_gaps; + for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) + { + int ptIdx_1 = segs[i].start; + int ptIdx_2 = segs[i].start + segs[i].len - 1; + SWD_segFeature a_gap; + a_gap.lineIdx = line; + a_gap.startPtIdx = ptIdx_1; + a_gap.endPtIdx = ptIdx_2; + a_gap.startPt = lineData[ptIdx_1].pt3D; + a_gap.endPt = lineData[ptIdx_2].pt3D; + a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y); + line_gaps.push_back(a_gap); + } + holeGaps.push_back(line_gaps); + } + // + wd_getSegFeatureGrowingTrees_2(holeGaps, segTrees_v, growParam); + + //ˮƽɨ + std::vector> hLines_raw; + hLines_raw.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + hLines_raw[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã + hLines_raw[j][line] = scanLines[line][j]; + hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y; + hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + //ˮƽarcȡ + std::vector> holeGaps_h; + for (int line = 0; line < linePtNum; line++) + { + if (line == 974) + int kkk = 1; + std::vector& lineData = hLines_raw[line]; + //˲˳쳣 + int ptNum = (int)lineData.size(); + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam); + + std::vector segs; + wd_getLineDataNullIntervals(lineData, lineSegPara, segs); + + //seg˵ΪԵ㡣˵ƽ󣬴ֱ׵ڲXYƽϾΪԵ㡣 + std::vector line_gaps; + for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) + { + int ptIdx_1 = segs[i].start; + int ptIdx_2 = segs[i].start + segs[i].len - 1; + SWD_segFeature a_gap; + a_gap.lineIdx = line; + a_gap.startPtIdx = ptIdx_1; + a_gap.endPtIdx = ptIdx_2; + a_gap.startPt = lineData[ptIdx_1].pt3D; + a_gap.endPt = lineData[ptIdx_2].pt3D; + a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y); + line_gaps.push_back(a_gap); + } + holeGaps_h.push_back(line_gaps); + } + // + wd_getSegFeatureGrowingTrees_2(holeGaps_h, segTrees_h, growParam); + + //Treeڿ׶Mask + std::vector> treeMask_v; + treeMask_v.resize(lineNum); + std::vector> treeMask_h; + treeMask_h.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + treeMask_v[i].resize(linePtNum); + std::fill(treeMask_v[i].begin(), treeMask_v[i].end(), -1); + treeMask_h[i].resize(linePtNum); + std::fill(treeMask_h[i].begin(), treeMask_h[i].end(), -1); + } + //ע + std::vector treeInfo_v; + treeInfo_v.resize(segTrees_v.size()); + for (int i = 0; i < (int)segTrees_v.size(); i++) + { + SWD_segFeatureTree& a_tree = segTrees_v[i]; + treeInfo_v[i].treeIdx = i; + treeInfo_v[i].sLineIdx = a_tree.sLineIdx; + treeInfo_v[i].eLineIdx = a_tree.eLineIdx; + treeInfo_v[i].vTreeFlag = 0; + treeInfo_v[i].treeType = 0; + treeInfo_v[i].roi = { -1, -1, -1, -1 }; + int nullPtSize = 0; + SSG_ROIRectD roi = { -1, -1, -1, -1 }; + SVzNLRange ptIdxRange = { -1, -1 }; + if (a_tree.treeNodes.size() > 0) + { + for (int m = 0; m < (int)a_tree.treeNodes.size(); m++) + { + SWD_segFeature& a_seg = a_tree.treeNodes[m]; + for (int n = a_seg.startPtIdx; n <= a_seg.endPtIdx; n++) + treeMask_v[a_seg.lineIdx][n] = i; + + nullPtSize += a_seg.endPtIdx - a_seg.startPtIdx + 1; + _updateROI(roi, scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D); + _updateROI(roi, scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D); + _updateRenge(ptIdxRange, a_seg.startPtIdx); + _updateRenge(ptIdxRange, a_seg.endPtIdx); + //scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = 0x01; + //scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = 0x01; + } + treeInfo_v[i].treeType = nullPtSize; + treeInfo_v[i].roi = roi; + treeInfo_v[i].ptIdxRange = ptIdxRange; + } + } + std::vector treeInfo_h; + treeInfo_h.resize(segTrees_h.size()); + for (int i = 0; i < (int)segTrees_h.size(); i++) + { + SWD_segFeatureTree& a_tree = segTrees_h[i]; + treeInfo_h[i].treeIdx = i; + treeInfo_h[i].sLineIdx = a_tree.sLineIdx; + treeInfo_h[i].eLineIdx = a_tree.eLineIdx; + treeInfo_h[i].vTreeFlag = 0; + treeInfo_h[i].treeType = 0; + treeInfo_h[i].roi = { -1, -1, -1, -1 }; + int nullPtSize = 0; + SSG_ROIRectD roi = { -1, -1, -1, -1 }; + SVzNLRange ptIdxRange = { -1, -1 }; + if (a_tree.treeNodes.size() > 0) + { + for (int m = 0; m < (int)a_tree.treeNodes.size(); m++) + { + SWD_segFeature& a_seg = a_tree.treeNodes[m]; + for (int n = a_seg.startPtIdx; n <= a_seg.endPtIdx; n++) + treeMask_h[n][a_seg.lineIdx] = i; + + nullPtSize += a_seg.endPtIdx - a_seg.startPtIdx + 1; + _updateROI(roi, scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D); + _updateROI(roi, scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D); + _updateRenge(ptIdxRange, a_seg.startPtIdx); + _updateRenge(ptIdxRange, a_seg.endPtIdx); + //scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx |= 0x02; + //scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx |= 0x02; + } + treeInfo_h[i].treeType = nullPtSize; + treeInfo_h[i].roi = roi; + treeInfo_h[i].ptIdxRange = ptIdxRange; + } + } + //ˮƽʹֱĿϲ + int vTreeSize = (int)segTrees_v.size(); + int hTreeSize = (int)segTrees_h.size(); + std::vector> treeHVInfo; //ͳƴֱˮƽtreeλϢ + treeHVInfo.resize(vTreeSize); + for (int i = 0; i < vTreeSize; i++) + { + treeHVInfo[i].resize(hTreeSize); + std::fill(treeHVInfo[i].begin(), treeHVInfo[i].end(), 0); + } + for (int line = 0; line < lineNum; line++) + { + for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++) + { + int idx_v = treeMask_v[line][ptIdx]; + int idx_h = treeMask_h[line][ptIdx]; + if ((idx_v >= 0) && (idx_h >= 0)) + treeHVInfo[idx_v][idx_h]++; + } + } + + //ɺѡĿ + std::vector objects; + for (int i = 0; i < vTreeSize; i++) + { + SWD_segFeatureTree& a_tree = segTrees_v[i]; + if ((a_tree.sLineIdx <= 1047) && (a_tree.eLineIdx >= 1047)) + int kkk = 1; + + int totalSize_v = treeInfo_v[i].treeType; + int commonSize = 0; + int hTreeIdx = -1; + for (int j = 0; j < hTreeSize; j++) + { + if (commonSize < treeHVInfo[i][j]) + { + hTreeIdx = j; + commonSize = treeHVInfo[i][j]; + } + } + if (hTreeIdx >= 0) + { + int totalSize_h = treeInfo_h[hTreeIdx].treeType; + int sizeV_th = (int)((double)totalSize_v * valieCommonNumRatio); + int sizeH_th = (int)((double)totalSize_h * valieCommonNumRatio); + if ((commonSize > sizeH_th) && (commonSize > sizeV_th)) + { + SSG_intPair a_obj; + a_obj.data_0 = i; + a_obj.data_1 = hTreeIdx; + a_obj.idx = commonSize; + objects.push_back(a_obj); + treeInfo_v[i].data = commonSize; + treeInfo_h[hTreeIdx].data = commonSize; + } + } + } + + //˳ڡÿĿ걣һ + for (int i = 0; i < (int)objects.size(); i++) + { + int vTreeIdx = objects[i].data_0; + if (treeInfo_v[vTreeIdx].vTreeFlag < 0) + continue; + + std::vector neighbours; + _searchNeighbours(vTreeIdx, 3, + treeInfo_v[vTreeIdx].sLineIdx, treeInfo_v[vTreeIdx].eLineIdx, + treeInfo_v[vTreeIdx].ptIdxRange, treeMask_v, neighbours); + + int bestIdx = vTreeIdx; + int maxValue = treeInfo_v[vTreeIdx].data; + if (neighbours.size() > 0) + { + for (int j = 0; j < (int)neighbours.size(); j++) + { + int idx = neighbours[j]; + if (maxValue < treeInfo_v[idx].data) + { + maxValue = treeInfo_v[idx].data; + bestIdx = idx; + } + } + if (bestIdx != vTreeIdx) + treeInfo_v[vTreeIdx].vTreeFlag = -1; + for (int j = 0; j < (int)neighbours.size(); j++) + { + int idx = neighbours[j]; + if (bestIdx != idx) + treeInfo_v[idx].vTreeFlag = -1; + } + } + } + + for (int i = 0; i < (int)objects.size(); i++) + { + int vTreeIdx = objects[i].data_0; + if (treeInfo_v[vTreeIdx].vTreeFlag < 0) + continue; + + validObjects.push_back(objects[i]); + } +} diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp index 094cd84..e51a4a7 100644 --- a/sourceCode/rodAndBarDetection.cpp +++ b/sourceCode/rodAndBarDetection.cpp @@ -12,7 +12,8 @@ //version 1.2.1 : ˶λ̬ //version 1.2.2 : ˶λ̬壺Z򣩣ңY //version 1.2.3 : ݶֱ߶ȷҵ -std::string m_strVersion = "1.2.3"; +//version 1.2.4 : ݶϵµȷ̬ +std::string m_strVersion = "1.2.4"; const char* wd_rodAndBarDetectionVersion(void) { return m_strVersion.c_str(); @@ -718,16 +719,52 @@ void _searchPlusCornerPeaks( } } +void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt) +{ + if (a_pt.z > 1E-4) + { + if (roi.zRange.max < 0) + { + roi.xRange.min = a_pt.x; + roi.xRange.max = a_pt.x; + roi.yRange.min = a_pt.y; + roi.yRange.max = a_pt.y; + roi.zRange.min = a_pt.z; + roi.zRange.max = a_pt.z; + } + else + { + if (roi.xRange.min > a_pt.x) + roi.xRange.min = a_pt.x; + if (roi.xRange.max < a_pt.x) + roi.xRange.max = a_pt.x; + if (roi.yRange.min > a_pt.y) + roi.yRange.min = a_pt.y; + if (roi.yRange.max < a_pt.y) + roi.yRange.max = a_pt.y; + if (roi.zRange.min > a_pt.z) + roi.zRange.min = a_pt.z; + if (roi.zRange.max < a_pt.z) + roi.zRange.max = a_pt.z; + } + } + return; +} + //㶨λĵλ -SSX_pointPoseInfo sx_getLocationPlatePose( +SSX_platePoseInfo sx_getLocationPlatePose( std::vector< std::vector>& scanLines, const SSG_cornerParam cornerPara, int* errCode) { *errCode = 0; - SSX_pointPoseInfo resultPose; + SSX_platePoseInfo resultPose; resultPose.center = { 0, 0, 0 }; resultPose.normalDir = { 0, 0, 0 }; + resultPose.holeLT = { 0, 0, 0 }; + resultPose.holeRB = { 0, 0, 0 }; + resultPose.xDir = { 0, 0, 0 }; + resultPose.yDir = { 0, 0, 0 }; int lineNum = (int)scanLines.size(); if (lineNum == 0) { @@ -960,10 +997,19 @@ SSX_pointPoseInfo sx_getLocationPlatePose( } //: z = Ax + By + C //res: [0]=A, [1]= B, [2]=-1.0, [3]=C, - //std::vector out_inliers; - //Plane res = ransacFitPlane( Points3ds, out_inliers ); +#if 1 + std::vector out_inliers; + Plane res = ransacFitPlane( Points3ds, out_inliers ); + if (res.C < 0) + { + res.A = -res.A; + res.B = -res.B; + res.C = -res.C; + res.D = -res.D; + } +#else Plane res = robustFitPlane(Points3ds); - +#endif //std::vector res; //vzCaculateLaserPlane(Points3ds, res); //ͶӰ @@ -972,11 +1018,14 @@ SSX_pointPoseInfo sx_getLocationPlatePose( SSG_planeCalibPara poseR = wd_computeRTMatrix(vec_1, vec_2); //ͶӰ + double normDataPlane = sqrt(res.A * res.A + res.B * res.B + res.C * res.C); std::vector projectPoints3ds; projectPoints3ds.resize(Points3ds.size()); - double sum_x = 0, sum_y = 0; + double sum_x = 0, sum_y = 0, sum_z = 0; + double sumZConter = 0; for (int i = 0; i < (int)Points3ds.size(); i++) { + double distToPlane = abs(res.A * Points3ds[i].x + res.B * Points3ds[i].y + res.C * Points3ds[i].z + res.D) / normDataPlane; double x = Points3ds[i].x * poseR.planeCalib[0] + Points3ds[i].y * poseR.planeCalib[1] + Points3ds[i].z * poseR.planeCalib[2]; double y = Points3ds[i].x * poseR.planeCalib[3] + Points3ds[i].y * poseR.planeCalib[4] + Points3ds[i].z * poseR.planeCalib[5]; double z = Points3ds[i].x * poseR.planeCalib[6] + Points3ds[i].y * poseR.planeCalib[7] + Points3ds[i].z * poseR.planeCalib[8]; @@ -986,6 +1035,13 @@ SSX_pointPoseInfo sx_getLocationPlatePose( polarPoints[i].z = z; sum_x += x; sum_y += y; + + if (distToPlane < 2.0) + { + sum_z += z; + sumZConter++; + } + } for (int i = 0; i < lineNum; i++) @@ -1000,7 +1056,7 @@ SSX_pointPoseInfo sx_getLocationPlatePose( // double center_x = sum_x / (double)Points3ds.size(); double center_y = sum_y / (double)Points3ds.size(); - + double center_z = sum_z / (double)sumZConter; //㼫RTheta for (int pi = 0; pi < (int)polarPoints.size(); pi++) { @@ -1024,6 +1080,224 @@ SSX_pointPoseInfo sx_getLocationPlatePose( double minCutAngleTh = 30; _searchPlusCornerPeaks(dirCornerAngles, minCutAngleTh, cornerPlusPeaks); +#if 1 + int cornerIdx_LT = -1; + int cornerIdx_RB = -1; + for (int i = 0; i < (int)cornerPlusPeaks.size(); i++) + { + if ((cornerPlusPeaks[i].point.angle > 0) && (cornerPlusPeaks[i].point.angle < 90)) + cornerIdx_LT = i; + if ((cornerPlusPeaks[i].point.angle > 180) && (cornerPlusPeaks[i].point.angle < 270)) + cornerIdx_RB = i; + } + if ((cornerIdx_LT < 0) || (cornerIdx_RB < 0)) + { + *errCode = SX_ERR_UNKNOWN_PLATE_DIR; + return resultPose; + } + SSG_dirCornerAngle& corner_LT = cornerPlusPeaks[cornerIdx_LT]; + SSG_dirCornerAngle& corner_RB = cornerPlusPeaks[cornerIdx_RB]; + + //Zи + //ڲ + double zTh_min = center_z - 1.0; + double zTh_max = center_z + 5.0; + std::vector< std::vector> zCutLines; + zCutLines.resize(scanLines.size()); + for (int line = 0; line < lineNum; line++) + { + //д + zCutLines[line].resize(linePtNum); + for (int j = 0; j < linePtNum; j++) + { + zCutLines[line][j] = scanLines[line][j]; + if ((scanLines[line][j].pt3D.z < zTh_min) || (scanLines[line][j].pt3D.z > zTh_max)) + zCutLines[line][j].pt3D = { 0.0, 0.0, 0.0 }; + } + } + //״ + std::vector validObjects; + std::vector holeSegTrees_v; + std::vector holeSegTrees_h; + SSG_lineSegParam hole_lineSegPara; + hole_lineSegPara.distScale = 3.0; + hole_lineSegPara.segGapTh_y = 15.0; // + hole_lineSegPara.segGapTh_z = 0.0; //z10mmΪǷֶ + SSG_outlierFilterParam hole_filterParam; + hole_filterParam.continuityTh = 20.0; //˳ڵzڴʱǷΪСoutlierLen Ϊ + hole_filterParam.outlierTh = 5; + SSG_treeGrowParam hole_growParam; + hole_growParam.maxLineSkipNum = 2; + hole_growParam.yDeviation_max = 4.0; + hole_growParam.maxSkipDistance = 2.0; + hole_growParam.zDeviation_max = 10.0;// + hole_growParam.minLTypeTreeLen = 2.0; //mm + hole_growParam.minVTypeTreeLen = 2.0; //mm + double valieCommonNumRatio = 0.25; + WD_getHoleInfo( + zCutLines, + hole_lineSegPara, + hole_filterParam, + hole_growParam, + valieCommonNumRatio, + holeSegTrees_v, + holeSegTrees_h, + validObjects + ); + + //ѰϿ׺¿ + for (int i = 0; i < lineNum; i++) + { + for (int j = 0; j < (int)scanLines[i].size(); j++) + zCutLines[i][j].nPointIdx = 0; // + } + //ɾϢ + std::vector> clusters; //ֻ¼λ + std::vector clustersRoi3D; + for (int i = 0; i < (int)validObjects.size(); i++) + { + std::vector< SVzNL2DPoint> a_cluster; + SVzNL3DRangeD a_roi3D = { {-1, -1}, {-1, -1}, {-1, -1 } }; + + int vTreeIdx = validObjects[i].data_0; + int hTreeIdx = validObjects[i].data_1; + for (int m = 0; m < (int)holeSegTrees_v[vTreeIdx].treeNodes.size(); m++) + { + SWD_segFeature& a_seg = holeSegTrees_v[vTreeIdx].treeNodes[m]; + if (zCutLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx == 0) + { + zCutLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01; + scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 3; // 0x01; + SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.endPtIdx }; + a_cluster.push_back(a_pos); + _updateRoi3D(a_roi3D, zCutLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D); + } + if (zCutLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx == 0) + { + zCutLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01; + scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 3; // 0x01; + SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.startPtIdx }; + a_cluster.push_back(a_pos); + _updateRoi3D(a_roi3D, zCutLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D); + } + } + for (int m = 0; m < (int)holeSegTrees_h[hTreeIdx].treeNodes.size(); m++) + { + SWD_segFeature& a_seg = holeSegTrees_h[hTreeIdx].treeNodes[m]; + if (zCutLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx == 0) + { + zCutLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = hTreeIdx + 1; // 0x02; + scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = hTreeIdx + 4; // 0x02; + SVzNL2DPoint a_pos = { a_seg.startPtIdx , a_seg.lineIdx }; + a_cluster.push_back(a_pos); + _updateRoi3D(a_roi3D, zCutLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D); + } + if (zCutLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx == 0) + { + zCutLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = hTreeIdx + 1; // 0x02; + scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = hTreeIdx + 4; // 0x02; + SVzNL2DPoint a_pos = { a_seg.endPtIdx , a_seg.lineIdx }; + a_cluster.push_back(a_pos); + _updateRoi3D(a_roi3D, zCutLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D); + } + } + clusters.push_back(a_cluster); + clustersRoi3D.push_back(a_roi3D); + } + + //:ϺµĿ + //ڲ + double minHoleSize = 10.0; + double maxHoleSize = 24.0; + double maxCornerHoleDistance = 40; //ǵ㵽׵ + int clusterSize = (int)clusters.size(); + int clusterIdx_LT = -1; + double minDistLT = -1; + int clusterIdx_RB = -1; + double minDistRB = -1; + for (int i = 0; i < clusterSize; i++) + { + SVzNL3DRangeD& a_roi = clustersRoi3D[i]; + double L = a_roi.xRange.max - a_roi.xRange.min; + double W = a_roi.yRange.max - a_roi.yRange.min; + double cx = (a_roi.xRange.max + a_roi.xRange.min) / 2; + double cy = (a_roi.yRange.max + a_roi.yRange.min) / 2; + + if ((L > minHoleSize) && (L < maxHoleSize) && (W > minHoleSize) && (W < maxHoleSize)) + { + double dist_1 = sqrt(pow(corner_LT.point.x - cx, 2) + pow(corner_LT.point.y - cy, 2)); + if ((minDistLT < 0) || (minDistLT > dist_1)) + { + minDistLT = dist_1; + clusterIdx_LT = i; + } + + double dist_2 = sqrt(pow(corner_RB.point.x - cx, 2) + pow(corner_RB.point.y - cy, 2)); + if ((minDistRB < 0) || (minDistRB > dist_2)) + { + minDistRB = dist_2; + clusterIdx_RB = i; + } + } + } + if( (clusterIdx_LT < 0) || (clusterIdx_RB < 0) || (minDistLT > maxCornerHoleDistance) || (minDistRB > maxCornerHoleDistance)) + { + *errCode = SX_ERR_UNKNOWN_PLATE_DIR; + return resultPose; + } + + //ĿԲ + std::vector pointArrayLT; + int clusterPtSizeLT = (int)clusters[clusterIdx_LT].size(); + for (int i = 0; i < clusterPtSizeLT; i++) + { + SVzNL2DPoint a_pos = clusters[clusterIdx_LT][i]; + int lineIdx = a_pos.x; + int ptIdx = a_pos.y; + SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D; + pointArrayLT.push_back(a_pt3d); + } + //Բ + SVzNL3DPoint centerLT; + double radiusLT; + double err = fitCircleByLeastSquare(pointArrayLT, centerLT, radiusLT); + centerLT.z = center_z; + + std::vector pointArrayRB; + int clusterPtSizeRB = (int)clusters[clusterIdx_RB].size(); + for (int i = 0; i < clusterPtSizeRB; i++) + { + SVzNL2DPoint a_pos = clusters[clusterIdx_RB][i]; + int lineIdx = a_pos.x; + int ptIdx = a_pos.y; + SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D; + pointArrayRB.push_back(a_pt3d); + } + //Բ + SVzNL3DPoint centerRB; + double radiusRB; + err = fitCircleByLeastSquare(pointArrayRB, centerRB, radiusRB); + centerRB.z = center_z; + + SVzNL3DPoint refVec = { (centerRB.x-centerLT.x)/2, (centerRB.y - centerLT.y)/2, center_z}; + double angleToHorizon = -46; //ˮƽ߼н46 + double angleToCenter = 3; //е붨λļн3 + //ʱתʱ > 0 ˳ʱתʱ < 0 + SVzNL3DPoint rVec_1 = wd_rotate2D(refVec, angleToCenter); + center_x = rVec_1.x + centerLT.x; + center_y = rVec_1.y + centerLT.y; + //center_z = center_z + 34.0; + SVzNL3DPoint xDir = wd_rotate2D(refVec, angleToHorizon); + double normData = sqrt(pow(xDir.x, 2) + pow(xDir.y, 2)); + xDir.x = xDir.x / normData; + xDir.y = xDir.y / normData; + xDir.z = 0; + + resultPose.holeLT = centerLT; + resultPose.holeRB = centerRB; + + center_z += 33.8; //λĵ33.8 +#else //ֱ߶ int cornerIdx_0 = -1; int cornerIdx_1 = -1; @@ -1084,7 +1358,7 @@ SSX_pointPoseInfo sx_getLocationPlatePose( lineFitting(lineFittingPts, &_k, &_b); double normData = sqrt(pow(_k, 2) + 1); SVzNL3DPoint xDir = { 1.0/normData, _k/normData, 0.0 }; - + //4 SVzNL2DPointD top_pt = { center_x, center_y - centerZ_R }; SVzNL2DPointD bottom_pt = { center_x, center_y + centerZ_R }; @@ -1166,7 +1440,8 @@ SSX_pointPoseInfo sx_getLocationPlatePose( } } double center_z = (ptTop.z + ptBtm.z + ptLeft.z + ptRight.z) / 4; - resultPose.center = { center_x, center_y, center_z }; +#endif + resultPose.center = { center_x, center_y, center_z }; resultPose.xDir = { 0.0, 0.0, -1.0 }; // { 1.0, 0.0, 0 }; //resultPose.yDir = { 0.0, 1.0, 0 }; resultPose.normalDir = xDir; // { 1.0, 0.0, 0 };// { 0.0, 0.0, 1.0 }; @@ -1200,6 +1475,16 @@ SSX_pointPoseInfo sx_getLocationPlatePose( z = resultPose.yDir.x * poseR.invRMatrix[6] + resultPose.yDir.y * poseR.invRMatrix[7] + resultPose.yDir.z * poseR.invRMatrix[8]; resultPose.yDir = { x, y, z }; + x = resultPose.holeLT.x * poseR.invRMatrix[0] + resultPose.holeLT.y * poseR.invRMatrix[1] + resultPose.holeLT.z * poseR.invRMatrix[2]; + y = resultPose.holeLT.x * poseR.invRMatrix[3] + resultPose.holeLT.y * poseR.invRMatrix[4] + resultPose.holeLT.z * poseR.invRMatrix[5]; + z = resultPose.holeLT.x * poseR.invRMatrix[6] + resultPose.holeLT.y * poseR.invRMatrix[7] + resultPose.holeLT.z * poseR.invRMatrix[8]; + resultPose.holeLT = { x, y, z }; + + x = resultPose.holeRB.x * poseR.invRMatrix[0] + resultPose.holeRB.y * poseR.invRMatrix[1] + resultPose.holeRB.z * poseR.invRMatrix[2]; + y = resultPose.holeRB.x * poseR.invRMatrix[3] + resultPose.holeRB.y * poseR.invRMatrix[4] + resultPose.holeRB.z * poseR.invRMatrix[5]; + z = resultPose.holeRB.x * poseR.invRMatrix[6] + resultPose.holeRB.y * poseR.invRMatrix[7] + resultPose.holeRB.z * poseR.invRMatrix[8]; + resultPose.holeRB = { x, y, z }; + return resultPose; } diff --git a/sourceCode/rodAndBarDetection_Export.h b/sourceCode/rodAndBarDetection_Export.h index 75b2fe8..3ec68bd 100644 --- a/sourceCode/rodAndBarDetection_Export.h +++ b/sourceCode/rodAndBarDetection_Export.h @@ -17,7 +17,9 @@ typedef struct SVzNL3DPoint xDir; SVzNL3DPoint yDir; SVzNL3DPoint normalDir; // -}SSX_pointPoseInfo; // + SVzNL3DPoint holeLT; + SVzNL3DPoint holeRB; +}SSX_platePoseInfo; // typedef struct { @@ -71,7 +73,7 @@ SG_APISHARED_EXPORT void sx_hexHeadScrewMeasure( int* errCode); //㶨λĵλ -SG_APISHARED_EXPORT SSX_pointPoseInfo sx_getLocationPlatePose( +SG_APISHARED_EXPORT SSX_platePoseInfo sx_getLocationPlatePose( std::vector< std::vector>& scanLines, const SSG_cornerParam cornerPara, int* errCode); diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index bda8456..57d1a3c 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -159,32 +159,6 @@ double _getMeanZ(std::vector>& quantiValue, SVzNL3DPoint see return (zSum / hist); } -void _updateROI(SSG_ROIRectD& roi, SVzNL3DPoint& a_pt) -{ - if (a_pt.z > 1E-4) - { - if (roi.left < 0) - { - roi.left = a_pt.x; - roi.right = a_pt.x; - roi.left = a_pt.y; - roi.right = a_pt.y; - } - else - { - if (roi.left > a_pt.x) - roi.left = a_pt.x; - if (roi.right < a_pt.x) - roi.right = a_pt.x; - if (roi.top > a_pt.y) - roi.top = a_pt.y; - if (roi.bottom < a_pt.y) - roi.bottom = a_pt.y; - } - } - return; -} - void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt) { if (a_pt.z > 1E-4) @@ -217,6 +191,33 @@ void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt) return; } +#if 0 +void _updateROI(SSG_ROIRectD& roi, SVzNL3DPoint& a_pt) +{ + if (a_pt.z > 1E-4) + { + if (roi.left < 0) + { + roi.left = a_pt.x; + roi.right = a_pt.x; + roi.left = a_pt.y; + roi.right = a_pt.y; + } + else + { + if (roi.left > a_pt.x) + roi.left = a_pt.x; + if (roi.right < a_pt.x) + roi.right = a_pt.x; + if (roi.top > a_pt.y) + roi.top = a_pt.y; + if (roi.bottom < a_pt.y) + roi.bottom = a_pt.y; + } + } + return; +} + void _updateRenge(SVzNLRange& range, int idx) { if (range.nMin < 0) @@ -274,39 +275,7 @@ void _searchNeighbours( return; } -bool _compareByXValue(WD_workpieceInfo& a, WD_workpieceInfo& b) -{ - return a.center.x < b.center.x; -} -void _getYTopLine( - std::vector< WD_workpieceInfo>& workpieceSrc, - std::vector< WD_workpieceInfo>& firstLine, - std::vector< WD_workpieceInfo>& restWorkpiece, - double yLayerTh) -{ - //搜索Y最小值 - if (workpieceSrc.size() == 0) - return; - - double minY = workpieceSrc[0].center.y; - for (int i = 1; i < (int)workpieceSrc.size(); i++) - { - if (minY > workpieceSrc[i].center.y) - minY = workpieceSrc[i].center.y; - } - double topLayerTh = minY + yLayerTh; - for (int i = 0; i < (int)workpieceSrc.size(); i++) - { - if (workpieceSrc[i].center.y < topLayerTh) - firstLine.push_back(workpieceSrc[i]); - else - restWorkpiece.push_back(workpieceSrc[i]); - } - std::sort(firstLine.begin(), firstLine.end(), _compareByXValue); - return; -} - -void wd_getHoleInfo( +void WD_getHoleInfo( std::vector< std::vector>& scanLines, const SSG_lineSegParam lineSegPara, const SSG_outlierFilterParam filterParam, @@ -595,6 +564,40 @@ void wd_getHoleInfo( validObjects.push_back(objects[i]); } } +#endif + +bool _compareByXValue(WD_workpieceInfo& a, WD_workpieceInfo& b) +{ + return a.center.x < b.center.x; +} +void _getYTopLine( + std::vector< WD_workpieceInfo>& workpieceSrc, + std::vector< WD_workpieceInfo>& firstLine, + std::vector< WD_workpieceInfo>& restWorkpiece, + double yLayerTh) +{ + //搜索Y最小值 + if (workpieceSrc.size() == 0) + return; + + double minY = workpieceSrc[0].center.y; + for (int i = 1; i < (int)workpieceSrc.size(); i++) + { + if (minY > workpieceSrc[i].center.y) + minY = workpieceSrc[i].center.y; + } + double topLayerTh = minY + yLayerTh; + for (int i = 0; i < (int)workpieceSrc.size(); i++) + { + if (workpieceSrc[i].center.y < topLayerTh) + firstLine.push_back(workpieceSrc[i]); + else + restWorkpiece.push_back(workpieceSrc[i]); + } + std::sort(firstLine.begin(), firstLine.end(), _compareByXValue); + return; +} + SSG_ROIRectD _getClusterROI(std::vector< SVzNL3DPosition>& listData) { if (listData.size() == 0) @@ -698,7 +701,7 @@ void wd_workpieceHolePositioning( std::vector segTrees_h; std::vector validObjects; double valieCommonNumRatio = 0.25; - wd_getHoleInfo(scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects); + WD_getHoleInfo(scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects); for (int i = 0; i < lineNum; i++) {