hybridPosePositioning

version 1.3.0 : 添加了海瑞马转子钢芯定位

hybridPosePositioning_test/hybridPosePositioning_test.cpp

@@ -673,7 +673,8 @@ void _outputRGBDResult_XYZ_RGBD_binInfo(
 void _outputRGBDResult_XYZ_RGBD_workpieceInfo(
 	char* fileName,
 	std::vector<std::vector<SVzNL3DPosition>>& scanLines,
-	std::vector<WD_workpieceInfo>& poseInfo)
+	std::vector<WD_workpieceInfo>& poseInfo, 
+	double dirLen)
 {
 	int objNumber = 0;
 	if (poseInfo.size()>0)
@@ -769,7 +770,7 @@ void _outputRGBDResult_XYZ_RGBD_workpieceInfo(
 		//输出方向线条
 		rgb = { 250, 0, 0 };
 		size = 2;
-		double dirLen = 500;
+		//double dirLen = 500;
 		for (int m = 0; m < (int)poseInfo.size(); m++)
 		{
 			SVzNL3DPoint dirPt1 = { poseInfo[m].center.x + poseInfo[m].z_dir.x * dirLen,
@@ -2111,7 +2112,7 @@ void HaiRuiMa_rotorCorePositioning_test(void)
 
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{
-			//fidx = 0;
+			//fidx = 3;
 			char _scan_file[256];
 			sprintf_s(_scan_file, "%s%d_cloud.txt", dataPath[grp], fidx);
 			std::vector<std::vector< SVzNLPositionD>> scanLines;
@@ -2152,11 +2153,24 @@ void HaiRuiMa_rotorCorePositioning_test(void)
 			else
 				printf("%s: %d(ms), errCode=%d ...", _scan_file, (int)(t2 - t1), errCode);
 			//输出测试结果
-			sprintf_s(_scan_file, "%sresult/debug_%d_data.txt", dataPath[grp], fidx);
-			_outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, debugScanLines, workpiecePositions);
 			sprintf_s(calibFile, "%sresult/LaserLine%d_workpiece_info.txt", dataPath[grp], fidx);
 			_outputWorkpieceInfo(calibFile, workpiecePositions);
-
+			//旋转到调平坐标系
+			int workpieceNum = (int)workpiecePositions.size();
+			for (int i = 0; i < workpieceNum; i++)
+			{
+				SVzNL3DPoint rpt;
+				rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.planeCalib);
+				workpiecePositions[i].center = rpt;
+				rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.planeCalib);
+				workpiecePositions[i].y_dir = rpt;
+				rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.planeCalib);
+				workpiecePositions[i].z_dir = rpt;
+				rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.planeCalib);
+				workpiecePositions[i].x_dir = rpt;
+			}
+			sprintf_s(_scan_file, "%sresult/result_%d.txt", dataPath[grp], fidx);
+			_outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, debugScanLines, workpiecePositions, 50);
 #if 0
 			sprintf_s(calibFile, "%sresult/LaserLine%d_holes_projection.png", dataPath[grp], fidx);
 			double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 };  //
@@ -2364,7 +2378,7 @@ void HaiRuiMa_workpieceSizeCompute_test(void)
 			a_pose.value = workpieceInfo.workpieceRadius;
 			a_pose.z_dir = { 0, 0, 1.0 };
 			poseInfo.push_back(a_pose);
-			_outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, scanLines, poseInfo);
+			_outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, scanLines, poseInfo, 500);
 			sprintf_s(_scan_file, "%sresult/%d_workpiece_info.txt", dataPath[grp], fidx);
 			_outputWorkpieceInfo(_scan_file, workpieceInfo);
 
@@ -2501,8 +2515,8 @@ typedef enum
 int main()
 {
 	//ESG_testMode testMode = keSG_2D3D定位_海瑞马_地面调平;
-	ESG_testMode testMode = keSG_2D3D定位_海瑞马_锥形工件;
+	//ESG_testMode testMode = keSG_2D3D定位_海瑞马_锥形工件;
-	//ESG_testMode testMode = keSG_2D3D定位_海瑞马_转子芯;
+	ESG_testMode testMode = keSG_2D3D定位_海瑞马_转子芯;
 	//ESG_testMode testMode = keSG_2D3D定位_海瑞马_码垛料筐定位;
 	//ESG_testMode testMode = keSG_2D3D定位_海瑞马_码垛工件尺寸测量;
 	//ESG_testMode testMode = keSG_2D3D定位_海瑞马_码垛位置规划;

sourceCode/hybridPosePositioning.cpp

@@ -10,7 +10,8 @@
 //version 1.0.0 : base version release to customer
 //version 1.1.0 : 海瑞马码垛规划版本
 //version 1.2.0 : 添加了海瑞马锥形工件定位版本（不包含二次定位）
-std::string	m_strVersion = "HybridPositioning 1.1.0";
+//version 1.3.0 : 添加了海瑞马转子钢芯定位
+std::string	m_strVersion = "HybridPositioning 1.3.0";
 const char* wd_hybridPositioningVersion(void)
 {
 	return m_strVersion.c_str();
@@ -348,15 +349,21 @@ WD_workpieceInfo _computeWorkpiecePose(
 	for (int i = 0; i < lineNum; i++)
 		flags[i].resize(linePtNum);
 
+	//内部参数
+	double topSeg_minWidth = 5.0; //
+	double topSeg_maxWidth = 20.0;
+
 	SSG_lineSegParam filterSegPara;
 	filterSegPara.distScale = 0;
-	filterSegPara.segGapTh_y = DBL_MAX;
+	filterSegPara.segGapTh_y = topSeg_maxWidth;
 	filterSegPara.segGapTh_z = workpieceParam.height / 2;
 
 	//垂直
 	std::vector<std::vector<SWD_indexingSeg>> vScanLineSegs;
 	for (int line = 0; line < lineNum; line++)
 	{
+		if (line == 83)
+			int kkk = 1;
 		//分段
 		std::vector<SSG_RUN> simpleSegs;
 		wd_getLineDataIntervals(scanLines[line], filterSegPara, simpleSegs);
@@ -430,6 +437,8 @@ WD_workpieceInfo _computeWorkpiecePose(
 				exSegs.push_back(a_seg);
 			}
 		}
+		if (exSegs.size() == 0)
+			return a_pose; 
 		//去除低的段：如果两个段的meanZ相差达到工件高度的一半，视为低的段
 		int highestSeg = 0;
 		for (int m = 1; m < (int)exSegs.size(); m++)
@@ -562,10 +571,11 @@ WD_workpieceInfo _computeWorkpiecePose(
 	SVzNL3DPoint centerPoint = scanLines[centerPosition.lineIdx][centerPosition.ptIdx].pt3D;
 	std::vector< SWD_indexingSeg> segsTop;
 	//生成工件顶面上的点: 宽度大于一个门限（滤噪）；边界点不能是边界
-	double minWidth = 5.0; //
 	//垂直扫描处理
 	for (int line = 0; line < lineNum; line++)
 	{
+		if (line == 83)
+			int kkk = 1;
 		int mostLeftSegId = -1;  //对于垂直扫描，实际上是最上面
 		int mostRightSegId = -1;//对于垂直扫描，实际上是最下面
 		for (int i = 0; i < (int)vScanLineSegs[line].size(); i++)
@@ -577,11 +587,13 @@ WD_workpieceInfo _computeWorkpiecePose(
 			double width = abs(scanLines[line][idx1].pt3D.y - scanLines[line][idx2].pt3D.y);
 			SVzNL3DPoint segCenter = exSeg.centerPoint.pt3D;
 			double distToCircleCenter = sqrt(pow(centerPoint.x - segCenter.x, 2) + pow(centerPoint.y - segCenter.y, 2));
-			if ( (width > minWidth) && (distToCircleCenter < workpieceParam.radius))
+			if ( (width > topSeg_minWidth) && (width < topSeg_maxWidth) && (distToCircleCenter < workpieceParam.radius))
 			{
-				if (mostLeftSegId < 0)
+				if ((mostLeftSegId < 0) && (idx2 < centerPosition.ptIdx) &&(idx1 > 0 ))
 					mostLeftSegId = i;
-				mostRightSegId = i;
+
+				if( (idx1 > centerPosition.ptIdx) && (idx2 < linePtNum - 1))
+					mostRightSegId = i;
 			}
 		}
 		if (mostLeftSegId >= 0)
@@ -627,11 +639,13 @@ WD_workpieceInfo _computeWorkpiecePose(
 			double width = abs(hLines[line][idx1].pt3D.y - hLines[line][idx2].pt3D.y);
 			SVzNL3DPoint segCenter = a_seg.centerPoint.pt3D;
 			double distToCircleCenter = sqrt(pow(centerPoint.x - segCenter.x, 2) + pow(centerPoint.y - segCenter.y, 2));
-			if ( (width > minWidth) && (distToCircleCenter < workpieceParam.radius))
+			if ( (width > topSeg_minWidth) && (width < topSeg_maxWidth) && (distToCircleCenter < workpieceParam.radius))
 			{
-				if (mostLeftSegId < 0)
+				if ((mostLeftSegId < 0) && (idx2 < centerPosition.lineIdx) && (idx1 > 0))
 					mostLeftSegId = i;
-				mostRightSegId = i;
+
+				if( (idx1 > centerPosition.lineIdx) && (idx2 < lineNum - 1))
+					mostRightSegId = i;
 			}
 		}
 		if (mostLeftSegId >= 0)
@@ -664,25 +678,6 @@ WD_workpieceInfo _computeWorkpiecePose(
 
 		}
 	}
-	//生成标注需要的信息
-	for (int i = 0; i < (int)segsTop.size(); i++)
-	{
-		SWD_indexingSeg& a_seg = segsTop[i];
-		if (a_seg.hScanFlag == 0)
-		{
-			SWDScanPosition pos_center = { a_seg.lineIdx, a_seg.centerPoint.nPointIdx };
-			centerPtIndice.push_back(pos_center);
-			SWDScanPosition pos_edge = { a_seg.lineIdx, a_seg.outsideEdgePoint.nPointIdx };
-			outsideEdgePtIndice.push_back(pos_edge);
-		}
-		else
-		{
-			SWDScanPosition pos_center = { a_seg.centerPoint.nPointIdx, a_seg.lineIdx};
-			centerPtIndice.push_back(pos_center);
-			SWDScanPosition pos_edge = { a_seg.outsideEdgePoint.nPointIdx, a_seg.lineIdx};
-			outsideEdgePtIndice.push_back(pos_edge);
-		}
-	}
 
 	//聚类
 	int clusterCheckWin = 5;
@@ -754,15 +749,48 @@ WD_workpieceInfo _computeWorkpiecePose(
 		for (int m = 0; m < nodeSize; m++)
 			pointArray.push_back(a_cluster[m].outsideEdgePoint.pt3D);
 	}
-
 	SVzNL3DPoint center;
 	double radius;
-	double err = fitCircleByLeastSquare(pointArray,center,radius);
+	double err = fitCircleByLeastSquare(pointArray, center, radius);
+
+	//迭代一次
+	pointArray.clear();
+	for (int i = 0; i < (int)objClusters.size(); i++)
+	{
+		std::vector< SWD_indexingSeg>& a_cluster = objClusters[i];
+		int nodeSize = (int)a_cluster.size();
+		for (int m = 0; m < nodeSize; m++)
+		{
+			SWD_indexingSeg& a_seg = a_cluster[m];
+			double r1 = sqrt(pow(center.x - a_seg.outsideEdgePoint.pt3D.x, 2) + pow(center.y - a_seg.outsideEdgePoint.pt3D.y, 2));
+			if (r1 < workpieceParam.radius * 1.05)
+			{
+				pointArray.push_back(a_cluster[m].outsideEdgePoint.pt3D);
+				//生成标注需要的信息
+				if (a_seg.hScanFlag == 0)
+				{
+					SWDScanPosition pos_center = { a_seg.lineIdx, a_seg.centerPoint.nPointIdx };
+					centerPtIndice.push_back(pos_center);
+					SWDScanPosition pos_edge = { a_seg.lineIdx, a_seg.outsideEdgePoint.nPointIdx };
+					outsideEdgePtIndice.push_back(pos_edge);
+				}
+				else
+				{
+					SWDScanPosition pos_center = { a_seg.centerPoint.nPointIdx, a_seg.lineIdx };
+					centerPtIndice.push_back(pos_center);
+					SWDScanPosition pos_edge = { a_seg.outsideEdgePoint.nPointIdx, a_seg.lineIdx };
+					outsideEdgePtIndice.push_back(pos_edge);
+				}
+			}
+		}
+	}
+
+	err = fitCircleByLeastSquare(pointArray, center, radius);
 	a_pose.center = { center.x, center.y, objZ };
 	a_pose.value = radius;
 	a_pose.z_dir = { 0.0, 0.0, 1.0 };
 	a_pose.y_dir = { 0.0, 0.0, 0.0 };
-	a_pose.z_dir = { 0.0, 0.0, 0.0 };
+	a_pose.x_dir = { 0.0, 0.0, 0.0 };
 	return a_pose;
 }
 
@@ -771,6 +799,38 @@ typedef struct
 	std::vector<SWDScanPosition> mappings;//每个2D点可能对应多个3D点
 }mapping2DTo3D;
 
+bool _compareByXValue(WD_workpieceInfo& a, WD_workpieceInfo& b)
+{
+	return a.center.x < b.center.x;
+}
+void _getYTopLine(
+	std::vector< WD_workpieceInfo>& workpieceSrc,
+	std::vector< WD_workpieceInfo>& firstLine,
+	std::vector< WD_workpieceInfo>& restWorkpiece,
+	double yLayerTh)
+{
+	//搜索Y最小值
+	if (workpieceSrc.size() == 0)
+		return;
+
+	double minY = workpieceSrc[0].center.y;
+	for (int i = 1; i < (int)workpieceSrc.size(); i++)
+	{
+		if (minY > workpieceSrc[i].center.y)
+			minY = workpieceSrc[i].center.y;
+	}
+	double topLayerTh = minY + yLayerTh;
+	for (int i = 0; i < (int)workpieceSrc.size(); i++)
+	{
+		if (workpieceSrc[i].center.y < topLayerTh)
+			firstLine.push_back(workpieceSrc[i]);
+		else
+			restWorkpiece.push_back(workpieceSrc[i]);
+	}
+	std::sort(firstLine.begin(), firstLine.end(), _compareByXValue);
+	return;
+}
+
 void wd_HRM_RotorCorePositioning(
 	std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
 	std::vector<WD_objArea2D>& objROIs,
@@ -978,8 +1038,8 @@ void wd_HRM_RotorCorePositioning(
 		}
 	}
 
-	int objNum = 1; // (int)objROIs.size();
+	int objNum =  (int)objROIs.size();
-	
+	std::vector<WD_workpieceInfo> allWorkpiece;
 	for (int idx = 0; idx < objNum; idx++)
 	{
 		WD_objArea2D& obj_roi = objROIs[idx];
@@ -1009,7 +1069,8 @@ void wd_HRM_RotorCorePositioning(
 					int ptIdx = a_mapping.mappings[m].ptIdx;
 					if (scanLines[lineIdx][ptIdx].pt3D.z > 1e-4)
 					{
-						scanLinesInput[lineIdx][ptIdx].nPointIdx = idx + 1;
+						scanLinesInput[lineIdx][ptIdx].nPointIdx &= 0xffff0000;
+						scanLinesInput[lineIdx][ptIdx].nPointIdx |= (idx + 1);
 						roiLineIndice.nMin = roiLineIndice.nMin > lineIdx ? lineIdx : roiLineIndice.nMin;
 						roiLineIndice.nMax = roiLineIndice.nMax < lineIdx ? lineIdx : roiLineIndice.nMax;
 						roiPtIndice.nMin = roiPtIndice.nMin > ptIdx ? ptIdx : roiPtIndice.nMin;
@@ -1041,7 +1102,8 @@ void wd_HRM_RotorCorePositioning(
 						int kkk = 1;
 					if (scanLines[lineIdx][ptIdx].pt3D.z > 1e-4)
 					{
-						scanLines[lineIdx][ptIdx].nPointIdx = idx + 1;
+						scanLines[lineIdx][ptIdx].nPointIdx &= 0xffff0000;
+						scanLines[lineIdx][ptIdx].nPointIdx |= idx + 1;
 						int roiLineIdx = lineIdx - roiLineIndice.nMin;
 						int roiPtIdx = ptIdx - roiPtIndice.nMin;
 						roiScanLines[roiLineIdx][roiPtIdx].nPointIdx = 0;
@@ -1062,6 +1124,7 @@ void wd_HRM_RotorCorePositioning(
 				}
 			}
 		}
+
 		//判断工件种类，计算工件姿态
 		std::vector<SWDScanPosition> outsideEdgePtIndice;
 		std::vector<SWDScanPosition> centerPtIndice;
@@ -1073,25 +1136,70 @@ void wd_HRM_RotorCorePositioning(
 			centerPtIndice);
 	
 #ifdef _OUTPUT_DEBUG_DATA
-		//标注
+		if (a_workpiece.center.z > 1e-4)
-		for (int i = 0; i < (int)outsideEdgePtIndice.size(); i++)
 		{
-			int line = outsideEdgePtIndice[i].lineIdx + roiLineIndice.nMin;
+			//标注
-			int ptIdx = outsideEdgePtIndice[i].ptIdx + roiPtIndice.nMin;
+			for (int i = 0; i < (int)outsideEdgePtIndice.size(); i++)
-			scanLines[line][ptIdx].nPointIdx |= 0x10000;
+			{
-		}
+				int line = outsideEdgePtIndice[i].lineIdx + roiLineIndice.nMin;
-		for (int i = 0; i < (int)centerPtIndice.size(); i++)
+				int ptIdx = outsideEdgePtIndice[i].ptIdx + roiPtIndice.nMin;
-		{
+				scanLines[line][ptIdx].nPointIdx |= 0x10000;
-			int line = centerPtIndice[i].lineIdx + roiLineIndice.nMin;
+			}
-			int ptIdx = centerPtIndice[i].ptIdx + roiPtIndice.nMin;
+			for (int i = 0; i < (int)centerPtIndice.size(); i++)
-			scanLines[line][ptIdx].nPointIdx |= 0x20000;
+			{
+				int line = centerPtIndice[i].lineIdx + roiLineIndice.nMin;
+				int ptIdx = centerPtIndice[i].ptIdx + roiPtIndice.nMin;
+				scanLines[line][ptIdx].nPointIdx |= 0x20000;
+			}
 		}
 #endif
 
 		if (a_workpiece.center.z > 1e-4)
-			workpiecePositions.push_back(a_workpiece);
+			allWorkpiece.push_back(a_workpiece);
 
 	}
+	if (allWorkpiece.size() == 0)
+		return;
+
+	//排序
+	//搜索最高目标
+	double highest_z = allWorkpiece[0].center.z;
+	for (int i = 1; i < (int)allWorkpiece.size(); i++)
+		highest_z = highest_z > allWorkpiece[i].center.z ? allWorkpiece[i].center.z : highest_z;
+	//z方向排序 
+	std::vector< WD_workpieceInfo> zSortWorkpiece;
+	double topLayerTh = highest_z + workpieceParam.height * 0.75;
+	for (int i = 0; i < (int)allWorkpiece.size(); i++)
+	{
+		if (allWorkpiece[i].center.z < topLayerTh)
+			zSortWorkpiece.push_back(allWorkpiece[i]);
+	}
+	//水平方向排序
+	while (zSortWorkpiece.size() > 0)
+	{
+		std::vector< WD_workpieceInfo> firstLine;
+		std::vector< WD_workpieceInfo> restWorkpiece;
+		_getYTopLine(zSortWorkpiece, firstLine, restWorkpiece, workpieceParam.radius);
+		workpiecePositions.insert(workpiecePositions.end(), firstLine.begin(), firstLine.end());
+		zSortWorkpiece.clear();
+		zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end());
+	}
+
+	//旋转回去
+	int workpieceNum = (int)workpiecePositions.size();
+	for (int i = 0; i < workpieceNum; i++)
+	{
+		SVzNL3DPoint rpt;
+		rpt =  wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix);
+		workpiecePositions[i].center = rpt;
+		rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix);
+		workpiecePositions[i].y_dir = rpt;
+		rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix);
+		workpiecePositions[i].z_dir = rpt;
+		rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix);
+		workpiecePositions[i].x_dir = rpt;
+	}
+
 #ifdef _OUTPUT_DEBUG_DATA
 	debugScanLines.resize(scanLines.size());
 	for (int line = 0; line < (int)scanLines.size(); line++)