rodAndBarDetection code update。在调试前备份算法代码

sourceCode/SG_baseAlgo_Export.h

@@ -410,6 +410,18 @@ SG_APISHARED_EXPORT void lineFitting_abc(
     double* _b,
     double* _c);
 
+/**
+ * @brief 空间直线最小二乘拟合
+ * @param points 输入的三维点集（至少2个点，否则无意义）
+ * @param center 输出：拟合直线的质心（基准点P0）
+ * @param direction 输出：拟合直线的方向向量v（单位化）
+ * @return 拟合是否成功（点集有效返回true）
+ */
+SG_APISHARED_EXPORT bool fitLine3DLeastSquares(
+    const std::vector<SVzNL3DPoint>& points, 
+    SVzNL3DPoint& center, 
+    SVzNL3DPoint& direction);
+
 //圆最小二乘拟合
 SG_APISHARED_EXPORT double fitCircleByLeastSquare(
     const std::vector<SVzNL3DPoint>& pointArray,
@@ -537,10 +549,15 @@ SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara_ROIs(
 //计算一个平面调平参数。
 //以数据输入中ROI以内的点进行平面拟合，计算调平参数
 //旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
-SSG_planeCalibPara sg_getPlaneCalibPara2_ROI(
+SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara2_ROI(
     std::vector< std::vector<SVzNL3DPosition>>& scanLines,
     SVzNL3DRangeD roi);
 
+//根据两个向量计算旋转矩阵
+SG_APISHARED_EXPORT SSG_planeCalibPara wd_conputeRTMatrix(
+    SVzNL3DPoint& vector1, 
+    SVzNL3DPoint& vector2);
+
 // 从旋转矩阵计算欧拉角（Z-Y-X顺序）
 SG_APISHARED_EXPORT SSG_EulerAngles rotationMatrixToEulerZYX(const double R[3][3]);
 // 从欧拉角计算旋转矩阵（Z-Y-X顺序）

sourceCode/SG_baseFunc.cpp

@@ -2417,6 +2417,44 @@ SSG_planeCalibPara sg_HCameraVScan_getGroundCalibPara(
 	return planePara;
 }
 
+SSG_planeCalibPara wd_conputeRTMatrix(SVzNL3DPoint& vector1, SVzNL3DPoint& vector2)
+{
+	Vector3 a = Vector3(vector1.x, vector1.y, vector1.z);
+	Vector3 b = Vector3(vector2.x, vector2.y, vector2.z);
+	Quaternion quanPara = rotationBetweenVectors(a, b);
+
+	RotationMatrix rMatrix;
+	quaternionToMatrix(quanPara, rMatrix.data);
+	//计算反向旋转矩阵
+	Quaternion invQuanPara = rotationBetweenVectors(b, a);
+	RotationMatrix invMatrix;
+	quaternionToMatrix(invQuanPara, invMatrix.data);
+
+	SSG_planeCalibPara calibPara;
+	calibPara.planeCalib[0] = rMatrix.data[0][0];
+	calibPara.planeCalib[1] = rMatrix.data[0][1];
+	calibPara.planeCalib[2] = rMatrix.data[0][2];
+	calibPara.planeCalib[3] = rMatrix.data[1][0];
+	calibPara.planeCalib[4] = rMatrix.data[1][1];
+	calibPara.planeCalib[5] = rMatrix.data[1][2];
+	calibPara.planeCalib[6] = rMatrix.data[2][0];
+	calibPara.planeCalib[7] = rMatrix.data[2][1];
+	calibPara.planeCalib[8] = rMatrix.data[2][2];
+
+	calibPara.invRMatrix[0] = invMatrix.data[0][0];
+	calibPara.invRMatrix[1] = invMatrix.data[0][1];
+	calibPara.invRMatrix[2] = invMatrix.data[0][2];
+	calibPara.invRMatrix[3] = invMatrix.data[1][0];
+	calibPara.invRMatrix[4] = invMatrix.data[1][1];
+	calibPara.invRMatrix[5] = invMatrix.data[1][2];
+	calibPara.invRMatrix[6] = invMatrix.data[2][0];
+	calibPara.invRMatrix[7] = invMatrix.data[2][1];
+	calibPara.invRMatrix[8] = invMatrix.data[2][2];
+
+	calibPara.planeHeight = 0;
+	return calibPara;
+}
+
 SSG_planeCalibPara sg_getPlaneCalibPara2_ROI(
 	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
 	SVzNL3DRangeD roi)
@@ -2796,7 +2834,7 @@ void lineDataRT(SVzNL3DLaserLine* a_line, const double* camPoseR, double groundH
 }
 void lineDataRT_vector(std::vector< SVzNL3DPosition>& a_line, const double* camPoseR, double groundH)
 {
-	for (int i = 0; i < a_line.size(); i++)
+	for (int i = 0; i < (int)a_line.size(); i++)
 	{
 		SVzNL3DPoint a_pt = a_line[i].pt3D;
 		if (a_pt.z < 1e-4)
@@ -3034,3 +3072,56 @@ void pointRT(const cv::Mat& R, const cv::Mat& T,
 	rtPt.z = result(2);
 	return;
 }
+
+
+/**
+ * @brief 空间直线最小二乘拟合
+ * @param points 输入的三维点集（至少2个点，否则无意义）
+ * @param center 输出：拟合直线的质心（基准点P0）
+ * @param direction 输出：拟合直线的方向向量v（单位化）
+ * @return 拟合是否成功（点集有效返回true）
+ */
+bool fitLine3DLeastSquares(const std::vector<SVzNL3DPoint>& points, SVzNL3DPoint& center, SVzNL3DPoint& direction) 
+{
+	// 检查点集有效性
+	if (points.size() < 2) {
+		std::cerr << "Error: 点集数量必须大于等于2！" << std::endl;
+		return false;
+	}
+
+	int n = points.size();
+	Eigen::MatrixXd A(n, 3); // 点集矩阵：每行一个点的(x,y,z)
+
+	// 1. 计算质心（center）
+	double cx = 0.0, cy = 0.0, cz = 0.0;
+	for (const auto& p : points) {
+		cx += p.x;
+		cy += p.y;
+		cz += p.z;
+		A.row(points.size() - n) << p.x, p.y, p.z; // 填充点集矩阵
+		n--;
+	}
+	cx /= points.size();
+	cy /= points.size();
+	cz /= points.size();
+	center = { cx, cy, cz };
+
+	// 2. 构造去中心化的协方差矩阵（3x3）
+	Eigen::MatrixXd centered = A.rowwise() - Eigen::Vector3d(cx, cy, cz);
+	Eigen::Matrix3d cov = centered.transpose() * centered / (points.size() - 1);
+
+	// 3. 特征值分解：求协方差矩阵的特征值和特征向量
+	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(cov);
+	if (eigensolver.info() != Eigen::Success) {
+		std::cerr << "Error: 特征值分解失败！" << std::endl;
+		return false;
+	}
+
+	// 最大特征值对应的特征向量即为方向向量（Eigen默认按特征值升序排列，取最后一个）
+	Eigen::Vector3d dir = eigensolver.eigenvectors().col(2);
+	// 单位化方向向量（可选，但工程中通常标准化）
+	dir.normalize();
+
+	direction = { dir(0), dir(1), dir(2) };
+	return true;
+}

sourceCode/SG_lineFeature.cpp

@@ -3443,7 +3443,7 @@ void wd_getRingArcFeature(
 	std::vector< SVzNL3DPosition>& lineData,
 	int lineIdx,
 	const SSG_cornerParam cornerPara,
-	double ringArcWidth, //定子的环宽度
+	double ringArcWidth, //»·¿í¶È
 	std::vector<SWD_segFeature>& line_ringArcs  //»·
 )
 {

sourceCode/rodAndBarDetection.cpp

@@ -12,6 +12,107 @@ const char* wd_rodAndBarDetectionVersion(void)
 	return m_strVersion.c_str();
 }
 
+SVzNL3DPoint getArcPeak(
+	std::vector< std::vector<SVzNL3DPosition>>& scanLines, 
+	SWD_segFeature & a_arcFeature)
+{
+	SVzNL3DPoint arcPeak = scanLines[a_arcFeature.lineIdx][a_arcFeature.startPtIdx].pt3D;
+	for (int i = a_arcFeature.startPtIdx+1; i <= a_arcFeature.endPtIdx; i++)
+	{
+		if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //跳开空点
+		{
+			if (arcPeak.z > scanLines[a_arcFeature.lineIdx][i].pt3D.z)
+				arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
+		}
+	}
+	return arcPeak;
+}
+
+//投影，提取ROI内的数据
+void xoyROIProjection(
+	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
+	const double* rtMatrix,
+	SSG_ROIRectD& roi_xoy,
+	std::vector<SVzNL3DPoint>& projectPoints
+	)
+{
+	int lineNum = (int)scanLines.size();
+	for (int line = 0; line < lineNum; line++)
+	{
+		std::vector<SVzNL3DPosition>& a_line = scanLines[line];
+		int ptNum = (int)a_line.size();
+		for (int i = 0; i < (int)a_line.size(); i++)
+		{
+			SVzNL3DPoint a_pt = a_line[i].pt3D;
+			if (a_pt.z < 1e-4)
+				continue;
+			double x = a_pt.x * rtMatrix[0] + a_pt.y * rtMatrix[1] + a_pt.z * rtMatrix[2];
+			double y = a_pt.x * rtMatrix[3] + a_pt.y * rtMatrix[4] + a_pt.z * rtMatrix[5];
+			double z = a_pt.x * rtMatrix[6] + a_pt.y * rtMatrix[7] + a_pt.z * rtMatrix[8];
+			if ((x >= roi_xoy.left) && (x <= roi_xoy.right) &&
+				(y >= roi_xoy.top) && (y <= roi_xoy.bottom))
+			{
+				a_pt.x = x;
+				a_pt.y = y;
+				a_pt.z = z;
+				projectPoints.push_back(a_pt);
+			}
+		}
+	}
+}
+
+SVzNLRangeD getZRange(std::vector<SVzNL3DPoint>& projectPoints)
+{
+	int ptNum = (int)projectPoints.size();
+	SVzNLRangeD zRange;
+	zRange.min = DBL_MAX;
+	zRange.max = DBL_MIN;
+	for (int i = 0; i < ptNum; i++)
+	{
+		zRange.min = zRange.min > projectPoints[i].z ? projectPoints[i].z : zRange.min;
+		zRange.max = zRange.max < projectPoints[i].z ? projectPoints[i].z : zRange.max;
+	}
+	return zRange;
+}
+
+void zCutPointClouds(
+	std::vector<SVzNL3DPoint>& projectPoints, 
+	SVzNLRangeD& zRange, 
+	std::vector<SVzNL3DPoint>& cutLayerPoints)
+{
+	int ptNum = (int)projectPoints.size();
+	for (int i = 0; i < ptNum; i++)
+	{
+		if ((projectPoints[i].z >= zRange.min) && (projectPoints[i].z <= zRange.max))
+			cutLayerPoints.push_back(projectPoints[i]);
+	}
+}
+
+SVzNL3DPoint getXoYCentroid(std::vector<SVzNL3DPoint>& points)
+{
+	int ptNum = (int)points.size();
+	SVzNL3DPoint centroid = { 0.0, 0.0, 0.0 };
+	if (ptNum == 0)
+		return centroid;
+	for (int i = 0; i < ptNum; i++)
+	{
+		centroid.x += points[i].x;
+		centroid.y += points[i].y;
+	}
+	centroid.x = centroid.x / ptNum;
+	centroid.y = centroid.y / ptNum;
+	return centroid;
+}
+
+SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
+{
+	SVzNL3DPoint _r_pt;
+	_r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2];
+	_r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5];
+	_r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8];
+	return _r_pt;
+}
+
  void sx_hexHeadScrewMeasure(
 	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
 	bool isHorizonScan,  //true:激光线平行槽道；false:激光线垂直槽道
@@ -128,160 +229,70 @@ const char* wd_rodAndBarDetectionVersion(void)
 	for (int i = 0; i < objNum; i++)
 	{
 		//空间直线拟合
-
+		std::vector<SVzNL3DPoint> fitPoints;
+		int nodeSize = (int)growTrees[i].treeNodes.size();
+		for (int j = 0; j < nodeSize; j++)
+		{
+			SVzNL3DPoint a_pt = getArcPeak(data_lines, growTrees[i].treeNodes[j]);
+			fitPoints.push_back(a_pt);
+		}
+		if (fitPoints.size() < 12)
+			continue;
+		//去除头尾各5个点，防止在端部和根部扫描时的数据有干扰
+		fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 5);
+		fitPoints.erase(fitPoints.end() - 5, fitPoints.end());
+		//拟合
+		SVzNL3DPoint P0_center, P1_dir;
+		bool result = fitLine3DLeastSquares(fitPoints, P0_center, P1_dir);
+		if (false == result)
+			continue;
 		//投影
-
+		//计算旋转向量
+		SVzNL3DPoint vector1 = P1_dir;
+		SVzNL3DPoint vector2 = { 0, 0, -1.0 };
+		SSG_planeCalibPara rotatePara = wd_conputeRTMatrix(	vector1, vector2);
 		//
+		SSG_ROIRectD roi_xoy;
+		roi_xoy.left = P0_center.x - rodRidius * 4; //2D范围
+		roi_xoy.right = P0_center.x + rodRidius * 4; //2D范围
+		roi_xoy.top = P0_center.y - rodRidius * 4; //2D范围
+		roi_xoy.bottom = P0_center.y + rodRidius * 4; //2D范围
+		std::vector< SVzNL3DPoint> roiProjectionData;
+		xoyROIProjection(scanLines, rotatePara.planeCalib, roi_xoy, roiProjectionData);
+		//取端面
+		SVzNLRangeD zRange = getZRange(roiProjectionData);
+		SVzNLRangeD cutZRange;
+		cutZRange.min = zRange.min;
+		cutZRange.max = zRange.min + 10.0;  //取10mm的端面
+		std::vector<SVzNL3DPoint> surfacePoints;
+		zCutPointClouds(roiProjectionData, 	cutZRange, surfacePoints);
+		//计算中心点
+		SVzNL3DPoint projectionCenter = getXoYCentroid(surfacePoints);
+		projectionCenter.z = zRange.min;
+		//旋转回原坐标系
+		SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix);
+		//生成Rod信息
+		SSX_hexHeadScrewInfo a_rod;
+		a_rod.center = surfaceCenter;
+		a_rod.axialDir = P1_dir;
+		a_rod.rotateAngle = 0;
+		screwInfo.push_back(a_rod);
 	}
-
+	if (true == isHorizonScan)
-
-	std::vector<SSG_basicFeatureGap>& nodes_1 = growTrees[0].treeNodes;
-	std::vector<SSG_basicFeatureGap>& nodes_2 = growTrees[1].treeNodes;
-#if _OUTPUT_DEBUG_DATA
-	for (int i = 0, i_max = (int)nodes_1.size(); i < i_max; i++)
 	{
-		int lineIdx, ptIdx;
+		int objNum = (int)screwInfo.size();
-		if (false == isHorizonScan)
+		for (int i = 0; i < objNum; i++)
 		{
-			lineIdx = nodes_1[i].lineIdx;
+			double tmp = screwInfo[i].center.x;
-			ptIdx = nodes_1[i].gapPt_0.nPointIdx;
+			screwInfo[i].center.x = screwInfo[i].center.y;
-			scanLines[lineIdx][ptIdx].nPointIdx = 1;
+			screwInfo[i].center.y = tmp;
-			ptIdx = nodes_1[i].gapPt_1.nPointIdx;
+			tmp = screwInfo[i].axialDir.x;
-			scanLines[lineIdx][ptIdx].nPointIdx = 2;
+			screwInfo[i].axialDir.x = screwInfo[i].axialDir.y;
-		}
+			screwInfo[i].axialDir.y = tmp;
-		else
+			//screwInfo[i].rotateAngle += 90;
-		{
-			ptIdx = nodes_1[i].lineIdx;
-			lineIdx = nodes_1[i].gapPt_0.nPointIdx;
-			scanLines[lineIdx][ptIdx].nPointIdx = 1;
-			lineIdx = nodes_1[i].gapPt_1.nPointIdx;
-			scanLines[lineIdx][ptIdx].nPointIdx = 2;
 		}
 	}
-	for (int i = 0, i_max = (int)nodes_2.size(); i < i_max; i++)
+	return;
-	{
-		int lineIdx, ptIdx;
-		if (false == isHorizonScan)
-		{
-			lineIdx = nodes_2[i].lineIdx;
-			ptIdx = nodes_2[i].gapPt_0.nPointIdx;
-			scanLines[lineIdx][ptIdx].nPointIdx = 1;
-			ptIdx = nodes_2[i].gapPt_1.nPointIdx;
-			scanLines[lineIdx][ptIdx].nPointIdx = 2;
-		}
-		else
-		{
-			ptIdx = nodes_2[i].lineIdx;
-			lineIdx = nodes_2[i].gapPt_0.nPointIdx;
-			scanLines[lineIdx][ptIdx].nPointIdx = 1;
-			lineIdx = nodes_2[i].gapPt_1.nPointIdx;
-			scanLines[lineIdx][ptIdx].nPointIdx = 2;
-		}
-	}
-#endif
-
-	//按扫描线配对
-	std::vector<SSG_basicFeatureGap> line_1;  //line_1和line_2为按扫描线对应的两个槽道上的Gap特征
-	std::vector<SSG_basicFeatureGap> line_2;
-	int i_idx = 0;
-	int j_idx = 0;
-	while (1)
-	{
-		int line_idx1 = nodes_1[i_idx].lineIdx;
-		int line_idx2 = nodes_2[j_idx].lineIdx;
-		if (line_idx1 == line_idx2)
-		{
-			line_1.push_back(nodes_1[i_idx]);
-			line_2.push_back(nodes_2[j_idx]);
-			i_idx++;
-			j_idx++;
-		}
-		else
-		{
-			if (line_idx1 < line_idx2)
-				i_idx++;
-			else
-				j_idx++;
-		}
-		if ((i_idx >= (int)nodes_1.size()) || (j_idx >= (int)nodes_2.size()))
-			break;
-	}
-
-#if _OUTPUT_DEBUG_DATA
-	for (int i = 0, i_max = (int)line_1.size(); i < i_max; i++)
-	{
-		int lineIdx, ptIdx;
-		if (false == isHorizonScan)
-		{
-			lineIdx = line_1[i].lineIdx;
-			ptIdx = line_1[i].gapPt_0.nPointIdx;
-			scanLines[lineIdx][ptIdx].nPointIdx = 3;
-			ptIdx = line_1[i].gapPt_1.nPointIdx;
-			scanLines[lineIdx][ptIdx].nPointIdx = 4;
-		}
-		else
-		{
-			ptIdx = line_1[i].lineIdx;
-			lineIdx = line_1[i].gapPt_0.nPointIdx;
-			scanLines[lineIdx][ptIdx].nPointIdx = 3;
-			lineIdx = line_1[i].gapPt_1.nPointIdx;
-			scanLines[lineIdx][ptIdx].nPointIdx = 4;
-		}
-	}
-	for (int i = 0, i_max = (int)line_2.size(); i < i_max; i++)
-	{
-		int lineIdx, ptIdx;
-		if (false == isHorizonScan)
-		{
-			lineIdx = line_2[i].lineIdx;
-			ptIdx = line_2[i].gapPt_0.nPointIdx;
-			scanLines[lineIdx][ptIdx].nPointIdx = 3;
-			ptIdx = line_2[i].gapPt_1.nPointIdx;
-			scanLines[lineIdx][ptIdx].nPointIdx = 4;
-		}
-		else
-		{
-			ptIdx = line_2[i].lineIdx;
-			lineIdx = line_2[i].gapPt_0.nPointIdx;
-			scanLines[lineIdx][ptIdx].nPointIdx = 3;
-			lineIdx = line_2[i].gapPt_1.nPointIdx;
-			scanLines[lineIdx][ptIdx].nPointIdx = 4;
-		}
-	}
-#endif
-
-	//旋转，保证扫描线与Gap垂直
-	double angleSearchWin = 30;
-	double angleStepping = 0.1;
-
-	int loop = (int)(angleSearchWin / angleStepping);
-	double bestSpace = 0;
-	double rotateAngle = 0;
-	for (int i = -loop; i <= loop; i++)
-	{
-		double angle = i * angleStepping;
-		std::vector<SSG_basicFeatureGap> rotate_line_1;
-		_XY_rotateLine(angle, line_1, rotate_line_1);
-		std::vector<SSG_basicFeatureGap> rotate_line_2;
-		_XY_rotateLine(angle, line_2, rotate_line_2);
-		double meanSpace = _computeChannelSpace(rotate_line_1, rotate_line_2);
-		if (bestSpace < meanSpace)
-		{
-			bestSpace = meanSpace;
-			rotateAngle = angle;
-		}
-	}
-	std::vector<SSG_basicFeatureGap> calib_line_1;
-	_XY_rotateLine(rotateAngle, line_1, calib_line_1);
-	std::vector<SSG_basicFeatureGap> calib_line_2;
-	_XY_rotateLine(rotateAngle, line_2, calib_line_2);
-
-	channelInfo.channelSpace = _computeChannelSpace(calib_line_1, calib_line_2);
-	channelInfo.channelWidth[0] = _computeGapMeanWidth(calib_line_1);
-	channelInfo.channelWidth[1] = _computeGapMeanWidth(calib_line_2);
-	channelInfo.channelDepth[0] = _computeGapMeanDepth(calib_line_1, data_lines);
-	channelInfo.channelDepth[1] = _computeGapMeanDepth(calib_line_2, data_lines);
-	return channelInfo;
 }
 
 

sourceCode/rodAndBarDetection_Export.h

@@ -8,7 +8,7 @@
 typedef struct
 {
 	SVzNL3DPoint center; //螺杆端部中心点
-	SVzNL3DPoint axialPoint; //轴向点，与center构成轴向向量
+	SVzNL3DPoint axialDir; //ÖáÏòÏòÁ¿
 	double rotateAngle; //-30 - 30度
 }SSX_hexHeadScrewInfo;  //六角头螺杆
 
@@ -16,10 +16,12 @@ typedef struct
 SG_APISHARED_EXPORT const char* wd_rodAndBarDetectionVersion(void);
 
 //六角头螺杆端部中心点和轴向测量
-SG_APISHARED_EXPORT SSX_hexHeadScrewInfo sx_hexHeadScrewMeasure(
+SG_APISHARED_EXPORT  void sx_hexHeadScrewMeasure(
 	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
 	bool isHorizonScan,  //true:激光线平行槽道；false:激光线垂直槽道
 	const SSG_cornerParam cornerPara,
 	const SSG_outlierFilterParam filterParam,
 	const SSG_treeGrowParam growParam,
+	double rodRidius,
+	std::vector<SSX_hexHeadScrewInfo>& screwInfo,
 	int* errCode);