hybridPosePositioning

version 1.4.3 : 转子钢芯， (1)分层系数和边距系数添加-1.0参数值，表示不进行分层或边距。（2）添加选项选择使用工件的自身法向或统一使用调平地面法向

hybridPosePositioning_test/hybridPosePositioning_test.cpp

@@ -2350,7 +2350,7 @@ void HaiRuiMa_groundCalib_test(void)
 }
 
 //拓普发工件孔定位（工件定位)
-#define HRM_RotorCore_TEST_GROUP 5
+#define HRM_RotorCore_TEST_GROUP 6
 void HaiRuiMa_rotorCorePositioning_test(void)
 {
 	const char* dataPath[HRM_RotorCore_TEST_GROUP] = {
@@ -2360,16 +2360,17 @@ void HaiRuiMa_rotorCorePositioning_test(void)
 		"F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/现场数据/",  //2
 		"F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/2026-09-海瑞马/09-14/",  //3
 		"F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/2026-09-海瑞马/09-14中/", //4
+		"F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/2026-09-海瑞马/异常数据/",  //5
 	};
 
 	SVzNLRange fileIdx[HRM_RotorCore_TEST_GROUP] = {
-		{1,10}, {3,70},{3,7}, {1,47}, {1,54}
+		{1,10}, {3,70},{8,8}, {5,47}, {1,54}, {1,6}
 	};
 
 	const char* ver = wd_hybridPositioningVersion();
 	printf("ver:%s\n", ver);
 
-	for (int grp = 2; grp <= 2; grp++)
+	for (int grp = 4; grp <= 4; grp++)
 	{
 		SSG_planeCalibPara groundCalibPara;
 		//初始化成单位阵
@@ -2442,10 +2443,10 @@ void HaiRuiMa_rotorCorePositioning_test(void)
 			vzReadObj2DROI(_scan_file, objROIs);
 
 			SWD_Cylinder workpieceParam;  //圆柱形工件的标称半径和高度
-			if (grp == 3)
+			if ( (grp == 3) || (grp == 5))
 			{
-				workpieceParam.radius = 60.0;
+				workpieceParam.radius = 62.0;
-				workpieceParam.height = 70.0;
+				workpieceParam.height = 50.0;
 			}
 			else if (grp == 4)
 			{
@@ -2462,9 +2463,11 @@ void HaiRuiMa_rotorCorePositioning_test(void)
 			spacerParam.dWidth = 720.0;
 			spacerParam.dHeight = 520.0;
 
-			double layerRatio = 0.5;  //分层系数，分层门限=分层系数*工件高度；当两个工件的高度差大于分层门限时，判断为在不同层
+			int useGroundCalibNormal = 0;  //1: 使用调平后的地面法向作为工件法向;  0:使用工件自身的法向
+
+			double layerRatio = -1.0; //不分层  //分层系数，分层门限=分层系数*工件高度；当两个工件的高度差大于分层门限时，判断为在不同层
 			const double distToSideRatio = 1.1;  //边距系数，边距门限=边距系数*工件直径；当工件距边的距离大于边距门限时，判断为边上的工件
-			double groundRemoveOffset = 200;  //料筐地面去除的Z偏置，实际地面去除高度为 groundCalibPara.planeHeight - groundRemoveOffset
+			double groundRemoveOffset =20;  //料筐地面去除的Z偏置，实际地面去除高度为 groundCalibPara.planeHeight - groundRemoveOffset
 			long t1 = (long)GetTickCount64();//统计时间
 
 			int errCode = 0;
@@ -2479,6 +2482,7 @@ void HaiRuiMa_rotorCorePositioning_test(void)
 				layerRatio,
 				distToSideRatio,
 				groundRemoveOffset,
+				useGroundCalibNormal,
 				workpiecePositions,
 				binInfo,
 #ifdef _OUTPUT_DEBUG_DATA

sourceCode/hybridPosePositioning.cpp

@@ -25,7 +25,8 @@
 //version 1.4.0 : 转子钢芯， 添加了隔板的识别和定位。 当定位隔板时，工件类型为9
 //version 1.4.1 : 转子钢芯， 每个输出结果添加了2D图像坐标
 //version 1.4.2 : 转子钢芯， 优化了点云中垂直部分去除的门限
-std::string	m_strVersion = "HybridPositioning 1.4.2";
+//version 1.4.3 : 转子钢芯， (1)分层系数和边距系数添加-1.0参数值，表示不进行分层或边距。（2）添加选项选择使用工件的自身法向或统一使用调平地面法向
+std::string	m_strVersion = "HybridPositioning 1.4.3";
 const char* wd_hybridPositioningVersion(void)
 {
 	return m_strVersion.c_str();
@@ -358,6 +359,7 @@ WD_workpieceInfo _computeWorkpiecePose(
 	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
 	const SWDScanPosition centerPosition,
 	const SWD_Cylinder workpieceParam,
+	const int useGroundCalibNormal,  //1: 使用调平后的地面法向作为工件法向;  0:使用工件自身的法向
 	std::vector<SWDScanPosition>& outsideEdgePtIndice,
 	std::vector<SWDScanPosition>& centerPtIndice)
 {
@@ -867,6 +869,7 @@ WD_workpieceInfo _computeWorkpiecePose(
 
 	//迭代一次
 	pointArray.clear();
+	std::vector<cv::Point3d> normalDataArray;
 	for (int i = 0; i < (int)objClusters.size(); i++)
 	{
 		std::vector< SWD_indexingSeg>& a_cluster = objClusters[i];
@@ -878,6 +881,10 @@ WD_workpieceInfo _computeWorkpiecePose(
 			if (r1 < workpieceParam.radius * 1.05)
 			{
 				pointArray.push_back(a_cluster[m].outsideEdgePoint.pt3D);
+				cv::Point3d a_pt = cv::Point3d(a_cluster[m].outsideEdgePoint.pt3D.x, a_cluster[m].outsideEdgePoint.pt3D.y, a_cluster[m].outsideEdgePoint.pt3D.z);
+				normalDataArray.push_back(a_pt);
+				a_pt = cv::Point3d(a_cluster[m].centerPoint.pt3D.x, a_cluster[m].centerPoint.pt3D.y, a_cluster[m].centerPoint.pt3D.z);
+				normalDataArray.push_back(a_pt);
 				//生成标注需要的信息
 				if (a_seg.hScanFlag == 0)
 				{
@@ -897,6 +904,21 @@ WD_workpieceInfo _computeWorkpiecePose(
 		}
 	}
 
+	SVzNL3DPoint poseNormal = { 0.0, 0.0, 1.0 };
+	if (0 == useGroundCalibNormal)  //1: 使用调平后的地面法向作为工件法向;  0:使用工件自身的法向
+	{
+		std::vector<double> res;
+		//计算面参数: z = Ax + By + C
+		//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
+		vzCaculateLaserPlane(normalDataArray, res);
+		SVzNL3DPoint normVector = { res[0], res[1], res[2] };
+		normVector = vec3_normalize(normVector);
+		if (normVector.z < 0)
+			poseNormal = { -normVector.x, -normVector.y, -normVector.z };
+		else
+			poseNormal = normVector;
+	}
+
 	err = fitCircleByLeastSquare(pointArray, center, radius);
 	double k = radius / workpieceParam.radius;
 	double distToRefCenter = sqrt(pow(center.x - centerPoint.x, 2) + pow(center.y - centerPoint.y, 2));
@@ -904,9 +926,9 @@ WD_workpieceInfo _computeWorkpiecePose(
 	{
 		a_pose.center = { center.x, center.y, objZ };
 		a_pose.value = radius;
-		a_pose.z_dir = { 0.0, 0.0, 1.0 };
+		a_pose.z_dir = poseNormal;
-		a_pose.y_dir = { 0.0, 0.0, 0.0 };
+		a_pose.x_dir = { 1.0, 0.0, 0.0 };
-		a_pose.x_dir = { 0.0, 0.0, 0.0 };
+		a_pose.y_dir = vec3_cross(a_pose.z_dir, a_pose.x_dir);  //叉乘出y_dir
 	}
 	return a_pose;
 }
@@ -1465,9 +1487,10 @@ void wd_HRM_RotorCorePositioning(
 	const SSG_planeCalibPara groundCalibPara,
 	const SVzNLSizeD spacerParam, //隔板参数：长和宽
 	const SWD_Cylinder workpieceParam,  //圆柱形工件的标称半径和高度
-	const double layerRatio, //分层系数，分层门限=分层系数*工件高度；当两个工件的高度差大于分层门限时，判断为在不同层
+	const double layerRatio, //分层系数，-1.0:不分层。 分层门限=分层系数*工件高度；当两个工件的高度差大于分层门限时，判断为在不同层
-	const double distToSideRatio, //边距系数，边距门限=边距系数*工件直径；当工件距边的距离大于边距门限时，判断为边上的工件
+	const double distToSideRatio, //边距系数，-1。0：不使用边距系数。边距门限=边距系数*工件直径；当工件距边的距离大于边距门限时，判断为边上的工件
 	const double groundRemoveOffset, //料筐地面去除的Z偏置，实际地面去除高度为 groundCalibPara.planeHeight - groundRemoveOffset
+	const int useGroundCalibNormal,  //1: 使用调平后的地面法向作为工件法向;  0:使用工件自身的法向
 	std::vector< WD_workpieceInfo>& workpiecePositions,
 	WD_HRM_BinInfo& binInfo,  //输出Bin信息，用于debug，确认料筐信息检测正确
 #ifdef _OUTPUT_DEBUG_DATA
@@ -1813,6 +1836,7 @@ void wd_HRM_RotorCorePositioning(
 			roiScanLines,
 			centerPosition,
 			workpieceParam,
+			useGroundCalibNormal, //1: 使用调平后的地面法向作为工件法向;  0:使用工件自身的法向
 			outsideEdgePtIndice,
 			centerPtIndice);
 		a_workpiece.imgCenter = { nCx, nCy };
@@ -1892,19 +1916,26 @@ void wd_HRM_RotorCorePositioning(
 			//计算料筐信息
 			SVzNLRangeD zSliceRange;
 			zSliceRange.min = highest_z - workpieceParam.height;
-			zSliceRange.max = groundCalibPara.planeHeight - groundRemoveOffset;
+			zSliceRange.max = highest_z + workpieceParam.height * 2;
+			if(zSliceRange.max > (groundCalibPara.planeHeight - groundRemoveOffset))
+				zSliceRange.max = groundCalibPara.planeHeight - groundRemoveOffset;
 			binInfo = wd_HRM_getBinSize_2(dataCopy, zSliceRange);
 
 			//z方向排序 
 			std::vector< WD_workpieceInfo> zSortWorkpiece;
-			double topLayerTh = highest_z + workpieceParam.height * layerRatio;
+			if (layerRatio >= 1e-4)
-			for (int i = 0; i < (int)allWorkpiece.size(); i++)
 			{
-				if (allWorkpiece[i].center.z < topLayerTh)
+				double topLayerTh = highest_z + workpieceParam.height * layerRatio;
-					zSortWorkpiece.push_back(allWorkpiece[i]);
+				for (int i = 0; i < (int)allWorkpiece.size(); i++)
+				{
+					if (allWorkpiece[i].center.z < topLayerTh)
+						zSortWorkpiece.push_back(allWorkpiece[i]);
+				}
 			}
+			else
+				zSortWorkpiece.insert(zSortWorkpiece.end(), allWorkpiece.begin(), allWorkpiece.end());
 
-			if (binInfo.binTopZ > 1e-4)
+			if ( (binInfo.binTopZ > 1e-4) && (distToSideRatio > 1e-4))
 			{
 				//计算工件位置属性
 				std::vector<SWD_3DPointPair> linePoint;

sourceCode/hybridPosePositioning_Export.h

@@ -68,6 +68,7 @@ SG_APISHARED_EXPORT void wd_HRM_RotorCorePositioning(
 	const double layerRatio, //分层系数，分层门限=分层系数*工件高度；当两个工件的高度差大于分层门限时，判断为在不同层
 	const double distToSideRatio, //边距系数，边距门限=边距系数*工件直径；当工件距边的距离大于边距门限时，判断为边上的工件
 	const double groundRemoveOffset, //料筐地面去除的Z偏置，实际地面去除高度为 groundCalibPara.planeHeight - groundRemoveOffset
+	const int useGroundCalibNormal,  //1: 使用调平后的地面法向作为工件法向;  0:使用工件自身的法向
 	std::vector< WD_workpieceInfo>& workpiecePositions,
 	WD_HRM_BinInfo& binInfo,  //输出Bin信息，用于debug，确认料筐信息检测正确
 #ifdef _OUTPUT_DEBUG_DATA