rodAndBarDetection version 1.2.6 :

定位盘位姿计算由RANSAC调整为迭代拟合，测试精度是否有提高

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<SVzNL3DPosition>>& scanLines,
+	const SSG_cornerParam cornerPara,
+	const SSG_outlierFilterParam filterParam,
+	const double rodDiameter,
+	std::vector<std::vector<SWD_rodArcFeature>>& arcFeatures)
+{
+	int lineNum = (int)scanLines.size();
+	int linePtNum = (int)scanLines[0].size();
+	for (int line = 0; line < lineNum; line++)
+	{
+		std::vector<SVzNL3DPosition>& lineData = scanLines[line];
+
+		// Filter outliers from line data
+		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
+		// Extract rod arc features
+		std::vector<SWD_rodArcFeature> 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<SVzNL3DPosition>>& scanLines,
@@ -287,72 +313,98 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
 		}
 	}
 
-	std::vector<std::vector<SWD_segFeature>> arcFeatures;
+	std::vector<std::vector<SWD_rodArcFeature>> arcFeatures;
-	for (int line = 0; line < lineNum; line++)
+	rodAarcFeatueDetection(
-	{
+		data_lines,
-		if (line == 329)
+		cornerPara,
-			int kkk = 1;
+		filterParam,
-		std::vector<SVzNL3DPosition>& lineData = data_lines[line];
+		rodDiameter,
+		arcFeatures);
 
-		//滤波，滤除异常点
-		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
-		std::vector<SWD_segFeature> 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<SWD_segFeatureTree> growTrees;
+	std::vector<SWD_rodArcFeatureTree> rodArcTrees;
-	wd_getSegFeatureGrowingTrees(
+	wd_getRodArcFeatureGrowingTrees(arcFeatures, rodArcTrees, growParam);
-		arcFeatures,
-		growTrees,
-		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;
+				lineIdx = rodArcTrees[i].treeNodes[j].lineIdx;
-				for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++)
+				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;
+				ptIdx = rodArcTrees[i].treeNodes[j].lineIdx;
-				for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++)
+				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<SVzNL3DRangeD> 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<cv::Point3f> out_inliers;
 	Plane res = ransacFitPlane(	Points3ds,	out_inliers	);
 	if (res.C < 0)