mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2025-06-07 09:25:45 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #24897 from JStech:fix-handeye

Browse Source 
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
This commit is contained in:
John Stechschulte 2024-05-25 04:28:13 -04:00 committed by GitHub
parent cc49aee04e
commit 7b31cc7314
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
3 changed files with 160 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
modules/calib3d/include/opencv2/calib3d.hpp
View File

@ -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

75
modules/calib3d/src/calibration_handeye.cpp
View File

@ -289,13 +289,12 @@ 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);
@ -303,18 +302,42 @@ static void calibrateHandEyeTsai(const std::vector<Mat>& Hg, const std::vector<M
//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)];
//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<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, 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<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));

106
modules/calib3d/test/test_calibration_hand_eye.cpp
View File

@ -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