workpieceHolePositioning version 1.6.0 : 添加了玖瑞工件（单件）的姿态计算。

sourceCode/workpieceHolePositioning.cpp

@@ -29,7 +29,8 @@
 //version 1.5.4 : 添加山东本事机电轮胎定位API。
 //version 1.5.5 : 山东本事机电轮胎定位。（1）添加了轮胎内径大小的输出， （2）按高度排序。最上面的目标排在第一个。
 //version 1.5.6 : 山东本事机电轮胎定位:优化了轮胎定位算法，使用圆扫描提取轮廓点。
-std::string	m_strVersion = "HolePostion 1.5.6";
+//version 1.6.0 : 添加了玖瑞工件（单件）的姿态计算。
+std::string	m_strVersion = "HolePostion 1.6.0";
 const char* wd_workpieceHolePositioningVersion(void)
 {
 	return m_strVersion.c_str();
@@ -2824,7 +2825,8 @@ void _computeAnlgeScanData(
 		angle = (angle / PI) * 180 + 180.0;
 		double R = sqrt(pow(a_pt.y - centerPoint.y, 2) + pow(a_pt.x - centerPoint.x, 2));
 
-		int angleLine = (int)(angle / angleScale);
+		int angleLine = (int)(angle / angleScale + 0.5);
+		angleLine = angleLine % (int)polarScanData.size();
 
 		SWD_polarPt a_polarPt;
 		a_polarPt.lineIdx = line;
@@ -4313,7 +4315,7 @@ void Jiurui_getWorkpiecePose(
 	centroid.z = centroid.z / centroid_size;
 
 	//圆周扫描:0.5度间隔
-	double angleScale = 0.5;
+	double angleScale = 1.0;
 	std::vector<std::vector<SWD_polarPt>> polarScanData;
 	int polarLines = (int)(360.0 / angleScale + 0.5);
 	polarScanData.resize(polarLines);
@@ -4330,12 +4332,17 @@ void Jiurui_getWorkpiecePose(
 	 }
 	
 	//提取平面点-计算法向
-	double distToPlane_min = 17.5;
+	double distToPlane_min = 19.5;
-	double distToPlane_max = 18.5;
+	double distToPlane_max = 20.5;
 	std::vector<SWD_polarPt> planePolarPoints;
 	std::vector<cv::Point3d> Points3ds;
 	for (int i = 0; i < polarLines; i++)
 	{
+		double angle = angleScale * i;
+		if ((angle < 30) || ((angle > 75) && (angle < 120)) || ((angle > 150) && (angle < 210)) ||
+			((angle > 240) && (angle < 300)) || (angle > 330))  //避开水平和垂直区域
+			continue;
+
 		int lineSize = (int)polarScanData[i].size();
 		if (lineSize == 0)
 			continue;
@@ -4367,6 +4374,8 @@ void Jiurui_getWorkpiecePose(
 
 	//计算投影向量
 	SVzNL3DPoint vec_1 = { res[0], res[1], res[2]};
+	if (res[2] < 0)
+		vec_1 = { -res[0], -res[1], -res[2] };
 	SVzNL3DPoint vec_z = { 0, 0, 1.0 };
 	SSG_planeCalibPara poseR = wd_computeRTMatrix(vec_1, vec_z);
 
@@ -4385,19 +4394,22 @@ void Jiurui_getWorkpiecePose(
 	// 
 	//迭代计算质心
 	centroid = { 0, 0, 0 };
-	for (int i = 0; i < centroid_size; i++)
+	std::vector< SVzNL2DPointD> points_2d;
+	points_2d.resize(projectPosition3ds.size());
+	for (int i = 0; i < (int)projectPosition3ds.size(); i++)
 	{
-		int line = objClusters[topClusterId][i].nPointIdx >> 16;
+		centroid.x += projectPosition3ds[i].pt3D.x;
-		int ptIdx = objClusters[topClusterId][i].nPointIdx & 0x0000FFFF;
+		centroid.y += projectPosition3ds[i].pt3D.y;
-		centroid.x += scanLinesInput[line][ptIdx].pt3D.x;
+		centroid.z += projectPosition3ds[i].pt3D.z;
-		centroid.y += scanLinesInput[line][ptIdx].pt3D.y;
+		points_2d[i] = { projectPosition3ds[i].pt3D.x ,  projectPosition3ds[i].pt3D.y };
-		centroid.z += scanLinesInput[line][ptIdx].pt3D.z;
 	}
 	centroid.x = centroid.x / centroid_size;
 	centroid.y = centroid.y / centroid_size;
 	centroid.z = centroid.z / centroid_size;
 
 	//圆周扫描:0.5度间隔
+	angleScale = 1.0;
+	polarLines = (int)(360.0 / angleScale + 0.5);
 	polarScanData.clear();
 	polarScanData.resize(polarLines);
 	_computeAnlgeScanData(
@@ -4429,30 +4441,43 @@ void Jiurui_getWorkpiecePose(
 	objCenter.y = objCenter.y / centerSum;
 	objCenter.z = objCenter.z / centerSum;
 
-	//计算长轴方向
+#if 0
+	//计算长轴方向: 长轴短轴比最大
+	int degree90 = (int)(90.0 / angleScale);
 	int degree180 = (int)(180.0 / angleScale);
 	int longAxisIndex = -1;
-	double longAxisLen = -1;
+	double axisLenRatio = 0;
+	std::vector<double> ratioBuff;
+	ratioBuff.resize(polarLines / 2);
+	std::vector<double> sumRBuff;
+	sumRBuff.resize(polarLines / 2);
 	for (int i = 0; i < polarLines/2; i++)
 	{
 		int idx_1 = i;
-		int idx_2 = i + degree180;
+		int idx_2 = (i + degree180) % polarLines;
+		int idx_3 = (i + degree90) % polarLines;
+		int idx_4 = (idx_3 + degree180) % polarLines;
 		if (idx_2 < polarLines)
 		{
-			if ((polarScanData[idx_1].size() == 0) || (polarScanData[idx_2].size() == 0))
+			if ((polarScanData[idx_1].size() > 0) && (polarScanData[idx_2].size() > 0) &&
+				(polarScanData[idx_3].size() > 0) && (polarScanData[idx_4].size() > 0))
 			{
-				double sumR = polarScanData[idx_1][0].R + polarScanData[idx_2][0].R;
+				double sumR_long = polarScanData[idx_1].back().R + polarScanData[idx_2].back().R;
+				double sumR_short = polarScanData[idx_3].back().R + polarScanData[idx_4].back().R;
+				double ratio = sumR_long / sumR_short;
+				sumRBuff[i] = sumR_long;
+				ratioBuff[i] = ratio;
 				if (longAxisIndex < 0)
 				{
 					longAxisIndex = idx_1;
-					longAxisLen = sumR;
+					axisLenRatio = ratio;
 				}
 				else
 				{
-					if (longAxisLen < sumR)
+					if (axisLenRatio < ratio)
 					{
 						longAxisIndex = idx_1;
-						longAxisLen = sumR;
+						axisLenRatio = ratio;
 					}
 				}
 			}
@@ -4464,8 +4489,14 @@ void Jiurui_getWorkpiecePose(
 		return;
 	}
 
-	double longAxisAngle = longAxisIndex * angleScale;
+	double longAxisAngle = longAxisIndex * angleScale * PI / 180;
-	SVzNL3DPoint vector_y = {cos(longAxisAngle/PI), sin(longAxisAngle / PI), 0 };
+	SVzNL3DPoint vector_y = { cos(longAxisAngle), sin(longAxisAngle), 0 };
+#else
+	//PCA方法
+	SVzNL2DPointD axis, roi_centroid;
+	pca2D(points_2d, axis, roi_centroid);
+	SVzNL3DPoint vector_y = { axis.x, axis.y, 0 };
+#endif
 	//叉乘出vector_x
 	SVzNL3DPoint vector_x;
 	vector_x.x = vector_y.y * vec_z.z - vec_z.y * vector_y.z;

workpieceHolePositioning_test/workpieceHolePositioning_test.cpp

@@ -1701,11 +1701,34 @@ void JiuRui_workpiecePosition_test(void)
 		{
 			//fidx =17;
 			char _scan_file[256];
-			sprintf_s(_scan_file, "%s%d_LaserData_Hi229229.txt", dataPath[grp], fidx);
+			sprintf_s(_scan_file, "%s%d_LaserData_Hi229229_sampleDown.txt", dataPath[grp], fidx);
 			std::vector<std::vector< SVzNL3DPosition>> scanLines;
 			vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
 			if (scanLines.size() == 0)
 				continue;
+#if 0
+			//抽样
+			std::vector<std::vector< SVzNL3DPosition>> sampleData;
+			int sampleLines = (int)scanLines.size() / 4;
+			sampleData.resize(sampleLines);
+			int sLine = 0;
+			for (int i = 0; i < (int)scanLines.size(); i++)
+			{
+				if (i % 4 == 0)
+				{
+					sampleData[sLine].resize(scanLines[i].size());
+					std::copy(scanLines[i].begin(), scanLines[i].end(), sampleData[sLine].begin()); // 使用std::copy算法
+					sLine++;
+					if (sLine >= sampleLines)
+						break;
+				}
+			}
+			//写入
+			sprintf_s(_scan_file, "%s%d_LaserData_Hi229229_sampleDown.txt", dataPath[grp], fidx);
+			int headNullLines = 0;
+			_outputScanDataFile_vector(_scan_file, sampleData, false, &headNullLines);
+			continue;
+#endif
 
 			long t1 = (long)GetTickCount64();//统计时间
 
@@ -1766,9 +1789,9 @@ int main()
 	//ESG_testMode testMode = keSG_华航孔定位;
 	//ESG_testMode testMode = keSG_微力砂轮盘定位;
 	//ESG_testMode testMode = keSG_微力砂轮架子定位;
-	ESG_testMode testMode = keSG_山东本事轮胎定位;
+	//ESG_testMode testMode = keSG_山东本事轮胎定位;
 	//ESG_testMode testMode = keSG_宁波海瑞马转子芯定位;
-	//ESG_testMode testMode = keSG_北京玖瑞工件定位;
+	ESG_testMode testMode = keSG_北京玖瑞工件定位;
 
 	if (keSG_拓普发孔定位 == testMode)
 		TuoPuFa_holePosition_test();