From 7b31cc731416c66993ca90636d31e9a6447f03e5 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Sat, 25 May 2024 04:28:13 -0400 Subject: [PATCH] Merge pull request #24897 from JStech:fix-handeye Fix handeye #24897 Fixes to the hand-eye calibration methods, from #24871. The Tsai method is sensitive to poses separated by small rotations, so I filter those out. The Horaud and Daniilidis methods use quaternions (and dual quaternions), where $q$ and $-q$ represent the same transform. However, these methods depend on the gripper motion and camera motion having the same sign for the real part. The fix was simply to multiply the (dual) quaternions by -1 if their real part is negative. ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [x] There is a reference to the original bug report and related work - [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [ ] ~~The feature is well documented and sample code can be built with the project CMake~~ N/A --- modules/calib3d/include/opencv2/calib3d.hpp | 1 + modules/calib3d/src/calibration_handeye.cpp | 75 +++++++++---- .../test/test_calibration_hand_eye.cpp | 106 ++++++++++++++++++ 3 files changed, 160 insertions(+), 22 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index aadfff7b1c..0280e05e21 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -48,6 +48,7 @@ #include "opencv2/core/types.hpp" #include "opencv2/features2d.hpp" #include "opencv2/core/affine.hpp" +#include "opencv2/core/utils/logger.hpp" /** @defgroup calib3d Camera Calibration and 3D Reconstruction diff --git a/modules/calib3d/src/calibration_handeye.cpp b/modules/calib3d/src/calibration_handeye.cpp index ab20597c8b..cd4999a618 100644 --- a/modules/calib3d/src/calibration_handeye.cpp +++ b/modules/calib3d/src/calibration_handeye.cpp @@ -289,13 +289,12 @@ static void calibrateHandEyeTsai(const std::vector& Hg, const std::vector& Hg, const std::vector 1.7 || Pcij_norm > 1.7) { + continue; + } + + vec_Hgij.push_back(Hgij); + vec_Hcij.push_back(Hcij); + //Left-hand side: skew(Pgij+Pcij) skew(Pgij+Pcij).copyTo(A(Rect(0, idx*3, 3, 3))); //Right-hand side: Pcij - Pgij Mat diff = Pcij - Pgij; diff.copyTo(B(Rect(0, idx*3, 1, 3))); + idx++; } } + // insufficient data + if (idx < 2) { + CV_LOG_ERROR(NULL, "Hand-eye calibration failed! Not enough informative motions--include larger rotations."); + return; + } + A.resize(3*idx); + B.resize(3*idx); + Mat Pcg_; //Rotation from camera to gripper is obtained from the set of equations: // skew(Pgij+Pcij) * Pcg_ = Pcij - Pgij (eq 12) @@ -327,28 +350,24 @@ static void calibrateHandEyeTsai(const std::vector& Hg, const std::vector(idx)]; - //Defines coordinate transformation from Ci to Cj - //Hci is from CW (calibration target) to Ci (camera) - //Hcj is from CW (calibration target) to Cj (camera) - Mat Hcij = vec_Hcij[static_cast(idx)]; + //Defines coordinate transformation from Gi to Gj + //Hgi is from Gi (gripper) to RW (robot base) + //Hgj is from Gj (gripper) to RW (robot base) + Mat Hgij = vec_Hgij[i]; + //Defines coordinate transformation from Ci to Cj + //Hci is from CW (calibration target) to Ci (camera) + //Hcj is from CW (calibration target) to Cj (camera) + Mat Hcij = vec_Hcij[i]; - //Left-hand side: (Rgij - I) - Mat diff = Hgij(Rect(0,0,3,3)) - Mat::eye(3,3,CV_64FC1); - diff.copyTo(A(Rect(0, idx*3, 3, 3))); + //Left-hand side: (Rgij - I) + Mat diff = Hgij(Rect(0,0,3,3)) - Mat::eye(3,3,CV_64FC1); + diff.copyTo(A(Rect(0, static_cast(i)*3, 3, 3))); - //Right-hand side: Rcg*Tcij - Tgij - diff = Rcg*Hcij(Rect(3, 0, 1, 3)) - Hgij(Rect(3, 0, 1, 3)); - diff.copyTo(B(Rect(0, idx*3, 1, 3))); - } + //Right-hand side: Rcg*Tcij - Tgij + diff = Rcg*Hcij(Rect(3, 0, 1, 3)) - Hgij(Rect(3, 0, 1, 3)); + diff.copyTo(B(Rect(0, static_cast(i)*3, 1, 3))); } Mat Tcg; @@ -449,6 +468,9 @@ static void calibrateHandEyeHoraud(const std::vector& Hg, const std::vector Mat Rcij = Hcij(Rect(0, 0, 3, 3)); Mat qgij = rot2quat(Rgij); + if (qgij.at(0, 0) < 0) { + qgij *= -1; + } double r0 = qgij.at(0,0); double rx = qgij.at(1,0); double ry = qgij.at(2,0); @@ -461,6 +483,9 @@ static void calibrateHandEyeHoraud(const std::vector& Hg, const std::vector rz, -ry, rx, r0); Mat qcij = rot2quat(Rcij); + if (qcij.at(0, 0) < 0) { + qcij *= -1; + } r0 = qcij.at(0,0); rx = qcij.at(1,0); ry = qcij.at(2,0); @@ -618,7 +643,13 @@ static void calibrateHandEyeDaniilidis(const std::vector& Hg, const std::ve Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]); Mat dualqa = homogeneous2dualQuaternion(Hgij); + if (dualqa.at(0, 0) < 0) { + dualqa *= -1; + } Mat dualqb = homogeneous2dualQuaternion(Hcij); + if (dualqb.at(0, 0) < 0) { + dualqb *= -1; + } Mat a = dualqa(Rect(0, 1, 1, 3)); Mat b = dualqb(Rect(0, 1, 1, 3)); diff --git a/modules/calib3d/test/test_calibration_hand_eye.cpp b/modules/calib3d/test/test_calibration_hand_eye.cpp index aa8b34d6d9..edfc5fec0e 100644 --- a/modules/calib3d/test/test_calibration_hand_eye.cpp +++ b/modules/calib3d/test/test_calibration_hand_eye.cpp @@ -756,4 +756,110 @@ TEST(Calib3d_CalibrateRobotWorldHandEye, regression) } } +TEST(Calib3d_CalibrateHandEye, regression_24871) +{ + std::vector R_target2cam, t_target2cam; + std::vector R_gripper2base, t_gripper2base; + Mat T_true_cam2gripper; + + T_true_cam2gripper = (cv::Mat_(4, 4) << 0, 0, -1, 0.1, + 1, 0, 0, 0.2, + 0, -1, 0, 0.3, + 0, 0, 0, 1); + + R_target2cam.push_back((cv::Mat_(3, 3) << + 0.04964505493834381, 0.5136826827431226, 0.8565427426404346, + -0.3923117691818854, 0.7987004864191318, -0.4562554205214679, + -0.9184916136152514, -0.3133809733274676, 0.2411752915926112)); + t_target2cam.push_back((cv::Mat_(3, 1) << + -1.588728904724121, + 0.07843752950429916, + -1.002813339233398)); + + R_gripper2base.push_back((cv::Mat_(3, 3) << + -0.4143743581399177, -0.6105088815982459, -0.6749613298595637, + -0.1598851232573451, -0.6812625208693498, 0.71436554019614, + -0.895952364066927, 0.4039310376145889, 0.1846864320259794)); + t_gripper2base.push_back((cv::Mat_(3, 1) << + -1.249274406461827, + -1.916570771580279, + 2.005069553422765)); + + R_target2cam.push_back((cv::Mat_(3, 3) << + -0.3048000068139332, 0.6971848192711539, 0.6488684640388026, + -0.9377589344241749, -0.3387497187353627, -0.07652979135179161, + 0.1664486009369332, -0.6318084803439735, 0.7570422097951847)); + t_target2cam.push_back((cv::Mat_(3, 1) << + -1.906493663787842, + -0.07281044125556946, + 0.6088893413543701)); + + R_gripper2base.push_back((cv::Mat_(3, 3) << + 0.7262439860936567, -0.201662933718935, -0.6571923111439066, + -0.4640017362244384, -0.8491808316335328, -0.2521791108852766, + -0.5072199339965884, 0.4880819361030014, -0.7102844234575628)); + t_gripper2base.push_back((cv::Mat_(3, 1) << + -0.7375172846804027, + -2.579760910816792, + 1.336561572270101)); + + R_target2cam.push_back((cv::Mat_(3, 3) << + -0.590234879685801, -0.7051138289845309, -0.3929850823848928, + 0.6017371069678565, -0.7088332765096816, 0.3680595606834615, + -0.5380847896941907, -0.01923211603859842, 0.8426712792141644)); + t_target2cam.push_back((cv::Mat_(3, 1) << + -0.9809040427207947, + -0.2707894444465637, + -0.2577074766159058)); + + R_gripper2base.push_back((cv::Mat_(3, 3) << + 0.2541996332132083, 0.6186461729765909, 0.7434106934499181, + 0.2194912986375709, 0.711701808961156, -0.6673111005698995, + -0.9419161938817396, 0.3328024155303503, 0.04512688689130734)); + t_gripper2base.push_back((cv::Mat_(3, 1) << + -1.040123533893404, + -0.1303773962721222, + 1.068029475621886)); + + R_target2cam.push_back((cv::Mat_(3, 3) << + 0.7643667483125168, -0.08523002870239212, 0.63912386614923, + -0.2583463792779588, 0.8676987164647345, 0.424683512464778, + -0.5907627462764713, -0.489729292214425, 0.6412211770980741)); + t_target2cam.push_back((cv::Mat_(3, 1) << + -1.58987033367157, + -1.924914002418518, + -0.3109001517295837)); + + R_gripper2base.push_back((cv::Mat_(3, 3) << + 0.116348305340805, -0.9917998080681939, 0.0528792261688552, + -0.2760629007224059, 0.01884966191381591, 0.9609547154213178, + -0.9540714578526358, -0.1264034452126562, -0.2716060057313114)); + t_gripper2base.push_back((cv::Mat_(3, 1) << + -2.551899142554571, + -2.986937398237611, + 1.317613923218308)); + + Mat R_true_cam2gripper; + Mat t_true_cam2gripper; + R_true_cam2gripper = T_true_cam2gripper(Rect(0, 0, 3, 3)); + t_true_cam2gripper = T_true_cam2gripper(Rect(3, 0, 1, 3)); + + std::vector methods = {CALIB_HAND_EYE_TSAI, + CALIB_HAND_EYE_PARK, + CALIB_HAND_EYE_HORAUD, + CALIB_HAND_EYE_ANDREFF, + CALIB_HAND_EYE_DANIILIDIS}; + + for (auto method : methods) { + SCOPED_TRACE(cv::format("method=%s", getMethodName(method).c_str())); + + Matx33d R_cam2gripper_est; + Matx31d t_cam2gripper_est; + calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, method); + + EXPECT_TRUE(cv::norm(R_cam2gripper_est - R_true_cam2gripper) < 1e-9); + EXPECT_TRUE(cv::norm(t_cam2gripper_est - t_true_cam2gripper) < 1e-9); + } +} + }} // namespace