From bf95cd65782ec7b6a2dbd465cb3e51d2e6c1f887 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Fri, 8 May 2026 06:10:42 +0800 Subject: [PATCH] =?UTF-8?q?rodAndBarDetection=20version=201.2.6=20:=20?= =?UTF-8?q?=E5=AE=9A=E4=BD=8D=E7=9B=98=E4=BD=8D=E5=A7=BF=E8=AE=A1=E7=AE=97?= =?UTF-8?q?=E7=94=B1RANSAC=E8=B0=83=E6=95=B4=E4=B8=BA=E8=BF=AD=E4=BB=A3?= =?UTF-8?q?=E6=8B=9F=E5=90=88=EF=BC=8C=E6=B5=8B=E8=AF=95=E7=B2=BE=E5=BA=A6?= =?UTF-8?q?=E6=98=AF=E5=90=A6=E6=9C=89=E6=8F=90=E9=AB=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/rodAndBarDetection.cpp | 131 +++++++++++++++++++++--------- 1 file changed, 92 insertions(+), 39 deletions(-) diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp index 6972c28..6605cc7 100644 --- a/sourceCode/rodAndBarDetection.cpp +++ b/sourceCode/rodAndBarDetection.cpp @@ -14,7 +14,8 @@ //version 1.2.3 : 根据定向盘轮廓的直线段确定向右的向量 //version 1.2.4 : 根据定向盘左上点和右下点确定姿态向量 //version 1.2.5 : 添加筑裕钢筋焊缝提取API -std::string m_strVersion = "1.2.5"; +//version 1.2.6 : 定位盘位姿计算由RANSAC调整为迭代拟合，测试精度是否有提高 +std::string m_strVersion = "1.2.6"; const char* wd_rodAndBarDetectionVersion(void) { return m_strVersion.c_str(); @@ -210,6 +211,31 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) return _r_pt; } + +void rodAarcFeatueDetection( + std::vector< std::vector>& scanLines, + const SSG_cornerParam cornerPara, + const SSG_outlierFilterParam filterParam, + const double rodDiameter, + std::vector>& arcFeatures) +{ + int lineNum = (int)scanLines.size(); + int linePtNum = (int)scanLines[0].size(); + for (int line = 0; line < lineNum; line++) + { + std::vector& lineData = scanLines[line]; + + // Filter outliers from line data + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); + // Extract rod arc features + std::vector line_rodArcs; + wd_getRodArcFeature_peakCornerMethod(lineData, line, rodDiameter / 2, cornerPara, line_rodArcs); + arcFeatures.push_back(line_rodArcs); + } + return; +} + + //计算螺杆端部中心点位姿 void sx_hexHeadScrewMeasure( std::vector< std::vector>& scanLines, @@ -287,72 +313,98 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) } } - std::vector> arcFeatures; - for (int line = 0; line < lineNum; line++) - { - if (line == 329) - int kkk = 1; - std::vector& lineData = data_lines[line]; + std::vector> arcFeatures; + rodAarcFeatueDetection( + data_lines, + cornerPara, + filterParam, + rodDiameter, + arcFeatures); - //滤波，滤除异常点 - sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); - std::vector line_ringArcs; - int dataSize = (int)lineData.size(); - SVzNLRangeD arcWidth; - arcWidth.min = rodDiameter / 2; - arcWidth.max = rodDiameter * 1.5; - //提取Arc特征 - wd_getRingArcFeature( - lineData, - line, //当前扫描线序号 - cornerPara, - arcWidth, //环宽度，以半径为基准，对应60度角 - line_ringArcs //环 - ); - arcFeatures.push_back(line_ringArcs); - } //特征生长 - std::vector growTrees; - wd_getSegFeatureGrowingTrees( - arcFeatures, - growTrees, - growParam); + std::vector rodArcTrees; + wd_getRodArcFeatureGrowingTrees(arcFeatures, rodArcTrees, growParam); - if (growTrees.size() == 0) + if (rodArcTrees.size() == 0) { *errCode = SG_ERR_NOT_GRID_FORMAT; return; } - int objNum = (int)growTrees.size(); + int objNum = (int)rodArcTrees.size(); //置标志，用于debug for (int i = 0; i < objNum; i++) { - int nodeNum = (int)growTrees[i].treeNodes.size(); + int nodeNum = (int)rodArcTrees[i].treeNodes.size(); for (int j = 0; j < nodeNum; j++) { int lineIdx, ptIdx; if (false == isHorizonScan) { - lineIdx = growTrees[i].treeNodes[j].lineIdx; - for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++) + lineIdx = rodArcTrees[i].treeNodes[j].lineIdx; + for (int m = rodArcTrees[i].treeNodes[j].startPtIdx; m <= rodArcTrees[i].treeNodes[j].endPtIdx; m++) { ptIdx = m; - scanLines[lineIdx][ptIdx].nPointIdx = 1; + if( (m == rodArcTrees[i].treeNodes[j].startPtIdx) || (m == rodArcTrees[i].treeNodes[j].endPtIdx) || + (m == rodArcTrees[i].treeNodes[j].peakPtIdx)) + scanLines[lineIdx][ptIdx].nPointIdx = 2; + else + scanLines[lineIdx][ptIdx].nPointIdx = 1; } } else { - ptIdx = growTrees[i].treeNodes[j].lineIdx; - for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++) + ptIdx = rodArcTrees[i].treeNodes[j].lineIdx; + for (int m = rodArcTrees[i].treeNodes[j].startPtIdx; m <= rodArcTrees[i].treeNodes[j].endPtIdx; m++) { lineIdx = m; - scanLines[lineIdx][ptIdx].nPointIdx = 1; + if ((m == rodArcTrees[i].treeNodes[j].startPtIdx) || (m == rodArcTrees[i].treeNodes[j].endPtIdx) || + (m == rodArcTrees[i].treeNodes[j].peakPtIdx)) + scanLines[lineIdx][ptIdx].nPointIdx = 2; + else + scanLines[lineIdx][ptIdx].nPointIdx = 1; } } } } + //确定目标 + //计算ROI，z最小的生长树为目标 + std::vector objROIs; + for (int i = 0; i < objNum; 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_MIN; + a_roi3D.yRange.min = DBL_MAX; a_roi3D.yRange.max = DBL_MIN; + a_roi3D.zRange.min = DBL_MAX; a_roi3D.zRange.max = DBL_MIN; + int nodeNum = (int)rodArcTrees[i].treeNodes.size(); + for (int j = 0; j < nodeNum; j++) + { + SVzNL3DPoint peak; + int lineIdx, ptIdx; + if (false == isHorizonScan) + { + int lineIdx = rodArcTrees[i].treeNodes[j].lineIdx; + int ptIdx = rodArcTrees[i].treeNodes[j].peakPtIdx; + SVzNL3DPoint peak = scanLines[lineIdx][ptIdx].pt3D; + } + else + { + int ptIdx = rodArcTrees[i].treeNodes[j].lineIdx; + int lineIdx = rodArcTrees[i].treeNodes[j].peakPtIdx; + SVzNL3DPoint peak = scanLines[lineIdx][ptIdx].pt3D; + } + a_roi3D.xRange.min = std::min(a_roi3D.xRange.min, peak.x); + a_roi3D.xRange.max = std::max(a_roi3D.xRange.max, peak.x); + a_roi3D.yRange.min = std::min(a_roi3D.yRange.min, peak.y); + a_roi3D.yRange.max = std::max(a_roi3D.yRange.max, peak.y); + a_roi3D.zRange.min = std::min(a_roi3D.zRange.min, peak.z); + a_roi3D.zRange.max = std::max(a_roi3D.zRange.max, peak.z); + } + } +#if 0 //逐个目标处理 for (int i = 0; i < objNum; i++) { @@ -455,6 +507,7 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) //screwInfo[i].rotateAngle += 90; } } +#endif return; } @@ -998,7 +1051,7 @@ SSX_platePoseInfo sx_getLocationPlatePose( } //计算面参数: z = Ax + By + C //res: [0]=A, [1]= B, [2]=-1.0, [3]=C, -#if 1 +#if 0 std::vector out_inliers; Plane res = ransacFitPlane( Points3ds, out_inliers ); if (res.C < 0)