From 965d82389c9136add0e2182a982e4f141932d675 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Wed, 10 Jun 2026 20:32:57 +0800 Subject: [PATCH] =?UTF-8?q?rodAndBarDetection=20version=201.3.8=20:=20?= =?UTF-8?q?=E6=96=B0=E7=9A=84=E8=9E=BA=E6=9D=86=E5=AE=9A=E4=BD=8D=E7=AE=97?= =?UTF-8?q?=E6=B3=95=EF=BC=8C=E4=BD=BF=E7=94=A8PCA=E6=96=B9=E6=B3=95?= =?UTF-8?q?=E7=A1=AE=E5=AE=9A=E8=9E=BA=E6=9D=86=E8=BD=B4=E5=90=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../rodAndBarDetection_test.cpp | 15 +- sourceCode/SG_baseAlgo_Export.h | 31 + sourceCode/SG_clustering.cpp | 128 +++- sourceCode/SG_lineFeature.cpp | 95 ++- sourceCode/dataFitting.cpp | 62 +- sourceCode/rodAndBarDetection.cpp | 620 +++++++++++++++++- sourceCode/rodAndBarDetection_Export.h | 12 + 7 files changed, 944 insertions(+), 19 deletions(-) diff --git a/rodAndBarDetection_test/rodAndBarDetection_test.cpp b/rodAndBarDetection_test/rodAndBarDetection_test.cpp index 1ed1e01..1978215 100644 --- a/rodAndBarDetection_test/rodAndBarDetection_test.cpp +++ b/rodAndBarDetection_test/rodAndBarDetection_test.cpp @@ -471,9 +471,10 @@ void _outputRGBDScan_RGBD( sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; //输出法向 + rgb = { 250, 255, 0 }; size = 1; double len1 = 30; - double len2 = 200; + double len2 = 300; lineIdx = 0; for (int i = 0; i < objNum; i++) { @@ -998,11 +999,11 @@ void screwTest(void) const char* ver = wd_rodAndBarDetectionVersion(); printf("ver:%s\n", ver); - for (int grp = 9; grp < SCREW_TEST_GROUP; grp++) + for (int grp = 3; grp <= 6; grp++) { for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx =3; + //fidx =2; char _scan_file[256]; if(0 == grp) @@ -1052,9 +1053,9 @@ void screwTest(void) bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道 int errCode = 0; std::vector screwInfo; - sx_hexHeadScrewMeasure( + sx_hexHeadScrewMeasure_PCA( scanLines, - isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道 + //isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道 cornerParam, filterParam, growParam, @@ -1743,9 +1744,9 @@ typedef enum int main() { - //ESG_testMode testMode = keSG_测试_配天螺杆定位; + ESG_testMode testMode = keSG_测试_配天螺杆定位; //ESG_testMode testMode = keSG_测试_配天定位盘定位; - ESG_testMode testMode = keSG_测试_配天新定位盘定位; + //ESG_testMode testMode = keSG_测试_配天新定位盘定位; //ESG_testMode testMode = keSG_测试_棒材抓取; //ESG_testMode testMode = keSG_测试_筑裕钢筋焊缝定位; diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 501130a..76b9192 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -308,6 +308,17 @@ SG_APISHARED_EXPORT void wd_getXYVertialFeature_dirAngleMethod( const SSG_cornerParam cornerPara, std::vector& xyVerticalFlags // ); +/// +/// ȡϵXYƽˮƽˮƽΣ +/// +SG_APISHARED_EXPORT void wd_getXYHorizontalFeature_dirAngleMethod( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const double maxDistTh, + const double minSegSize, + const SSG_cornerParam cornerPara, + std::vector& xyHorizontalFlags +); /// ȡϵĹյPSM LVTypeFeature, BQȹյ㷨Ļϵİ汾 /// ʹƽм @@ -799,6 +810,13 @@ SG_APISHARED_EXPORT Plane ransacFitPlane( int stop_no_improve = 250 // ٴǰ˳ ); +// 룺3D std::vector +// axis λ, centroid +SG_APISHARED_EXPORT void computeCylinderAxisFromIncompletePCA( + const std::vector& points, + SVzNL3DPoint& vec_axis, + SVzNL3DPoint& vec_centroid); + //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ @@ -884,6 +902,19 @@ SG_APISHARED_EXPORT void wd_pointClustering_speedUp( std::vector>& objClusters //result ); +//һľࣨѾе㣩һо +//ʹSVzNL3DPositionnPointIdxʾ2DϢ16λLine 16λptIdx +//ʱԼ +SG_APISHARED_EXPORT void wd_clusterGrowing_speedUp( + std::vector< SVzNL3DPosition>& pts, + std::vector< SVzNL3DPosition >& a_cluster, + SVzNL3DRangeD& growingROI, //෶Χڼ + int lineNum, int linePtSize, int clusterCheckWin, // + double clusterDist, + int distType, //0 - 2d distance; 1- 3d distance + std::vector< SVzNL3DPosition >& added_points +); + //դϵĴڵڵľ࣬3D //ʹvector2άṹ SG_APISHARED_EXPORT void wd_gridPointClustering( diff --git a/sourceCode/SG_clustering.cpp b/sourceCode/SG_clustering.cpp index d02214e..4546170 100644 --- a/sourceCode/SG_clustering.cpp +++ b/sourceCode/SG_clustering.cpp @@ -138,6 +138,10 @@ void wd_pointClustering_speedUp( std::vector>& objClusters //result ) { + int ptSize = (int)pts.size(); + if (ptSize == 0) + return; + std::vector> indexing2D; indexing2D.resize(lineNum); for (int i = 0; i < lineNum; i++) @@ -152,10 +156,6 @@ void wd_pointClustering_speedUp( 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); @@ -187,6 +187,126 @@ void wd_pointClustering_speedUp( return; } +//һľࣨѾе㣩һо +//ʹSVzNL3DPositionnPointIdxʾ2DϢ16λLine 16λptIdx +//ʱԼ +void wd_clusterGrowing_speedUp( + std::vector< SVzNL3DPosition>& pts, + std::vector< SVzNL3DPosition >& a_cluster, + SVzNL3DRangeD& growingROI, //෶Χڼ + int lineNum, int linePtSize, int clusterCheckWin, // + double clusterDist, + int distType, //0 - 2d distance; 1- 3d distance + std::vector< SVzNL3DPosition >& added_points +) +{ + int ptSize = (int)pts.size(); + if (ptSize == 0) + return; + + 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); + } + // + std::vector flags; + flags.resize(pts.size()); + 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; + flags[i] = 0; + } + + //Ѿĵñ + for (int i = 0; i < (int)a_cluster.size(); i++) + { + int line = a_cluster[i].nPointIdx >> 16; + int ptIdx = a_cluster[i].nPointIdx & 0x0000FFFF; + int backIdx = indexing2D[line][ptIdx]; + flags[backIdx] = -1; + } + + //ROIȡδĵ㣨ѡļ㣩 + std::vector toGrowPtIndice; + for (int i = 0; i < (int)pts.size(); i++) + { + SVzNL3DPosition a_pt = pts[i]; + if (a_pt.pt3D.z < 1e-4) + continue; + + int line = a_pt.nPointIdx >> 16; + int ptIdx = a_pt.nPointIdx & 0x0000FFFF; + if ((a_pt.pt3D.x > growingROI.xRange.min) && (a_pt.pt3D.x < growingROI.xRange.max) && + (a_pt.pt3D.y > growingROI.yRange.min) && (a_pt.pt3D.y < growingROI.yRange.max) && + (a_pt.pt3D.z > growingROI.zRange.min) && (a_pt.pt3D.z < growingROI.zRange.max) && + (flags[i] == 0)) + toGrowPtIndice.push_back(i); + } + if (toGrowPtIndice.size() == 0) + return; + + //toGrowPtsѰµ + std::vector< SVzNL3DPosition > new_seeds; + for (int i = 0; i < (int)toGrowPtIndice.size(); i++) + { + int indexing = toGrowPtIndice[i]; + SVzNL3DPosition a_pt = pts[indexing]; + int pt_line = a_pt.nPointIdx >> 16; + int pt_idx = a_pt.nPointIdx & 0x0000FFFF; + if (flags[indexing] != 0) //ֹظ + continue; + + for (int line = pt_line - clusterCheckWin; line <= pt_line + clusterCheckWin; line++) + { + if ((line >= 0) && (line < lineNum)) + { + for (int ptIdx = pt_idx - clusterCheckWin; ptIdx <= pt_idx + clusterCheckWin; ptIdx++) + { + if ((ptIdx >= 0) && (ptIdx < ptSize)) + { + int backIdx = indexing2D[line][ptIdx]; + if (backIdx < 0) + continue; + if (flags[backIdx] < 0) //оеĵ + { + double dist; + if (0 == distType) + dist = sqrt(pow(a_pt.pt3D.x - pts[backIdx].pt3D.x, 2) + pow(a_pt.pt3D.y - pts[backIdx].pt3D.y, 2)); + else + dist = sqrt(pow(a_pt.pt3D.x - pts[backIdx].pt3D.x, 2) + pow(a_pt.pt3D.y - pts[backIdx].pt3D.y, 2) + pow(a_pt.pt3D.z - pts[backIdx].pt3D.z, 2)); + if (dist < clusterDist) + { + new_seeds.push_back(pts[backIdx]); + flags[indexing] = -1; // + } + } + } + } + } + } + } + if (new_seeds.size() == 0) + return; + + _seedClustering_speedUp( + new_seeds, + pts, + flags, + clusterDist, + distType, + indexing2D, + lineNum, linePtSize, clusterCheckWin); + + added_points.insert(added_points.end(), new_seeds.begin(), new_seeds.end()); + a_cluster.insert(a_cluster.end(), new_seeds.begin(), new_seeds.end()); + return; +} + //ľ void wd_gridPointClustering( std::vector>& featureMask, diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index e12029d..fccc73e 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -5706,12 +5706,7 @@ void wd_getLineCorerFeature( /// nPointIdx¶Feature /// 㷨̣ /// 1ǰǺͺ -/// 2ͬĹս -/// 2Zֵ -/// 2սǣ˳ʱΪʱΪ -/// 3սǵļֵ -/// 4жϹսǷΪ -/// +/// 2ȥǰǺͺǴ޵ĵ /// void wd_getXYVertialFeature_dirAngleMethod( std::vector< SVzNL3DPosition>& lineData, @@ -5741,6 +5736,94 @@ void wd_getXYVertialFeature_dirAngleMethod( return; } +/// +/// ȡϵXYƽˮƽˮƽΣ +/// seg˵㣺z +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2ȥǰǺͺС޵ĵ +/// +void wd_getXYHorizontalFeature_dirAngleMethod( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const double maxDistTh, + const double minSegSize, + const SSG_cornerParam cornerPara, + std::vector& xyHorizontalFlags +) +{ + if (lineIdx == 562) + int kkk = 1; + double maxHorizontalAngle = cornerPara.cornerTh; //arcÿתֵ + + xyHorizontalFlags.resize(lineData.size()); + std::fill(xyHorizontalFlags.begin(), xyHorizontalFlags.end(), 0); + + //zԷֶ + std::vector segs; + wd_lineDataSegment_dist_2( + lineData, + segs, + maxDistTh, + minSegSize + ); + //ǰǺͺ + std::vector< SSG_pntDirAngle> ptDirAngles; + _computeDirAngle_perSeg_2(lineData, segs, cornerPara, ptDirAngles); + + for (int i = 0; i < (int)ptDirAngles.size(); i++) + { + if ( (ptDirAngles[i].type < 0) || (ptDirAngles[i].pntIdx < 0)) + continue; + + if ((abs(ptDirAngles[i].backwardAngle) < maxHorizontalAngle) && (abs(ptDirAngles[i].forwardAngle) < maxHorizontalAngle)) + xyHorizontalFlags[i] = 1; + } + //Ե + for (int i = 1; i < (int)ptDirAngles.size() - 1; i++) + { + if ((xyHorizontalFlags[i - 1] == 0) && (xyHorizontalFlags[i] == 1)) + { + int sIdx = ptDirAngles[i].backwardPntIdx; + bool isEdge = true; + for (int j = sIdx; j < i; j++) + { + if (ptDirAngles[j].pntIdx >= 0) + { + isEdge = false; + break; + } + } + if (true == isEdge) + { + for (int j = sIdx; j < i; j++) + xyHorizontalFlags[j] = 1; + } + } + if ((xyHorizontalFlags[i + 1] == 0) && (xyHorizontalFlags[i] == 1)) + { + int eIdx = ptDirAngles[i].forwardPntIdx; + bool isEdge = true; + for (int j = i+1; j <= eIdx; j++) + { + if (ptDirAngles[j].pntIdx >= 0) + { + isEdge = false; + break; + } + } + if (true == isEdge) + { + for (int j = i+1; j <= eIdx; j++) + xyHorizontalFlags[j] = 1; + } + } + } + + return; +} + /// ȡϵĹյPSM LVTypeFeature, BQȹյ㷨Ļϵİ汾 /// ʹƽм /// nPointIdx¶Feature diff --git a/sourceCode/dataFitting.cpp b/sourceCode/dataFitting.cpp index a736b9e..5e8fda3 100644 --- a/sourceCode/dataFitting.cpp +++ b/sourceCode/dataFitting.cpp @@ -816,4 +816,64 @@ Plane ransacFitPlane( } return best_plane; -} \ No newline at end of file +} + +// 룺3D std::vector +// axis λ, centroid +void computeCylinderAxisFromIncompletePCA( + const std::vector& src_points, + SVzNL3DPoint& vec_axis, + SVzNL3DPoint& vec_centroid) +{ + vec_axis = { 0.0, 0.0, 0.0 }; + vec_centroid = { 0.0, 0.0, 0.0 }; + + if (src_points.empty()) { + return; + } + + std::vector points; + for (int i = 0; i < (int)src_points.size(); i++) + points.emplace_back(src_points[i].pt3D.x, src_points[i].pt3D.y, src_points[i].pt3D.z); + + Eigen::Vector3d axis, centroid; + // -------------------------- + // 1. + Ļ + // -------------------------- + centroid.setZero(); + for (const auto& p : points) centroid += p; + centroid /= points.size(); + + std::vector centered_pts; + centered_pts.reserve(points.size()); + for (const auto& p : points) centered_pts.emplace_back(p - centroid); + + // -------------------------- + // 2. 3x3 Э + // -------------------------- + Eigen::Matrix3d cov = Eigen::Matrix3d::Zero(); + for (const auto& p : centered_pts) cov += p * p.transpose(); + cov /= centered_pts.size() - 1; + + // -------------------------- + // 3. ֵֽ + // -------------------------- + Eigen::SelfAdjointEigenSolver solver(cov); + Eigen::Vector3d eig_vals = solver.eigenvalues(); // + Eigen::Matrix3d eig_vecs = solver.eigenvectors(); + + // -------------------------- + // ؼȱԲ ȡֵӦ = + // -------------------------- + axis = eig_vecs.col(2); // eigenvalues [2] + axis.normalize(); + + vec_axis.x = axis[0]; + vec_axis.y = axis[1]; + vec_axis.z = axis[2]; + + vec_centroid.x = centroid[0]; + vec_centroid.y = centroid[1]; + vec_centroid.z = centroid[2]; + return; +} diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp index dc75f6b..4ac90fe 100644 --- a/sourceCode/rodAndBarDetection.cpp +++ b/sourceCode/rodAndBarDetection.cpp @@ -28,7 +28,8 @@ //version 1.3.5 : µĶλIJռfloatijdouble ,PC3588 //version 1.3.6 : µĶλIJܣŻǰĴֱȥ㷨֤ȷ //version 1.3.7 : µĶλIJܣһŻڲŻ˴ֱȥЧ -std::string m_strVersion = "1.3.7"; +//version 1.3.8 : µݸ˶λ㷨ʹPCAȷݸ +std::string m_strVersion = "1.3.8"; const char* wd_rodAndBarDetectionVersion(void) { return m_strVersion.c_str(); @@ -912,6 +913,623 @@ SVzNL3DRangeD _getPointCloudROI(std::vector& scanData) return; } +//PCAݸ˶˲ĵλ +//sx_hexHeadScrewMeasure()㷨ϣ1ȥˮƽΣ2ʹPCA +void sx_hexHeadScrewMeasure_PCA( + std::vector< std::vector>& scanLines, + //bool isHorizonScan, //true:ƽв۵false:ߴֱ۵ + const SSG_cornerParam cornerPara, + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + double rodDiameter, + std::vector& screwInfo, + int* errCode) +{ + *errCode = 0; + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + { + *errCode = SG_ERR_3D_DATA_NULL; + return; + } + + int linePtNum = (int)scanLines[0].size(); + + //жݸʽǷΪgrid㷨ֻܴgridݸʽ + bool isGridData = true; + for (int line = 0; line < lineNum; line++) + { + if (linePtNum != (int)scanLines[line].size()) + { + isGridData = false; + break; + } + } + if (false == isGridData)//ݲʽ + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return; + } + + //Copyˮƽɨ + std::vector< std::vector> scanLines_copy; + scanLines_copy.resize(scanLines.size()); + + std::vector< std::vector> scanLines_h; + scanLines_h.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + scanLines_h[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + scanLines_copy[line].insert(scanLines_copy[line].end(), scanLines[line].begin(), scanLines[line].end()); + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã + scanLines_h[j][line] = scanLines[line][j]; + scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y; + scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + for (int line = 0; line < linePtNum; line++) + { + for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++) + scanLines_h[line][j].nPointIdx = j; + } + + //㷨̣ + //1ˮƽݲȥ + //2 + //3ǰĿ + //ڲ + SSG_cornerParam removeHorizonPara = cornerPara; + removeHorizonPara.scale = 5.0; + removeHorizonPara.cornerTh = 45; + double maxDistTh = 10.0; + double minSegSize = 3.0; //С3mmsegmentȱ˵ + + std::vector> flags; + flags.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + flags[i].resize(linePtNum); + std::fill(flags[i].begin(), flags[i].end(), 0); + } + std::vector> zHorizonFlags; + for (int line = 0; line < lineNum; line++) + { + if (line == 248) + int kkk = 1; + std::vector line_horizontalFlags; + wd_getXYHorizontalFeature_dirAngleMethod( + scanLines_copy[line], + line, + maxDistTh, + minSegSize, + removeHorizonPara, + line_horizontalFlags + ); + zHorizonFlags.push_back(line_horizontalFlags); + + for (int i = 0; i < (int)line_horizontalFlags.size(); i++) + { + if (line_horizontalFlags[i] > 0) + flags[line][i] = 1; + } + } + +#if 0 + std::vector> zHorizonFlags_h; + for (int line = 0; line < linePtNum; line++) + { + if (line == 1177) + int kkk = 1; + std::vector line_horizontalFlags; + wd_getXYHorizontalFeature_dirAngleMethod( + scanLines_h[line], + line, + removeHorizonPara, + line_horizontalFlags + ); + zHorizonFlags_h.push_back(line_horizontalFlags); + + for (int i = 0; i < (int)line_horizontalFlags.size(); i++) + { + if (line_horizontalFlags[i] > 0) + flags[i][line] = 1; + } + } +#endif + + //ȥ + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if (flags[line][j] > 0) + { + scanLines_copy[line][j].pt3D.z = 0; + scanLines_h[j][line].pt3D.z = 0; + } + } + } + //һ + SSG_lineSegParam lineSegPara; + lineSegPara.distScale = 10.0; + lineSegPara.segGapTh_y = 10.0; + lineSegPara.segGapTh_z = 10.0; + const int minSegLen = 5; + for (int line = 0; line < lineNum; line++) + { + std::vector segs; + wd_getLineDataIntervals( + scanLines_copy[line], + lineSegPara, + segs); + for (int i = 0; i < (int)segs.size(); i++) + { + if (segs[i].len <= minSegLen) + { + int idx0 = segs[i].start; + for (int j = 0; j < segs[i].len; j++) + flags[line][idx0 + j] = 1; + } + } + } + for (int line = 0; line < linePtNum; line++) + { + std::vector segs; + wd_getLineDataIntervals( + scanLines_h[line], + lineSegPara, + segs); + for (int i = 0; i < (int)segs.size(); i++) + { + if (segs[i].len <= minSegLen) + { + int idx0 = segs[i].start; + for (int j = 0; j < segs[i].len; j++) + flags[idx0 + j][line] = 1; + } + } + } + + //ע + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + scanLines_copy[line][j].nPointIdx = 0; //ԭʼݵ0תʹã + } + //ֱ߶ȥ + std::vector< SVzNL3DPosition> validPoints; + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if (flags[line][j] > 0) + scanLines_copy[line][j].pt3D.z = 0; + + if (scanLines_copy[line][j].pt3D.z > 1e-4) + { + SVzNL3DPosition a_vldPt; + a_vldPt.pt3D = scanLines_copy[line][j].pt3D; + a_vldPt.nPointIdx = (line << 16) | (j & 0xffff); + validPoints.push_back(a_vldPt); + } + } + } + + // + //ڲ + double minObjSize_w = 150; + double minScrewLen = 50; + + int clusterCheckWin = 5; + double clusterDist = 2.5; + int distType = 1; //0 - 2d distance; 1- 3d distance + std::vector> objClusters; //result + wd_pointClustering_speedUp( + validPoints, + lineNum, linePtNum, clusterCheckWin, // + clusterDist, + distType, + objClusters //result + ); + + //ʹclusterROIϢĿ꣬ZСķҪĿжΪмݸ + int clusterSize = (int)objClusters.size(); + std::vector objROIs; + for (int i = 0; i < clusterSize; i++) + { + // Initialize min and max values + // Calculate X, Y and Z ranges + SVzNL3DRangeD a_roi3D; + a_roi3D.xRange.min = DBL_MAX; a_roi3D.xRange.max = -DBL_MAX; + a_roi3D.yRange.min = DBL_MAX; a_roi3D.yRange.max = -DBL_MAX; + a_roi3D.zRange.min = DBL_MAX; a_roi3D.zRange.max = -DBL_MAX; + + int nodeNum = (int)objClusters[i].size(); + for (int j = 0; j < nodeNum; j++) + { + SVzNL3DPosition& a_pt = objClusters[i][j]; + + if (a_pt.pt3D.z > 1e-4) + { + a_roi3D.xRange.min = std::min(a_roi3D.xRange.min, a_pt.pt3D.x); + a_roi3D.xRange.max = std::max(a_roi3D.xRange.max, a_pt.pt3D.x); + a_roi3D.yRange.min = std::min(a_roi3D.yRange.min, a_pt.pt3D.y); + a_roi3D.yRange.max = std::max(a_roi3D.yRange.max, a_pt.pt3D.y); + a_roi3D.zRange.min = std::min(a_roi3D.zRange.min, a_pt.pt3D.z); + a_roi3D.zRange.max = std::max(a_roi3D.zRange.max, a_pt.pt3D.z); + } + } + objROIs.push_back(a_roi3D); + } + std::vector objCluster; + for (int i = 0; i < clusterSize; i++) + { + double x_width = objROIs[i].xRange.max - objROIs[i].xRange.min; + double y_width = objROIs[i].yRange.max - objROIs[i].yRange.min; + double z_width = objROIs[i].zRange.max - objROIs[i].zRange.min; + if ((x_width < rodDiameter * 3.0) && (y_width < rodDiameter*3.0) && (z_width > minScrewLen) && (objClusters[i].size() > 100)) + objCluster.push_back(i); + } + + //ȡǰ + int targetClusterID = -1; + for (int i = 0; i < objCluster.size(); i++) + { + int clusterIdx = objCluster[i]; + if (targetClusterID < 0) + targetClusterID = clusterIdx; + else if(objROIs[targetClusterID].zRange.min > objROIs[clusterIdx].zRange.min) + targetClusterID = clusterIdx; + } + if(targetClusterID < 0) + { + *errCode = SX_ERR_ZERO_OBJECTS; + return; + } + //PCAǰȥݸ˸ֹӰPCA + double zMax = objROIs[targetClusterID].zRange.max - 30; //ȥݸ˸Ӱ + double zMin = objROIs[targetClusterID].zRange.min + 30; //ȥݸ˸Ӱ + std::vector< SVzNL3DPosition > PCA_points; + for (int i = 0; i < objClusters[targetClusterID].size(); i++) + { + if ((objClusters[targetClusterID][i].pt3D.z < zMax) && (objClusters[targetClusterID][i].pt3D.z > zMin)) + PCA_points.push_back(objClusters[targetClusterID][i]); + } + + SVzNL3DPoint vec_axis, vec_centroid; + //PCA + computeCylinderAxisFromIncompletePCA( + PCA_points, + vec_axis, + vec_centroid); + if (vec_axis.z < 0) //ȷΨһ + vec_axis = { -vec_axis.x, -vec_axis.y, -vec_axis.z }; + + //ͶӰ + //ת + SVzNL3DPoint vector1 = vec_axis; + SVzNL3DPoint vector2 = { 0, 0, 1.0 }; + SSG_planeCalibPara rotatePara = wd_computeRTMatrix(vector1, vector2); + + ///˴ǵбǰݸ˺ͶӰ»вͬҪһ + //һΣȷȷݸ + std::vector> rotate_objClusters; //result + rotate_objClusters.resize(objClusters.size()); + std::vector rotate_objROIs; + for (int i = 0; i < clusterSize; i++) + { + rotate_objClusters[i].resize(objClusters[i].size()); + // Initialize min and max values + // Calculate X, Y and Z ranges + SVzNL3DRangeD a_roi3D; + a_roi3D.xRange.min = DBL_MAX; a_roi3D.xRange.max = -DBL_MAX; + a_roi3D.yRange.min = DBL_MAX; a_roi3D.yRange.max = -DBL_MAX; + a_roi3D.zRange.min = DBL_MAX; a_roi3D.zRange.max = -DBL_MAX; + + int nodeNum = (int)objClusters[i].size(); + for (int j = 0; j < nodeNum; j++) + { + SVzNL3DPosition& a_pt = objClusters[i][j]; + + if (a_pt.pt3D.z > 1e-4) + { + SVzNL3DPosition rotate_pt; + rotate_pt.nPointIdx = a_pt.nPointIdx; + rotate_pt.pt3D = _translatePoint(a_pt.pt3D, rotatePara.planeCalib); + rotate_objClusters[i][j] = rotate_pt; + + a_roi3D.xRange.min = std::min(a_roi3D.xRange.min, rotate_pt.pt3D.x); + a_roi3D.xRange.max = std::max(a_roi3D.xRange.max, rotate_pt.pt3D.x); + a_roi3D.yRange.min = std::min(a_roi3D.yRange.min, rotate_pt.pt3D.y); + a_roi3D.yRange.max = std::max(a_roi3D.yRange.max, rotate_pt.pt3D.y); + a_roi3D.zRange.min = std::min(a_roi3D.zRange.min, rotate_pt.pt3D.z); + a_roi3D.zRange.max = std::max(a_roi3D.zRange.max, rotate_pt.pt3D.z); + } + } + rotate_objROIs.push_back(a_roi3D); + } + //ȷZСĿ + //ȡǰ + targetClusterID = -1; + for (int i = 0; i < objCluster.size(); i++) + { + int clusterIdx = objCluster[i]; + if (targetClusterID < 0) + targetClusterID = clusterIdx; + else if (rotate_objROIs[targetClusterID].zRange.min > rotate_objROIs[clusterIdx].zRange.min) + targetClusterID = clusterIdx; + } + if (targetClusterID < 0) + { + *errCode = SX_ERR_ZERO_OBJECTS; + return; + } + + //PCAǰȥݸ˸ֹӰPCA + zMax = objROIs[targetClusterID].zRange.max - 30.0; //ȥݸ˸Ӱ + zMin = objROIs[targetClusterID].zRange.min + 30.0; //ȥݸ˸Ӱ + PCA_points.clear(); + for (int i = 0; i < objClusters[targetClusterID].size(); i++) + { + if ((objClusters[targetClusterID][i].pt3D.z < zMax) && (objClusters[targetClusterID][i].pt3D.z > zMin)) + PCA_points.push_back(objClusters[targetClusterID][i]); + } + //ʹPCA򣬴˴ʹתǰ + computeCylinderAxisFromIncompletePCA( + PCA_points, + vec_axis, + vec_centroid); + if (vec_axis.z < 0) //ȷΨһ + vec_axis = { -vec_axis.x, -vec_axis.y, -vec_axis.z }; + + //ԭʼݵȥĵ + std::vector< SVzNL3DPosition> raw_validPoints; + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if (scanLines[line][j].pt3D.z > 1e-4) + { + SVzNL3DPosition a_vldPt; + a_vldPt.pt3D = scanLines[line][j].pt3D; + a_vldPt.nPointIdx = (line << 16) | (j & 0xffff); + raw_validPoints.push_back(a_vldPt); + } + } + } + + // 2 + //δתĵмȥˮƽбȥ + SVzNL3DRangeD growingROI = objROIs[targetClusterID]; + growingROI.xRange.min -= rodDiameter; + growingROI.xRange.max += rodDiameter; + growingROI.yRange.min -= rodDiameter; + growingROI.yRange.max += rodDiameter; + growingROI.zRange.max = (growingROI.zRange.max + growingROI.zRange.min) / 2; //Zм + growingROI.zRange.min -= rodDiameter; + std::vector< SVzNL3DPosition>& screw_cluster = objClusters[targetClusterID]; + std::vector< SVzNL3DPosition> added_points; + wd_clusterGrowing_speedUp( + raw_validPoints, + screw_cluster, + growingROI, //෶Χڼ + lineNum, linePtNum, clusterCheckWin, // + clusterDist, + distType, //0 - 2d distance; 1- 3d distance + added_points + ); +#if 1 + for (int i = 0; i < (int)added_points.size(); i++) + { + int line = added_points[i].nPointIdx >> 16; + int ptIdx = added_points[i].nPointIdx & 0x0000FFFF; + scanLines_copy[line][ptIdx].pt3D = added_points[i].pt3D; //ָ + } +#endif + SVzNL3DPoint rotate_centroid = _ptRotate(vec_centroid, rotatePara.planeCalib); + std::vector< SWD3DPointPostion> roiProjectionData; + //ͶӰȡROIڵ + for (int i = 0; i < (int)objClusters[targetClusterID].size(); i++) + { + SVzNL3DPoint a_pt = objClusters[targetClusterID][i].pt3D; + if (a_pt.z < 1e-4) + continue; + int line = objClusters[targetClusterID][i].nPointIdx >> 16; + int ptIdx = objClusters[targetClusterID][i].nPointIdx & 0x0000FFFF; + double x = a_pt.x * rotatePara.planeCalib[0] + a_pt.y * rotatePara.planeCalib[1] + a_pt.z * rotatePara.planeCalib[2]; + double y = a_pt.x * rotatePara.planeCalib[3] + a_pt.y * rotatePara.planeCalib[4] + a_pt.z * rotatePara.planeCalib[5]; + double z = a_pt.x * rotatePara.planeCalib[6] + a_pt.y * rotatePara.planeCalib[7] + a_pt.z * rotatePara.planeCalib[8]; + if (z <= rotate_centroid.z) + { + SWD3DPointPostion projectPt; + projectPt.lineIdx = line; + projectPt.ptIdx = ptIdx; + projectPt.point.x = x; + projectPt.point.y = y; + projectPt.point.z = z; + roiProjectionData.push_back(projectPt); + } + } + + bool dirInverting = false; + //ȡ + SVzNLRangeD zRange = getZRange(roiProjectionData); + SVzNLRangeD cutZRange; + if (false == dirInverting) + { + cutZRange.min = zRange.min; + cutZRange.max = zRange.min + 10.0; //5mmĶ + } + else + { + cutZRange.max = zRange.max; + cutZRange.min = zRange.max - 10.0; //5mmĶ + } + std::vector surfacePoints; + std::vector>addrMapping; + addrMapping.resize(scanLines.size()); + for (int i = 0; i < (int)scanLines.size(); i++) + { + addrMapping[i].resize(scanLines[i].size()); + std::fill(addrMapping[i].begin(), addrMapping[i].end(), -1); + } + zCutPointClouds(roiProjectionData, cutZRange, surfacePoints, addrMapping); + //ĵ + SWD3DPointPostion projectionCenter;// = getXoYCentroid(surfacePoints); + SVzNL3DRangeD roi3D = _getPointCloudROI(surfacePoints); + //XYƽϵ + double sum_x = 0, sum_y = 0; + int sum_size = 0; + for (int i = 0; i < (int)surfacePoints.size(); i++) + { + if (surfacePoints[i].point.z > 1e-4) + { + sum_x += surfacePoints[i].point.x; + sum_y += surfacePoints[i].point.y; + sum_size++; + } + } + if (sum_size == 0) + { + *errCode = SX_ERR_ZERO_OBJECTS; + return; + } + projectionCenter.lineIdx = -1; + projectionCenter.ptIdx = -1; + projectionCenter.point.x = sum_x / sum_size; // (roi3D.xRange.min + roi3D.xRange.max) / 2; + projectionCenter.point.y = sum_y / sum_size; // (roi3D.yRange.min + roi3D.yRange.max) / 2; + projectionCenter.point.z = zRange.min; + //projectionCenterΪ5mmzĵĵΪĵ + double searchR = 5.0; + int centerIdx = -1; + double maxZ = -1; + for (int i = 0; i < (int)surfacePoints.size(); i++) + { + double dist = sqrt(pow(surfacePoints[i].point.x - projectionCenter.point.x, 2) + pow(surfacePoints[i].point.y - projectionCenter.point.y, 2)); + if (dist < searchR) + { + if (centerIdx < 0) + { + centerIdx = i; + maxZ = surfacePoints[i].point.z; + } + else + { + + if (((false == dirInverting) && (surfacePoints[centerIdx].point.z < surfacePoints[i].point.z)) || + ((true == dirInverting) && (surfacePoints[centerIdx].point.z > surfacePoints[i].point.z))) + { + centerIdx = i; + maxZ = surfacePoints[i].point.z; + } + } + } + } + if (centerIdx < 0) + { + *errCode = SX_ERR_ZERO_OBJECTS; + return; + } + int centerIdx_test = addrMapping[surfacePoints[centerIdx].lineIdx][surfacePoints[centerIdx].ptIdx]; + //һ + projectionCenter.lineIdx = surfacePoints[centerIdx].lineIdx; + projectionCenter.ptIdx = surfacePoints[centerIdx].ptIdx; + int sLine = projectionCenter.lineIdx - 5; + if (sLine < 0) + sLine = 0; + int eLine = projectionCenter.lineIdx + 5; + if (eLine >= (int)scanLines.size()) + eLine = (int)scanLines.size() - 1; + int sPtIdx = projectionCenter.ptIdx - 5; + if (sPtIdx < 0) + sPtIdx = 0; + int ePtIdx = projectionCenter.ptIdx + 5; + if (ePtIdx >= (int)scanLines[0].size()) + ePtIdx = (int)scanLines[0].size() - 1; + + int objLine = -1; + int objPtIdx = -1; + maxZ = -1; + for (int line = sLine; line <= eLine; line++) + { + for (int ptIdx = sPtIdx; ptIdx <= ePtIdx; ptIdx++) + { + int idx_center = addrMapping[line][ptIdx]; + if (idx_center < 0) + continue; + + int sL = line - 1; + if (sL < 0) + sL = 0; + int eL = line + 1; + if (eL >= (int)scanLines.size()) + eL = (int)scanLines.size() - 1; + int sPt = ptIdx - 1; + if (sPt < 0) + sPt = 0; + int ePt = ptIdx + 1; + if (ePt >= (int)scanLines[0].size()) + ePt = (int)scanLines[0].size() - 1; + int size = 0; + double sumZ = 0; + for (int i = sL; i <= eL; i++) + { + for (int j = sPt; j <= ePt; j++) + { + int idx = addrMapping[i][j]; + if (idx >= 0) + { + if (roiProjectionData[idx].point.z > 1e-4) + { + sumZ += roiProjectionData[idx].point.z; + size++; + } + } + } + } + if (size > 0) + { + sumZ = sumZ / size; + if (maxZ < 0) + { + maxZ = sumZ; + objLine = line; + objPtIdx = ptIdx; + } + else if (((false == dirInverting) && (maxZ < sumZ)) || ((true == dirInverting) && (maxZ > sumZ))) + { + maxZ = sumZ; + objLine = line; + objPtIdx = ptIdx; + } + + } + } + } + if ((objLine >= 0) && (objPtIdx >= 0)) + centerIdx = addrMapping[objLine][objPtIdx]; + //תԭϵ + SVzNL3DPoint surfaceCenter = _ptRotate(roiProjectionData[centerIdx].point, rotatePara.invRMatrix); + //RodϢ + SSX_rodPoseInfo a_rod; + a_rod.center = surfaceCenter; + a_rod.axialDir = vec_axis; + screwInfo.push_back(a_rod); + +#if 0 + //scanlines_copyݣڲ + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + scanLines[line][j].pt3D = scanLines_copy[line][j].pt3D; + } +#endif + return; +} + double _getListMeanZ(std::vector< SVzNL3DPosition>& listData, SVzNLRangeD& zRange) { if (listData.size() == 0) diff --git a/sourceCode/rodAndBarDetection_Export.h b/sourceCode/rodAndBarDetection_Export.h index 56251a0..75d24a1 100644 --- a/sourceCode/rodAndBarDetection_Export.h +++ b/sourceCode/rodAndBarDetection_Export.h @@ -84,6 +84,18 @@ SG_APISHARED_EXPORT void sx_hexHeadScrewMeasure( std::vector& screwInfo, int* errCode); +//PCAݸ˶˲ĵλ +//sx_hexHeadScrewMeasure()㷨ϣ1ȥˮƽΣ2ʹPCA +SG_APISHARED_EXPORT void sx_hexHeadScrewMeasure_PCA( + std::vector< std::vector>& scanLines, + //bool isHorizonScan, //true:ƽв۵false:ߴֱ۵ + const SSG_cornerParam cornerPara, + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + double rodDiameter, + std::vector& screwInfo, + int* errCode); + //㶨λĵλ SG_APISHARED_EXPORT SSX_platePoseInfo sx_getLocationPlatePose( std::vector< std::vector>& scanLines,