jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
jerryzeng 2026-02-02 13:01:23 +08:00
parent 0a2c350aec
commit 4f9d6360af
1 changed files with 45 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
workpieceHolePositioning_test/workpieceHolePositioning_test.cpp
View File

@ -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;
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组数据
//使用前6组数据
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前后的点
//根据欧拉角计算方向向量
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);
pointRT_2(R, T, pts_eye[i], rtPt); //RT前后的点
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];