rodAndBarDetection version 1.2.9 :

修正螺杆定位算法中轴向拟合计算时方向确定的问题，保证投影后的点云与投影前的点云的Z变化趋势一致

sourceCode/rodAndBarDetection.cpp

@@ -17,7 +17,8 @@
 //version 1.2.6 : 定位盘位姿计算由RANSAC调整为迭代拟合，测试精度是否有提高
 //version 1.2.7 : (1)根据实际点云修正了螺杆定位算法（2）定位盘位姿计算改回RANSAC
 //version 1.2.8 : 修正螺杆定位算法中的一个Bug
-std::string	m_strVersion = "1.2.8";
+//version 1.2.9 : 修正螺杆定位算法中轴向拟合计算时方向确定的问题，保证投影后的点云与投影前的点云的Z变化趋势一致
+std::string	m_strVersion = "1.2.9";
 const char* wd_rodAndBarDetectionVersion(void)
 {
 	return m_strVersion.c_str();
@@ -172,7 +173,7 @@ SVzNLRangeD getZRange(std::vector<SVzNL3DPoint>& projectPoints)
 	int ptNum = (int)projectPoints.size();
 	SVzNLRangeD zRange;
 	zRange.min = DBL_MAX;
-	zRange.max = DBL_MIN;
+	zRange.max = -DBL_MAX;
 	for (int i = 0; i < ptNum; i++)
 	{
 		zRange.min = zRange.min > projectPoints[i].z ? projectPoints[i].z : zRange.min;
@@ -662,6 +663,36 @@ SVzNL3DRangeD _getPointCloudROI(std::vector<SVzNL3DPoint>& scanData)
 	SVzNL3DPoint vector1 = P1_dir;
 	SVzNL3DPoint vector2 = { 0, 0, 1.0 };
 	SSG_planeCalibPara rotatePara = wd_computeRTMatrix(	vector1, vector2);
+#if 1
+	std::vector< SVzNL3DPoint> verifyData;
+	for (int m = 0; m < (int)fitPoints.size(); m++)
+	{
+		SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib);
+		verifyData.push_back(rPt);
+	}
+#endif
+	bool dirInverting = false;
+	if (((fitPoints[0].z < fitPoints.back().z) && (verifyData[0].z > verifyData.back().z)) ||
+		((fitPoints[0].z > fitPoints.back().z) && (verifyData[0].z < verifyData.back().z)))
+		dirInverting = true;
+	if (true == dirInverting)
+	{
+		P1_dir = { -P1_dir.x, -P1_dir.y, -P1_dir.z };
+		vector1 = P1_dir;
+		rotatePara = wd_computeRTMatrix(vector1, vector2);
+	}
+	verifyData.clear();
+	for (int m = 0; m < (int)fitPoints.size(); m++)
+	{
+		SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib);
+		verifyData.push_back(rPt);
+	}
+	if (((fitPoints[0].z < fitPoints.back().z) && (verifyData[0].z > verifyData.back().z)) ||
+		((fitPoints[0].z > fitPoints.back().z) && (verifyData[0].z < verifyData.back().z)))
+		dirInverting = true;
+	else
+		dirInverting = false;
+
 	//
 	SVzNL3DPoint test_center = { P0_center.x, P0_center.y + rodDiameter / 2,  P0_center.z - 100 };
 	SVzNL3DPoint P0_rotate = _ptRotate(P0_center, rotatePara.planeCalib);
@@ -672,22 +703,22 @@ SVzNL3DRangeD _getPointCloudROI(std::vector<SVzNL3DPoint>& scanData)
 	roi_xoy.top = P0_rotate.y - rodDiameter * 2.0; //2D范围
 	roi_xoy.bottom = P0_rotate.y + rodDiameter * 2.0; //2D范围
 
-#if 1
-	std::vector< SVzNL3DPoint> verifyData;
-	for (int m = 0; m < (int)fitPoints.size(); m++)
-	{
-		SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib);
-		verifyData.push_back(rPt);
-	}
-#endif
 	std::vector< SVzNL3DPoint> roiProjectionData;
 	xoyROIProjection(scanLines, rotatePara.planeCalib, roi_xoy, resultObjClusterId, clusterIdMask, roiProjectionData);
 
 	//取端面
 	SVzNLRangeD zRange = getZRange(roiProjectionData);
 	SVzNLRangeD cutZRange;
-	cutZRange.min = zRange.min;
+	if (false == dirInverting)
-	cutZRange.max = zRange.min + 5.0;  //5mmµÄ¶ËÃæ
+	{
+		cutZRange.min = zRange.min;
+		cutZRange.max = zRange.min + 5.0;  //5mm的端面
+	}
+	else
+	{
+		cutZRange.max = zRange.max;
+		cutZRange.min = zRange.max - 5.0;  //5mm的端面
+	}
 	std::vector<SVzNL3DPoint> surfacePoints;
 	zCutPointClouds(roiProjectionData, 	cutZRange, surfacePoints);
 	//计算中心点
@@ -706,8 +737,13 @@ SVzNL3DRangeD _getPointCloudROI(std::vector<SVzNL3DPoint>& scanData)
 		{
 			if (centerIdx < 0)
 				centerIdx = i;
-			else if(surfacePoints[centerIdx].z < surfacePoints[i].z)
+			else
-				centerIdx = i;
+			{
+			
+				if (((false == dirInverting) && (surfacePoints[centerIdx].z < surfacePoints[i].z)) || 
+					((true == dirInverting) && (surfacePoints[centerIdx].z > surfacePoints[i].z)))
+					centerIdx = i;
+			}
 		}
 	}
 	if(centerIdx < 0)