hybridPosePositioning

version 1.3.1 : 添加了海瑞马转子钢芯定位位置属性计算，区分出中央，边和角的位置，按照中心排序，先输出最中央的目标

bagPositioning_test/bagPositioning_test.vcxproj

@@ -42,13 +42,13 @@
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
     <ConfigurationType>Application</ConfigurationType>
     <UseDebugLibraries>true</UseDebugLibraries>
-    <PlatformToolset>v142</PlatformToolset>
+    <PlatformToolset>v143</PlatformToolset>
     <CharacterSet>Unicode</CharacterSet>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
     <ConfigurationType>Application</ConfigurationType>
     <UseDebugLibraries>false</UseDebugLibraries>
-    <PlatformToolset>v142</PlatformToolset>
+    <PlatformToolset>v143</PlatformToolset>
     <WholeProgramOptimization>true</WholeProgramOptimization>
     <CharacterSet>Unicode</CharacterSet>
   </PropertyGroup>

hybridPosePositioning_test/hybridPosePositioning_test.cpp

@@ -2110,6 +2110,10 @@ void HaiRuiMa_rotorCorePositioning_test(void)
 		sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
 		groundCalibPara = _readCalibPara(calibFile);
 
+		//读取料筐参数
+		sprintf_s(calibFile, "%sbin_info.txt", dataPath[grp]);
+		WD_HRM_BinInfo binInfo = _readBinInfo(calibFile);
+
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{
 			//fidx = 3;
@@ -2137,6 +2141,7 @@ void HaiRuiMa_rotorCorePositioning_test(void)
 			wd_HRM_RotorCorePositioning(
 				scanLines,
 				objROIs,
+				binInfo,
 				groundCalibPara,
 				workpieceParam,
 				workpiecePositions,

sourceCode/SG_errCode.h

@@ -29,6 +29,7 @@
 #define SX_ERR_ZERO_OBJ_TOPLAYER	-2102
 #define SX_ERR_ZERO_OBJ_BTMLAYER	-2103
 #define SX_ERR_GET_INVALID_PALTE	-2104  //몰겼瓊혤댄轎
+#define SX_ERR_BIN_EDGE_ERROR		-2105
 
 //뀔관
 #define SX_BAG_TRAY_EMPTY			-2201

sourceCode/hybridPosePositioning.cpp

@@ -11,7 +11,8 @@
 //version 1.1.0 : 海瑞马码垛规划版本
 //version 1.2.0 : 添加了海瑞马锥形工件定位版本（不包含二次定位）
 //version 1.3.0 : 添加了海瑞马转子钢芯定位
-std::string	m_strVersion = "HybridPositioning 1.3.0";
+//version 1.3.1 : 添加了海瑞马转子钢芯定位位置属性计算，区分出中央，边和角的位置，按照中心排序，先输出最中央的目标
+std::string	m_strVersion = "HybridPositioning 1.3.1";
 const char* wd_hybridPositioningVersion(void)
 {
 	return m_strVersion.c_str();
@@ -799,6 +800,16 @@ typedef struct
 	std::vector<SWDScanPosition> mappings;//每个2D点可能对应多个3D点
 }mapping2DTo3D;
 
+typedef struct
+{
+	double dist;
+	int idx;
+}WD_indexingDistance;
+bool _compareByDistance(WD_indexingDistance& a, WD_indexingDistance& b)
+{
+	return a.dist < b.dist;
+}
+
 bool _compareByXValue(WD_workpieceInfo& a, WD_workpieceInfo& b)
 {
 	return a.center.x < b.center.x;
@@ -834,6 +845,7 @@ void _getYTopLine(
 void wd_HRM_RotorCorePositioning(
 	std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
 	std::vector<WD_objArea2D>& objROIs,
+	const WD_HRM_BinInfo binInfo,
 	const SSG_planeCalibPara groundCalibPara,
 	const SWD_Cylinder workpieceParam,  //圆柱形工件的标称半径和高度
 	std::vector< WD_workpieceInfo>& workpiecePositions,
@@ -1174,7 +1186,259 @@ void wd_HRM_RotorCorePositioning(
 		if (allWorkpiece[i].center.z < topLayerTh)
 			zSortWorkpiece.push_back(allWorkpiece[i]);
 	}
-	//水平方向排序
+
+	if (binInfo.binTopZ > 1e-4)
+	{
+		//计算工件位置属性
+		std::vector<SWD_3DPointPair> linePoint;
+		linePoint.resize(4);
+		linePoint[0].pt1 = binInfo.minRectVertex[0];
+		linePoint[0].pt2 = binInfo.minRectVertex[1];
+		linePoint[1].pt1 = binInfo.minRectVertex[1];
+		linePoint[1].pt2 = binInfo.minRectVertex[2];
+		linePoint[2].pt1 = binInfo.minRectVertex[2];
+		linePoint[2].pt2 = binInfo.minRectVertex[3];
+		linePoint[3].pt1 = binInfo.minRectVertex[3];
+		linePoint[3].pt2 = binInfo.minRectVertex[0];
+		int lineTopIdx = -1;
+		for (int i = 0; i < (int)linePoint.size(); i++)
+		{
+			if ((linePoint[i].pt1.y < binInfo.center.y) && (linePoint[i].pt2.y < binInfo.center.y))
+			{
+				lineTopIdx = i;
+				break;
+			}
+		}
+		if (lineTopIdx < 0)
+		{
+			*errCode = SX_ERR_BIN_EDGE_ERROR;
+			//旋转回去
+			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;
+			}
+			return;
+		}
+		SWD_3DPointPair lineTop = linePoint[lineTopIdx];
+		linePoint.erase(linePoint.begin() + lineTopIdx);//去掉已经识别的edge
+
+		int lineBtmIdx = -1;
+		for (int i = 0; i < (int)linePoint.size(); i++)
+		{
+			if ((linePoint[i].pt1.y > binInfo.center.y) && (linePoint[i].pt2.y > binInfo.center.y))
+			{
+				lineBtmIdx = i;
+				break;
+			}
+		}
+		if (lineBtmIdx < 0)
+		{
+			*errCode = SX_ERR_BIN_EDGE_ERROR;
+			//旋转回去
+			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;
+			}
+			return;
+		}
+		SWD_3DPointPair lineBtm = linePoint[lineBtmIdx];
+		linePoint.erase(linePoint.begin() + lineBtmIdx);	//去掉已经识别的edge
+
+		int lineLeftIdx = -1;
+		for (int i = 0; i < (int)linePoint.size(); i++)
+		{
+			if ((linePoint[i].pt1.x < binInfo.center.x) && (linePoint[i].pt2.x < binInfo.center.x))
+			{
+				lineLeftIdx = i;
+				break;
+			}
+		}
+		if (lineLeftIdx < 0)
+		{
+			*errCode = SX_ERR_BIN_EDGE_ERROR;
+			//旋转回去
+			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;
+			}
+			return;
+		}
+		SWD_3DPointPair lineLeft = linePoint[lineLeftIdx];
+		linePoint.erase(linePoint.begin() + lineLeftIdx);	//去掉已经识别的edge
+
+		int lineRightIdx = -1;
+		for (int i = 0; i < (int)linePoint.size(); i++)
+		{
+			if ((linePoint[i].pt1.x > binInfo.center.x) && (linePoint[i].pt2.x > binInfo.center.x))
+			{
+				lineRightIdx = i;
+				break;
+			}
+		}
+		if (lineRightIdx < 0)
+		{
+			*errCode = SX_ERR_BIN_EDGE_ERROR;
+			//旋转回去
+			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;
+			}
+			return;
+		}
+		SWD_3DPointPair lineRight = linePoint[lineRightIdx];
+		//计算工件的位置：左上、右上、左下、右下四个角；上下左右四条边：中央部分
+		//机械臂对不同部分的工件会采用不同的抓取策略
+		double diameter = workpieceParam.radius * 2;
+		double lineTop_a, lineTop_b, lineTop_c;
+		compute2ptLine(lineTop.pt1, lineTop.pt2, &lineTop_a, &lineTop_b, &lineTop_c);
+		double lineBtm_a, lineBtm_b, lineBtm_c;
+		compute2ptLine(lineBtm.pt1, lineBtm.pt2, &lineBtm_a, &lineBtm_b, &lineBtm_c);
+		double lineLeft_a, lineLeft_b, lineLeft_c;
+		compute2ptLine(lineLeft.pt1, lineLeft.pt2, &lineLeft_a, &lineLeft_b, &lineLeft_c);
+		double lineRight_a, lineRight_b, lineRight_c;
+		compute2ptLine(lineRight.pt1, lineRight.pt2, &lineRight_a, &lineRight_b, &lineRight_c);
+		std::vector< WD_workpieceInfo> topSide;  //上边
+		std::vector< WD_workpieceInfo> btmSide;  //下边
+		std::vector< WD_workpieceInfo> leftSide; //左边
+		std::vector< WD_workpieceInfo> rightSide;  //右边
+		std::vector< WD_workpieceInfo> LTCorner;  //左上角
+		std::vector< WD_workpieceInfo> RTCorner;  //右上角
+		std::vector< WD_workpieceInfo> LBCorner; //左边
+		std::vector< WD_workpieceInfo> RBCorner;  //右边
+		std::vector< WD_workpieceInfo> centerRegion;  //右边
+		for (int i = 0; i < (int)zSortWorkpiece.size(); i++) //对上层目标进行分析
+		{
+			SVzNL3DPoint& objCenter = zSortWorkpiece[i].center;
+			double distTop = computePtDistToLine(objCenter.x, objCenter.y, lineTop_a, lineTop_b, lineTop_c);
+			double distBtm = computePtDistToLine(objCenter.x, objCenter.y, lineBtm_a, lineBtm_b, lineBtm_c);
+			double distLeft = computePtDistToLine(objCenter.x, objCenter.y, lineLeft_a, lineLeft_b, lineLeft_c);
+			double distRight = computePtDistToLine(objCenter.x, objCenter.y, lineRight_a, lineRight_b, lineRight_c);
+
+			if (distTop < diameter)
+			{
+				if (distLeft < diameter)
+				{
+					zSortWorkpiece[i].workpieceType = 5;  //左上
+					LTCorner.push_back(zSortWorkpiece[i]);
+				}
+				else if (distRight < diameter)
+				{
+					zSortWorkpiece[i].workpieceType = 6;  //右上
+					RTCorner.push_back(zSortWorkpiece[i]);
+				}
+				else
+				{
+					zSortWorkpiece[i].workpieceType = 1;  //上
+					topSide.push_back(zSortWorkpiece[i]);
+				}
+			}
+			else if (distBtm < diameter)
+			{
+				if (distLeft < diameter)
+				{
+					zSortWorkpiece[i].workpieceType = 7;  //左下
+					LBCorner.push_back(zSortWorkpiece[i]);
+				}
+				else if (distRight < diameter)
+				{
+					zSortWorkpiece[i].workpieceType = 8;  //右下
+					RBCorner.push_back(zSortWorkpiece[i]);
+				}
+				else
+				{
+					zSortWorkpiece[i].workpieceType = 2;  //下
+					btmSide.push_back(zSortWorkpiece[i]);
+				}
+			}
+			else if (distLeft < diameter)
+			{
+				zSortWorkpiece[i].workpieceType = 3;  //左
+				leftSide.push_back(zSortWorkpiece[i]);
+			}
+			else if (distRight < diameter)
+			{
+				zSortWorkpiece[i].workpieceType = 4;  //右
+				rightSide.push_back(zSortWorkpiece[i]);
+			}
+			else
+			{
+				zSortWorkpiece[i].workpieceType = 0;  //中央
+				centerRegion.push_back(zSortWorkpiece[i]);
+			}
+		}
+		//对中央部分工件按距中央距离排序
+		if (centerRegion.size() > 0)
+		{
+			std::vector<WD_indexingDistance> distanceIndice;
+			distanceIndice.resize(centerRegion.size());
+			for (int m = 0; m < (int)centerRegion.size(); m++)
+			{
+				distanceIndice[m].dist = sqrt(pow(centerRegion[m].center.x - binInfo.center.x, 2) +
+					pow(centerRegion[m].center.y - binInfo.center.y, 2));
+				distanceIndice[m].idx = m;
+			}
+			std::sort(distanceIndice.begin(), distanceIndice.end(), _compareByDistance);
+			for (int m = 0; m < (int)distanceIndice.size(); m++)
+			{
+				int idx = distanceIndice[m].idx;
+				workpiecePositions.push_back(centerRegion[idx]);
+			}
+		}
+		if(topSide.size() > 0)
+			workpiecePositions.insert(workpiecePositions.end(), topSide.begin(), topSide.end());
+		if (btmSide.size() > 0)
+			workpiecePositions.insert(workpiecePositions.end(), btmSide.begin(), btmSide.end());
+		if(leftSide.size() >0)
+			workpiecePositions.insert(workpiecePositions.end(), leftSide.begin(), leftSide.end());
+		if (rightSide.size() > 0)
+			workpiecePositions.insert(workpiecePositions.end(), rightSide.begin(), rightSide.end());
+		if(LTCorner.size() >0)
+			workpiecePositions.insert(workpiecePositions.end(), LTCorner.begin(), LTCorner.end());
+		if (RTCorner.size() > 0)
+			workpiecePositions.insert(workpiecePositions.end(), RTCorner.begin(), RTCorner.end());
+		if (LBCorner.size() > 0)
+			workpiecePositions.insert(workpiecePositions.end(), LBCorner.begin(), LBCorner.end());
+		if (RBCorner.size() > 0)
+			workpiecePositions.insert(workpiecePositions.end(), RBCorner.begin(), RBCorner.end());
+	}
+	else
+	{//水平方向排序
 		while (zSortWorkpiece.size() > 0)
 		{
 			std::vector< WD_workpieceInfo> firstLine;
@@ -1184,6 +1448,7 @@ void wd_HRM_RotorCorePositioning(
 			zSortWorkpiece.clear();
 			zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end());
 		}
+	}
 
 	//旋转回去
 	int workpieceNum = (int)workpiecePositions.size();

sourceCode/hybridPosePositioning_Export.h

@@ -14,7 +14,7 @@ typedef struct
 
 typedef struct
 {
-	int workpieceType;
+	int workpieceType; //0:中央; 1-上边; 2-下边; 3-左边; 4-右边; 5-左上角; 6-右上角; 7-左下角; 8-右下角;
 	SVzNL3DPoint center;
 	SVzNL3DPoint z_dir;  //z方向向量（归一化）
 	SVzNL3DPoint y_dir; //y方向向量（归一化）
@@ -60,6 +60,7 @@ SG_APISHARED_EXPORT void wd_lineDataR(
 SG_APISHARED_EXPORT void wd_HRM_RotorCorePositioning(
 	std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
 	std::vector<WD_objArea2D>& objROIs,
+	const WD_HRM_BinInfo binInfo,
 	const SSG_planeCalibPara groundCalibPara,
 	const SWD_Cylinder workpieceParam,  //圆柱形工件的标称半径和高度
 	std::vector< WD_workpieceInfo>& workpiecePositions,