Merge pull request #24897 from JStech:fix-handeye

JStech · web-flow · commit 7b31cc731416 · 2024-05-25T11:28:13.000+03:00
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
diff --git 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
@@ -289,32 +289,55 @@ static void calibrateHandEyeTsai(const std::vector<Mat>& Hg, const std::vector<M
     int idx = 0;
     for (size_t i = 0; i < Hg.size(); i++)
     {
-        for (size_t j = i+1; j < Hg.size(); j++, idx++)
+        for (size_t j = i+1; j < Hg.size(); j++)
         {
             //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 = homogeneousInverse(Hg[j]) * Hg[i]; //eq 6
-            vec_Hgij.push_back(Hgij);
             //Rotation axis for Rgij which is the 3D rotation from gripper coordinate frame Gi to Gj
             Mat Pgij = 2*rot2quatMinimal(Hgij);
 
             //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 = Hc[j] * homogeneousInverse(Hc[i]); //eq 7
-            vec_Hcij.push_back(Hcij);
             //Rotation axis for Rcij
             Mat Pcij = 2*rot2quatMinimal(Hcij);
 
+            // Discard motions with rotation too small or too close to pi radians
+            //   The limits 1.7 and 0.3 correspond to angles less than 17 degrees or greater than 120 degrees. They are
+            //   based on verifying equation 12 from the source paper using data generated with a known hand-eye
+            //   calibration. The data contained 25 poses, so 300 motions were considered. Of these, 188 satisfied
+            //   equation 12, and the remaining 112 all had Pcij or Pgij with norms greater than 1.7. Although errors
+            //   from small rotations were not observed, it is known that these motions are less informative (see
+            //   section II.B.3, and figure 6).
+            double Pgij_norm = cv::norm(Pgij);
+            double Pcij_norm = cv::norm(Pcij);
+            if (Pgij_norm < 0.3 || Pcij_norm < 0.3 || Pgij_norm > 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<Mat>& Hg, const std::vector<M
 
     Mat Rcg = quatMinimal2rot(Pcg/2.0);
 
-    idx = 0;
-    for (size_t i = 0; i < Hg.size(); i++)
+    for (size_t i = 0; i < vec_Hgij.size(); i++)
     {
-        for (size_t j = i+1; j < Hg.size(); j++, 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[static_cast<size_t>(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<size_t>(idx)];
-
-            //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)));
-
-            //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)));
-        }
+        //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, static_cast<int>(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, static_cast<int>(i)*3, 1, 3)));
     }
 
     Mat Tcg;
@@ -449,6 +468,9 @@ static void calibrateHandEyeHoraud(const std::vector<Mat>& Hg, const std::vector
             Mat Rcij = Hcij(Rect(0, 0, 3, 3));
 
             Mat qgij = rot2quat(Rgij);
+            if (qgij.at<double>(0, 0) < 0) {
+                qgij *= -1;
+            }
             double r0 = qgij.at<double>(0,0);
             double rx = qgij.at<double>(1,0);
             double ry = qgij.at<double>(2,0);
@@ -461,6 +483,9 @@ static void calibrateHandEyeHoraud(const std::vector<Mat>& Hg, const std::vector
                         rz, -ry,  rx,  r0);
 
             Mat qcij = rot2quat(Rcij);
+            if (qcij.at<double>(0, 0) < 0) {
+                qcij *= -1;
+            }
             r0 = qcij.at<double>(0,0);
             rx = qcij.at<double>(1,0);
             ry = qcij.at<double>(2,0);
@@ -618,7 +643,13 @@ static void calibrateHandEyeDaniilidis(const std::vector<Mat>& Hg, const std::ve
             Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
 
             Mat dualqa = homogeneous2dualQuaternion(Hgij);
+            if (dualqa.at<double>(0, 0) < 0) {
+                dualqa *= -1;
+            }
             Mat dualqb = homogeneous2dualQuaternion(Hcij);
+            if (dualqb.at<double>(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
@@ -756,4 +756,110 @@ TEST(Calib3d_CalibrateRobotWorldHandEye, regression)
     }
 }
 
+TEST(Calib3d_CalibrateHandEye, regression_24871)
+{
+    std::vector<Mat> R_target2cam, t_target2cam;
+    std::vector<Mat> R_gripper2base, t_gripper2base;
+    Mat T_true_cam2gripper;
+
+    T_true_cam2gripper = (cv::Mat_<double>(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_<double>(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_<double>(3, 1) <<
+            -1.588728904724121,
+            0.07843752950429916,
+            -1.002813339233398));
+
+    R_gripper2base.push_back((cv::Mat_<double>(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_<double>(3, 1) <<
+            -1.249274406461827,
+            -1.916570771580279,
+            2.005069553422765));
+
+    R_target2cam.push_back((cv::Mat_<double>(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_<double>(3, 1) <<
+            -1.906493663787842,
+            -0.07281044125556946,
+            0.6088893413543701));
+
+    R_gripper2base.push_back((cv::Mat_<double>(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_<double>(3, 1) <<
+            -0.7375172846804027,
+            -2.579760910816792,
+            1.336561572270101));
+
+    R_target2cam.push_back((cv::Mat_<double>(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_<double>(3, 1) <<
+            -0.9809040427207947,
+            -0.2707894444465637,
+            -0.2577074766159058));
+
+    R_gripper2base.push_back((cv::Mat_<double>(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_<double>(3, 1) <<
+            -1.040123533893404,
+            -0.1303773962721222,
+            1.068029475621886));
+
+    R_target2cam.push_back((cv::Mat_<double>(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_<double>(3, 1) <<
+            -1.58987033367157,
+            -1.924914002418518,
+            -0.3109001517295837));
+
+    R_gripper2base.push_back((cv::Mat_<double>(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_<double>(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<HandEyeCalibrationMethod> 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
