更新了手眼标定算法和验算

workpieceHolePositioning_test/workpieceHolePositioning_test.cpp

@@ -470,25 +470,7 @@ int main()
 	pts_robot[4] = { -58.843, -969.793, 95.297 }; // A:-1.029 B : -0.367 C : 0.442
 	pts_robot[5] = { 108.031, -1029.803, 98.333 }; // A:-9.208 B : -0.367 C : 0.442
 
-	std::vector< cv::Point3d>  rpy_eye;
+	//使用前6组数据
-	rpy_eye.resize(6);
-	std::vector< cv::Point3d>  rpy_robot;
-	rpy_robot.resize(6);
-	rpy_eye[0] = { 0.6413, 0.0302, -91.1494 };
-	rpy_eye[1] = { 0.6413, 0.0302, -88.8130 };
-	rpy_eye[2] = { 0.6413, 0.0302, -89.3432 };
-	rpy_eye[3] = { 0.6413, 0.0302, -89.5285 };
-	rpy_eye[4] = { 0.6413, 0.0302, -89.6450 };
-	rpy_eye[5] = { 0.6413, 0.0302, -82.0278 };
-
-	rpy_robot[0] = { -0.292, -0.638, 0.018 }; // A:0.018 B : -0.638 C : -0.292
-	rpy_robot[1] = { 1.001, -0.354, -1.032 }; //  A:-1.032 B : -0.354 C : 1.001
-	rpy_robot[2] = { 0.998, -0.356, -1.032 }; // A:-1.032 B : -0.356 C : 0.998
-	rpy_robot[3] = { 0.442, -0.367,-1.028 }; // A:-1.028 B :-0.367 C : 0.442
-	rpy_robot[4] = { 0.442, -0.367, -1.029 }; // A:-1.029 B : -0.367 C : 0.442
-	rpy_robot[5] = { 0.442, -0.367, -9.208 }; // A:-9.208 B : -0.367 C : 0.442
-
-	//使用前5组数据
 	std::vector<cv::Point3d> test_pts_eye;
 	std::vector<cv::Point3d> test_pts_robot;
 	for (int i = 0; i < 5; i++)
@@ -506,26 +488,38 @@ int main()
 		R, T,
 		C_eye, C_robot);
 
+	std::cout << "方向向量转换结果：" << std::endl;
+	std::cout << std::fixed << std::setprecision(6); // 固定小数位数为6
+	std::cout << R << std::endl;
+	std::cout << T << std::endl;
+
+	//验算6轴姿态
+	std::vector<std::vector< cv::Point3d>>  pose_eye;
+	pose_eye.resize(6);
+	for (int i = 0; i < 6; i++)
+		pose_eye[i].resize(3);
+	pose_eye[0][0] = { -0.020, -1.000, -0.011 }; pose_eye[0][1] = { 1.000, -0.020, -0.001 }; pose_eye[0][2] = { 0.001, -0.011, 1.000 };
+	pose_eye[1][0] = { 0.021,-1.000,-0.011 }; pose_eye[1][1] = { 1.000,0.021,-0.000 }; pose_eye[1][2] = { 0.001,-0.011,1.000 };
+	pose_eye[2][0] = { 0.011,-1.000,-0.011 }; pose_eye[2][1] = { 1.000,0.011,-0.000 }; pose_eye[2][2] = { 0.001,-0.011,1.000 };
+	pose_eye[3][0] = { 0.008,-1.000,-0.011 }; pose_eye[3][1] = { 1.000,0.008,-0.000 }; pose_eye[3][2] = { 0.001,-0.011,1.000 };
+	pose_eye[4][0] = { 0.006,-1.000,-0.011 }; pose_eye[4][1] = { 1.000,0.006,-0.000 }; pose_eye[4][2] = { 0.001,-0.011,1.000 };
+	pose_eye[5][0] = { 0.139,-0.990,-0.011 }; pose_eye[5][1] = { 0.990,0.139,0.001 }; pose_eye[5][2] = { 0.001,-0.011,1.000 };
+
+	for (int i = 0; i < 6; i++)
+	{
 		cv::Point3d rtPt;
-	pointRT_2(R, T, pts_eye[5], rtPt);  //RT前后的点
+		pointRT_2(R, T, pts_eye[i], rtPt);  //RT前后的点
-	//根据欧拉角计算方向向量
-	std::vector<cv::Point3d> vectors;
-
-	double rpy[3] = { rpy_eye[5].x, rpy_eye[5].y, rpy_eye[5].z };
-	std::vector<cv::Point3d> dirVectors_eye;
-	WD_EulerRpyToDirVectors(rpy, dirVectors_eye);
 
+		std::vector<cv::Point3d> dirVectors_eye = pose_eye[i];
+		dirVectors_eye[1] = { -dirVectors_eye[1].x, -dirVectors_eye[1].y, -dirVectors_eye[1].z };
+		dirVectors_eye[2] = { -dirVectors_eye[2].x, -dirVectors_eye[2].y, -dirVectors_eye[2].z };
 		std::vector<cv::Point3d> dirVectors_robot;
-	for (int i = 0; i < 3; i++)
+		for (int j = 0; j < 3; j++)
 		{
 			cv::Point3d rt_pt;
-		pointRotate(R, dirVectors_eye[i], rt_pt);
+			pointRotate(R, dirVectors_eye[j], rt_pt);
 			dirVectors_robot.push_back(rt_pt);
 		}
-	//dirVectors_robot[0] = { -dirVectors_robot[0].x, dirVectors_robot[0].y, dirVectors_robot[0].z };
-	dirVectors_robot[1] = { -dirVectors_robot[1].x, dirVectors_robot[1].y, dirVectors_robot[1].z };
-	dirVectors_robot[2] = { -dirVectors_robot[2].x, dirVectors_robot[2].y, dirVectors_robot[2].z };
-
 		//生成旋转矩阵
 		double R_pose[3][3];
 		R_pose[0][0] = dirVectors_robot[0].x;
@@ -538,6 +532,10 @@ int main()
 		R_pose[2][1] = dirVectors_robot[1].z;
 		R_pose[2][2] = dirVectors_robot[2].z;
 		SSG_EulerAngles test_rpy = rotationMatrixToEulerZYX(R_pose);
+		std::cout << i << ":" << std::endl;
+		std::cout << rtPt.x << "," << rtPt.y << "," << rtPt.z << std::endl;
+		std::cout << test_rpy.roll << "," << test_rpy.pitch << "," << test_rpy.yaw << std::endl;
+	}
 #endif
 #if TEST_COMPUTE_CALIB_PARA
 	char _calib_datafile[256];