t_cam2gripper = t;
}
-// sign function, return -1 if negative values, +1 otherwise
-static int sign_double(double val)
+static Mat_<double> normalizeRotation(const Mat_<double>& R_)
{
- return (0 < val) - (val < 0);
+ // Make R unit determinant
+ Mat_<double> R = R_.clone();
+ double det = determinant(R);
+ if (std::fabs(det) < FLT_EPSILON)
+ {
+ CV_Error(Error::StsNoConv, "Rotation normalization issue: determinant(R) is null");
+ }
+ R = std::cbrt(std::copysign(1, det) / std::fabs(det)) * R;
+
+ // Make R orthogonal
+ Mat w, u, vt;
+ SVDecomp(R, w, u, vt);
+ R = u*vt;
+
+ // Handle reflection case
+ if (determinant(R) < 0)
+ {
+ Matx33d diag(1.0, 0.0, 0.0,
+ 0.0, 1.0, 0.0,
+ 0.0, 0.0, -1.0);
+ R = u*diag*vt;
+ }
+
+ return R;
}
//Reference:
int newSize[] = {3, 3};
R = R.reshape(1, 2, newSize);
//Eq 15
- double det = determinant(R);
- if (std::fabs(det) < FLT_EPSILON)
- {
- CV_Error(Error::StsNoConv, "calibrateHandEye() with CALIB_HAND_EYE_ANDREFF method: determinant(R) is null");
- }
- R = cubeRoot(static_cast<float>(sign_double(det) / abs(det))) * R;
-
- Mat w, u, vt;
- SVDecomp(R, w, u, vt);
- R = u*vt;
-
- if (determinant(R) < 0)
- {
- Mat diag = (Mat_<double>(3,3) << 1.0, 0.0, 0.0,
- 0.0, 1.0, 0.0,
- 0.0, 0.0, -1.0);
- R = u*diag*vt;
- }
-
- R_cam2gripper = R;
-
- Mat t = X(Rect(0, 9, 1, 3));
- t_cam2gripper = t;
+ R_cam2gripper = normalizeRotation(R);
+ t_cam2gripper = X(Rect(0, 9, 1, 3));
}
//Reference:
CV_Assert(R_gripper2base_.size() == t_gripper2base_.size() &&
R_target2cam_.size() == t_target2cam_.size() &&
R_gripper2base_.size() == R_target2cam_.size());
- CV_Assert(R_gripper2base_.size() >= 3);
+ CV_Check(R_gripper2base_.size(), R_gripper2base_.size() >= 3, "At least 3 measurements are needed");
//Notation used in Tsai paper
//Defines coordinate transformation from G (gripper) to RW (robot base)
Rcg.copyTo(R_cam2gripper);
Tcg.copyTo(t_cam2gripper);
}
+
+//Reference:
+//M. Shah, "Solving the robot-world/hand-eye calibration problem using the kronecker product"
+//Journal of Mechanisms and Robotics, vol. 5, p. 031007, 2013.
+//Matlab code: http://math.loyola.edu/~mili/Calibration/
+static void calibrateRobotWorldHandEyeShah(const std::vector<Mat_<double>>& cRw, const std::vector<Mat_<double>>& ctw,
+ const std::vector<Mat_<double>>& gRb, const std::vector<Mat_<double>>& gtb,
+ Matx33d& wRb, Matx31d& wtb, Matx33d& cRg, Matx31d& ctg)
+{
+ Mat_<double> T = Mat_<double>::zeros(9, 9);
+ for (size_t i = 0; i < cRw.size(); i++)
+ {
+ T += kron(gRb[i], cRw[i]);
+ }
+
+ Mat_<double> w, u, vt;
+ SVDecomp(T, w, u, vt);
+
+ Mat_<double> RX(3,3), RZ(3,3);
+ for (int i = 0; i < 3; i++)
+ {
+ for (int j = 0; j < 3; j++)
+ {
+ RX(j,i) = vt(0, i*3+j);
+ RZ(j,i) = u(i*3+j, 0);
+ }
+ }
+
+ wRb = normalizeRotation(RX);
+ cRg = normalizeRotation(RZ);
+ Mat_<double> Z = Mat(cRg.t()).reshape(1, 9);
+
+ const int n = static_cast<int>(cRw.size());
+ Mat_<double> A = Mat_<double>::zeros(3*n, 6);
+ Mat_<double> b = Mat_<double>::zeros(3*n, 1);
+ Mat_<double> I3 = Mat_<double>::eye(3,3);
+
+ for (int i = 0; i < n; i++)
+ {
+ Mat cRw_ = -cRw[i];
+ cRw_.copyTo(A(Range(i*3, (i+1)*3), Range(0,3)));
+ I3.copyTo(A(Range(i*3, (i+1)*3), Range(3,6)));
+
+ Mat ctw_ = ctw[i] - kron(gtb[i].t(), I3) * Z;
+ ctw_.copyTo(b(Range(i*3, (i+1)*3), Range::all()));
+ }
+
+ Mat_<double> t;
+ solve(A, b, t, DECOMP_SVD);
+
+ for (int i = 0; i < 3; i++)
+ {
+ wtb(i) = t(i);
+ ctg(i) = t(i+3);
+ }
+}
+
+//Reference:
+//A. Li, L. Wang, and D. Wu, "Simultaneous robot-world and hand-eye calibration using dual-quaternions and kronecker product"
+//International Journal of Physical Sciences, vol. 5, pp. 1530–1536, 2010.
+//Matlab code: http://math.loyola.edu/~mili/Calibration/
+static void calibrateRobotWorldHandEyeLi(const std::vector<Mat_<double>>& cRw, const std::vector<Mat_<double>>& ctw,
+ const std::vector<Mat_<double>>& gRb, const std::vector<Mat_<double>>& gtb,
+ Matx33d& wRb, Matx31d& wtb, Matx33d& cRg, Matx31d& ctg)
+{
+ const int n = static_cast<int>(cRw.size());
+ Mat_<double> A = Mat_<double>::zeros(12*n, 24);
+ Mat_<double> b = Mat_<double>::zeros(12*n, 1);
+ Mat_<double> I3 = Mat_<double>::eye(3,3);
+
+ for (int i = 0; i < n; i++)
+ {
+ //Eq 19
+ kron(cRw[i], I3).copyTo(A(Range(i*12, i*12 + 9), Range(0, 9)));
+ kron(-I3, gRb[i].t()).copyTo(A(Range(i*12, i*12 + 9), Range(9, 18)));
+
+ kron(I3, gtb[i].t()).copyTo(A(Range(i*12 + 9, (i+1)*12), Range(9, 18)));
+ Mat cRw_ = -cRw[i];
+ cRw_.copyTo(A(Range(i*12 + 9, (i+1)*12), Range(18, 21)));
+ I3.copyTo(A(Range(i*12 + 9, (i+1)*12), Range(21, 24)));
+
+ ctw[i].copyTo(b(Range(i*12 + 9, i*12+12), Range::all()));
+ }
+
+ Mat_<double> x;
+ solve(A, b, x, DECOMP_SVD);
+
+ Mat_<double> RX = x(Range(0,9), Range::all()).reshape(3, 3);
+ wRb = normalizeRotation(RX);
+ x(Range(18,21), Range::all()).copyTo(wtb);
+
+ Mat_<double> RZ = x(Range(9,18), Range::all()).reshape(3, 3);
+ cRg = normalizeRotation(RZ);
+ x(Range(21,24), Range::all()).copyTo(ctg);
+}
+
+void calibrateRobotWorldHandEye(InputArrayOfArrays R_world2cam, InputArrayOfArrays t_world2cam,
+ InputArrayOfArrays R_base2gripper, InputArrayOfArrays t_base2gripper,
+ OutputArray R_base2world, OutputArray t_base2world,
+ OutputArray R_gripper2cam, OutputArray t_gripper2cam,
+ RobotWorldHandEyeCalibrationMethod method)
+{
+ CV_Assert(R_base2gripper.isMatVector() && t_base2gripper.isMatVector() &&
+ R_world2cam.isMatVector() && t_world2cam.isMatVector());
+
+ std::vector<Mat> R_base2gripper_tmp, t_base2gripper_tmp;
+ R_base2gripper.getMatVector(R_base2gripper_tmp);
+ t_base2gripper.getMatVector(t_base2gripper_tmp);
+
+ std::vector<Mat> R_world2cam_tmp, t_world2cam_tmp;
+ R_world2cam.getMatVector(R_world2cam_tmp);
+ t_world2cam.getMatVector(t_world2cam_tmp);
+
+ CV_Assert(R_base2gripper_tmp.size() == t_base2gripper_tmp.size() &&
+ R_world2cam_tmp.size() == t_world2cam_tmp.size() &&
+ R_base2gripper_tmp.size() == R_world2cam_tmp.size());
+ CV_Check(R_base2gripper_tmp.size(), R_base2gripper_tmp.size() >= 3, "At least 3 measurements are needed");
+
+ // Convert to double
+ std::vector<Mat_<double>> R_base2gripper_, t_base2gripper_;
+ std::vector<Mat_<double>> R_world2cam_, t_world2cam_;
+
+ R_base2gripper_.reserve(R_base2gripper_tmp.size());
+ t_base2gripper_.reserve(R_base2gripper_tmp.size());
+ R_world2cam_.reserve(R_world2cam_tmp.size());
+ t_world2cam_.reserve(R_base2gripper_tmp.size());
+
+ // Convert to rotation matrix if needed
+ for (size_t i = 0; i < R_base2gripper_tmp.size(); i++)
+ {
+ {
+ Mat rot = R_base2gripper_tmp[i];
+ Mat R(3, 3, CV_64FC1);
+ if (rot.size() == Size(3,3))
+ {
+ rot.convertTo(R, CV_64F);
+ R_base2gripper_.push_back(R);
+ }
+ else
+ {
+ Rodrigues(rot, R);
+ R_base2gripper_.push_back(R);
+ }
+ Mat tvec = t_base2gripper_tmp[i];
+ Mat t;
+ tvec.convertTo(t, CV_64F);
+ t_base2gripper_.push_back(t);
+ }
+ {
+ Mat rot = R_world2cam_tmp[i];
+ Mat R(3, 3, CV_64FC1);
+ if (rot.size() == Size(3,3))
+ {
+ rot.convertTo(R, CV_64F);
+ R_world2cam_.push_back(R);
+ }
+ else
+ {
+ Rodrigues(rot, R);
+ R_world2cam_.push_back(R);
+ }
+ Mat tvec = t_world2cam_tmp[i];
+ Mat t;
+ tvec.convertTo(t, CV_64F);
+ t_world2cam_.push_back(t);
+ }
+ }
+
+ CV_Assert(R_world2cam_.size() == t_world2cam_.size() &&
+ R_base2gripper_.size() == t_base2gripper_.size() &&
+ R_world2cam_.size() == R_base2gripper_.size());
+
+ Matx33d wRb, cRg;
+ Matx31d wtb, ctg;
+ switch (method)
+ {
+ case CALIB_ROBOT_WORLD_HAND_EYE_SHAH:
+ calibrateRobotWorldHandEyeShah(R_world2cam_, t_world2cam_, R_base2gripper_, t_base2gripper_, wRb, wtb, cRg, ctg);
+ break;
+
+ case CALIB_ROBOT_WORLD_HAND_EYE_LI:
+ calibrateRobotWorldHandEyeLi(R_world2cam_, t_world2cam_, R_base2gripper_, t_base2gripper_, wRb, wtb, cRg, ctg);
+ break;
+ }
+
+ Mat(wRb).copyTo(R_base2world);
+ Mat(wtb).copyTo(t_base2world);
+
+ Mat(cRg).copyTo(R_gripper2cam);
+ Mat(ctg).copyTo(t_gripper2cam);
+}
}
namespace opencv_test { namespace {
-static std::string getMethodName(HandEyeCalibrationMethod method)
-{
- std::string method_name = "";
- switch (method)
- {
- case CALIB_HAND_EYE_TSAI:
- method_name = "Tsai";
- break;
-
- case CALIB_HAND_EYE_PARK:
- method_name = "Park";
- break;
-
- case CALIB_HAND_EYE_HORAUD:
- method_name = "Horaud";
- break;
-
- case CALIB_HAND_EYE_ANDREFF:
- method_name = "Andreff";
- break;
-
- case CALIB_HAND_EYE_DANIILIDIS:
- method_name = "Daniilidis";
- break;
-
- default:
- break;
- }
-
- return method_name;
-}
-
-class CV_CalibrateHandEyeTest : public cvtest::BaseTest
-{
-public:
- CV_CalibrateHandEyeTest() {
- eps_rvec[CALIB_HAND_EYE_TSAI] = 1.0e-8;
- eps_rvec[CALIB_HAND_EYE_PARK] = 1.0e-8;
- eps_rvec[CALIB_HAND_EYE_HORAUD] = 1.0e-8;
- eps_rvec[CALIB_HAND_EYE_ANDREFF] = 1.0e-8;
- eps_rvec[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-8;
-
- eps_tvec[CALIB_HAND_EYE_TSAI] = 1.0e-8;
- eps_tvec[CALIB_HAND_EYE_PARK] = 1.0e-8;
- eps_tvec[CALIB_HAND_EYE_HORAUD] = 1.0e-8;
- eps_tvec[CALIB_HAND_EYE_ANDREFF] = 1.0e-8;
- eps_tvec[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-8;
-
- eps_rvec_noise[CALIB_HAND_EYE_TSAI] = 2.0e-2;
- eps_rvec_noise[CALIB_HAND_EYE_PARK] = 2.0e-2;
- eps_rvec_noise[CALIB_HAND_EYE_HORAUD] = 2.0e-2;
- eps_rvec_noise[CALIB_HAND_EYE_ANDREFF] = 1.0e-2;
- eps_rvec_noise[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-2;
-
- eps_tvec_noise[CALIB_HAND_EYE_TSAI] = 5.0e-2;
- eps_tvec_noise[CALIB_HAND_EYE_PARK] = 5.0e-2;
- eps_tvec_noise[CALIB_HAND_EYE_HORAUD] = 5.0e-2;
- eps_tvec_noise[CALIB_HAND_EYE_ANDREFF] = 5.0e-2;
- eps_tvec_noise[CALIB_HAND_EYE_DANIILIDIS] = 5.0e-2;
- }
-protected:
- virtual void run(int);
- void generatePose(RNG& rng, double min_theta, double max_theta,
- double min_tx, double max_tx,
- double min_ty, double max_ty,
- double min_tz, double max_tz,
- Mat& R, Mat& tvec,
- bool randSign=false);
- void simulateData(RNG& rng, int nPoses,
- std::vector<Mat> &R_gripper2base, std::vector<Mat> &t_gripper2base,
- std::vector<Mat> &R_target2cam, std::vector<Mat> &t_target2cam,
- bool noise, Mat& R_cam2gripper, Mat& t_cam2gripper);
- Mat homogeneousInverse(const Mat& T);
- double sign_double(double val);
-
- double eps_rvec[5];
- double eps_tvec[5];
- double eps_rvec_noise[5];
- double eps_tvec_noise[5];
-};
-
-void CV_CalibrateHandEyeTest::run(int)
-{
- ts->set_failed_test_info(cvtest::TS::OK);
-
- RNG& rng = ts->get_rng();
-
- std::vector<std::vector<double> > vec_rvec_diff(5);
- std::vector<std::vector<double> > vec_tvec_diff(5);
- std::vector<std::vector<double> > vec_rvec_diff_noise(5);
- std::vector<std::vector<double> > vec_tvec_diff_noise(5);
-
- std::vector<HandEyeCalibrationMethod> methods;
- methods.push_back(CALIB_HAND_EYE_TSAI);
- methods.push_back(CALIB_HAND_EYE_PARK);
- methods.push_back(CALIB_HAND_EYE_HORAUD);
- methods.push_back(CALIB_HAND_EYE_ANDREFF);
- methods.push_back(CALIB_HAND_EYE_DANIILIDIS);
-
- const int nTests = 100;
- for (int i = 0; i < nTests; i++)
- {
- const int nPoses = 10;
- {
- //No noise
- std::vector<Mat> R_gripper2base, t_gripper2base;
- std::vector<Mat> R_target2cam, t_target2cam;
- Mat R_cam2gripper_true, t_cam2gripper_true;
-
- const bool noise = false;
- simulateData(rng, nPoses, R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, noise, R_cam2gripper_true, t_cam2gripper_true);
-
- for (size_t idx = 0; idx < methods.size(); idx++)
- {
- Mat rvec_cam2gripper_true;
- cv::Rodrigues(R_cam2gripper_true, rvec_cam2gripper_true);
-
- Mat R_cam2gripper_est, t_cam2gripper_est;
- calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, methods[idx]);
-
- Mat rvec_cam2gripper_est;
- cv::Rodrigues(R_cam2gripper_est, rvec_cam2gripper_est);
-
- double rvecDiff = cvtest::norm(rvec_cam2gripper_true, rvec_cam2gripper_est, NORM_L2);
- double tvecDiff = cvtest::norm(t_cam2gripper_true, t_cam2gripper_est, NORM_L2);
-
- vec_rvec_diff[idx].push_back(rvecDiff);
- vec_tvec_diff[idx].push_back(tvecDiff);
-
- const double epsilon_rvec = eps_rvec[idx];
- const double epsilon_tvec = eps_tvec[idx];
-
- //Maybe a better accuracy test would be to compare the mean and std errors with some thresholds?
- if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec)
- {
- ts->printf(cvtest::TS::LOG, "Invalid accuracy (no noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n",
- getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec);
- ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
- }
- }
- }
-
- {
- //Gaussian noise on transformations between calibration target frame and camera frame and between gripper and robot base frames
- std::vector<Mat> R_gripper2base, t_gripper2base;
- std::vector<Mat> R_target2cam, t_target2cam;
- Mat R_cam2gripper_true, t_cam2gripper_true;
-
- const bool noise = true;
- simulateData(rng, nPoses, R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, noise, R_cam2gripper_true, t_cam2gripper_true);
-
- for (size_t idx = 0; idx < methods.size(); idx++)
- {
- Mat rvec_cam2gripper_true;
- cv::Rodrigues(R_cam2gripper_true, rvec_cam2gripper_true);
-
- Mat R_cam2gripper_est, t_cam2gripper_est;
- calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, methods[idx]);
-
- Mat rvec_cam2gripper_est;
- cv::Rodrigues(R_cam2gripper_est, rvec_cam2gripper_est);
-
- double rvecDiff = cvtest::norm(rvec_cam2gripper_true, rvec_cam2gripper_est, NORM_L2);
- double tvecDiff = cvtest::norm(t_cam2gripper_true, t_cam2gripper_est, NORM_L2);
-
- vec_rvec_diff_noise[idx].push_back(rvecDiff);
- vec_tvec_diff_noise[idx].push_back(tvecDiff);
-
- const double epsilon_rvec = eps_rvec_noise[idx];
- const double epsilon_tvec = eps_tvec_noise[idx];
-
- //Maybe a better accuracy test would be to compare the mean and std errors with some thresholds?
- if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec)
- {
- ts->printf(cvtest::TS::LOG, "Invalid accuracy (noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n",
- getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec);
- ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
- }
- }
- }
- }
-
- for (size_t idx = 0; idx < methods.size(); idx++)
- {
- {
- double max_rvec_diff = *std::max_element(vec_rvec_diff[idx].begin(), vec_rvec_diff[idx].end());
- double mean_rvec_diff = std::accumulate(vec_rvec_diff[idx].begin(),
- vec_rvec_diff[idx].end(), 0.0) / vec_rvec_diff[idx].size();
- double sq_sum_rvec_diff = std::inner_product(vec_rvec_diff[idx].begin(), vec_rvec_diff[idx].end(),
- vec_rvec_diff[idx].begin(), 0.0);
- double std_rvec_diff = std::sqrt(sq_sum_rvec_diff / vec_rvec_diff[idx].size() - mean_rvec_diff * mean_rvec_diff);
-
- double max_tvec_diff = *std::max_element(vec_tvec_diff[idx].begin(), vec_tvec_diff[idx].end());
- double mean_tvec_diff = std::accumulate(vec_tvec_diff[idx].begin(),
- vec_tvec_diff[idx].end(), 0.0) / vec_tvec_diff[idx].size();
- double sq_sum_tvec_diff = std::inner_product(vec_tvec_diff[idx].begin(), vec_tvec_diff[idx].end(),
- vec_tvec_diff[idx].begin(), 0.0);
- double std_tvec_diff = std::sqrt(sq_sum_tvec_diff / vec_tvec_diff[idx].size() - mean_tvec_diff * mean_tvec_diff);
-
- std::cout << "\nMethod " << getMethodName(methods[idx]) << ":\n"
- << "Max rvec error: " << max_rvec_diff << ", Mean rvec error: " << mean_rvec_diff
- << ", Std rvec error: " << std_rvec_diff << "\n"
- << "Max tvec error: " << max_tvec_diff << ", Mean tvec error: " << mean_tvec_diff
- << ", Std tvec error: " << std_tvec_diff << std::endl;
- }
- {
- double max_rvec_diff = *std::max_element(vec_rvec_diff_noise[idx].begin(), vec_rvec_diff_noise[idx].end());
- double mean_rvec_diff = std::accumulate(vec_rvec_diff_noise[idx].begin(),
- vec_rvec_diff_noise[idx].end(), 0.0) / vec_rvec_diff_noise[idx].size();
- double sq_sum_rvec_diff = std::inner_product(vec_rvec_diff_noise[idx].begin(), vec_rvec_diff_noise[idx].end(),
- vec_rvec_diff_noise[idx].begin(), 0.0);
- double std_rvec_diff = std::sqrt(sq_sum_rvec_diff / vec_rvec_diff_noise[idx].size() - mean_rvec_diff * mean_rvec_diff);
-
- double max_tvec_diff = *std::max_element(vec_tvec_diff_noise[idx].begin(), vec_tvec_diff_noise[idx].end());
- double mean_tvec_diff = std::accumulate(vec_tvec_diff_noise[idx].begin(),
- vec_tvec_diff_noise[idx].end(), 0.0) / vec_tvec_diff_noise[idx].size();
- double sq_sum_tvec_diff = std::inner_product(vec_tvec_diff_noise[idx].begin(), vec_tvec_diff_noise[idx].end(),
- vec_tvec_diff_noise[idx].begin(), 0.0);
- double std_tvec_diff = std::sqrt(sq_sum_tvec_diff / vec_tvec_diff_noise[idx].size() - mean_tvec_diff * mean_tvec_diff);
-
- std::cout << "Method (noise) " << getMethodName(methods[idx]) << ":\n"
- << "Max rvec error: " << max_rvec_diff << ", Mean rvec error: " << mean_rvec_diff
- << ", Std rvec error: " << std_rvec_diff << "\n"
- << "Max tvec error: " << max_tvec_diff << ", Mean tvec error: " << mean_tvec_diff
- << ", Std tvec error: " << std_tvec_diff << std::endl;
- }
- }
-}
-
-void CV_CalibrateHandEyeTest::generatePose(RNG& rng, double min_theta, double max_theta,
- double min_tx, double max_tx,
- double min_ty, double max_ty,
- double min_tz, double max_tz,
- Mat& R, Mat& tvec,
- bool random_sign)
+static void generatePose(RNG& rng, double min_theta, double max_theta,
+ double min_tx, double max_tx,
+ double min_ty, double max_ty,
+ double min_tz, double max_tz,
+ Mat& R, Mat& tvec,
+ bool random_sign)
{
Mat axis(3, 1, CV_64FC1);
for (int i = 0; i < 3; i++)
double theta = rng.uniform(min_theta, max_theta);
if (random_sign)
{
- theta *= sign_double(rng.uniform(-1.0, 1.0));
+ theta *= std::copysign(1.0, rng.uniform(-1.0, 1.0));
}
Mat rvec(3, 1, CV_64FC1);
if (random_sign)
{
- tvec.at<double>(0,0) *= sign_double(rng.uniform(-1.0, 1.0));
- tvec.at<double>(1,0) *= sign_double(rng.uniform(-1.0, 1.0));
- tvec.at<double>(2,0) *= sign_double(rng.uniform(-1.0, 1.0));
+ tvec.at<double>(0,0) *= std::copysign(1.0, rng.uniform(-1.0, 1.0));
+ tvec.at<double>(1,0) *= std::copysign(1.0, rng.uniform(-1.0, 1.0));
+ tvec.at<double>(2,0) *= std::copysign(1.0, rng.uniform(-1.0, 1.0));
}
cv::Rodrigues(rvec, R);
}
-void CV_CalibrateHandEyeTest::simulateData(RNG& rng, int nPoses,
- std::vector<Mat> &R_gripper2base, std::vector<Mat> &t_gripper2base,
- std::vector<Mat> &R_target2cam, std::vector<Mat> &t_target2cam,
- bool noise, Mat& R_cam2gripper, Mat& t_cam2gripper)
+static Mat homogeneousInverse(const Mat& T)
+{
+ CV_Assert( T.rows == 4 && T.cols == 4 );
+
+ Mat R = T(Rect(0, 0, 3, 3));
+ Mat t = T(Rect(3, 0, 1, 3));
+ Mat Rt = R.t();
+ Mat tinv = -Rt * t;
+ Mat Tinv = Mat::eye(4, 4, T.type());
+ Rt.copyTo(Tinv(Rect(0, 0, 3, 3)));
+ tinv.copyTo(Tinv(Rect(3, 0, 1, 3)));
+
+ return Tinv;
+}
+
+static void simulateDataEyeInHand(RNG& rng, int nPoses,
+ std::vector<Mat> &R_gripper2base, std::vector<Mat> &t_gripper2base,
+ std::vector<Mat> &R_target2cam, std::vector<Mat> &t_target2cam,
+ bool noise, Mat& R_cam2gripper, Mat& t_cam2gripper)
{
//to avoid generating values close to zero,
//we use positive range values and randomize the sign
t_gripper2base_noise.at<double>(2,0) += rng.gaussian(0.001);
}
- // test rvec represenation
+ //Test rvec representation
Mat rvec_target2cam;
cv::Rodrigues(T_target2cam(Rect(0, 0, 3, 3)), rvec_target2cam);
R_target2cam.push_back(rvec_target2cam);
}
}
-Mat CV_CalibrateHandEyeTest::homogeneousInverse(const Mat& T)
+static void simulateDataEyeToHand(RNG& rng, int nPoses,
+ std::vector<Mat> &R_base2gripper, std::vector<Mat> &t_base2gripper,
+ std::vector<Mat> &R_target2cam, std::vector<Mat> &t_target2cam,
+ bool noise, Mat& R_cam2base, Mat& t_cam2base)
{
- CV_Assert( T.rows == 4 && T.cols == 4 );
+ //to avoid generating values close to zero,
+ //we use positive range values and randomize the sign
+ const bool random_sign = true;
+ generatePose(rng, 10.0*CV_PI/180.0, 50.0*CV_PI/180.0,
+ 0.5, 3.5, 0.5, 3.5, 0.5, 3.5,
+ R_cam2base, t_cam2base, random_sign);
- Mat R = T(Rect(0, 0, 3, 3));
- Mat t = T(Rect(3, 0, 1, 3));
- Mat Rt = R.t();
- Mat tinv = -Rt * t;
- Mat Tinv = Mat::eye(4, 4, T.type());
- Rt.copyTo(Tinv(Rect(0, 0, 3, 3)));
- tinv.copyTo(Tinv(Rect(3, 0, 1, 3)));
+ Mat R_target2gripper, t_target2gripper;
+ generatePose(rng, 5.0*CV_PI/180.0, 85.0*CV_PI/180.0,
+ 0.05, 0.5, 0.05, 0.5, 0.05, 0.5,
+ R_target2gripper, t_target2gripper, random_sign);
- return Tinv;
+ Mat T_target2gripper = Mat::eye(4, 4, CV_64FC1);
+ R_target2gripper.copyTo(T_target2gripper(Rect(0, 0, 3, 3)));
+ t_target2gripper.copyTo(T_target2gripper(Rect(3, 0, 1, 3)));
+
+ for (int i = 0; i < nPoses; i++)
+ {
+ Mat R_gripper2base_, t_gripper2base_;
+ generatePose(rng, 5.0*CV_PI/180.0, 45.0*CV_PI/180.0,
+ 0.5, 1.5, 0.5, 1.5, 0.5, 1.5,
+ R_gripper2base_, t_gripper2base_, random_sign);
+
+ Mat R_base2gripper_ = R_gripper2base_.t();
+ Mat t_base2gripper_ = -R_base2gripper_ * t_gripper2base_;
+
+ Mat T_gripper2base = Mat::eye(4, 4, CV_64FC1);
+ R_gripper2base_.copyTo(T_gripper2base(Rect(0, 0, 3, 3)));
+ t_gripper2base_.copyTo(T_gripper2base(Rect(3, 0, 1, 3)));
+
+ Mat T_cam2base = Mat::eye(4, 4, CV_64FC1);
+ R_cam2base.copyTo(T_cam2base(Rect(0, 0, 3, 3)));
+ t_cam2base.copyTo(T_cam2base(Rect(3, 0, 1, 3)));
+
+ Mat T_target2cam = homogeneousInverse(T_cam2base) * T_gripper2base * T_target2gripper;
+
+ if (noise)
+ {
+ //Add some noise for the transformation between the target and the camera
+ Mat R_target2cam_noise = T_target2cam(Rect(0, 0, 3, 3));
+ Mat rvec_target2cam_noise;
+ cv::Rodrigues(R_target2cam_noise, rvec_target2cam_noise);
+ rvec_target2cam_noise.at<double>(0,0) += rng.gaussian(0.002);
+ rvec_target2cam_noise.at<double>(1,0) += rng.gaussian(0.002);
+ rvec_target2cam_noise.at<double>(2,0) += rng.gaussian(0.002);
+
+ cv::Rodrigues(rvec_target2cam_noise, R_target2cam_noise);
+
+ Mat t_target2cam_noise = T_target2cam(Rect(3, 0, 1, 3));
+ t_target2cam_noise.at<double>(0,0) += rng.gaussian(0.005);
+ t_target2cam_noise.at<double>(1,0) += rng.gaussian(0.005);
+ t_target2cam_noise.at<double>(2,0) += rng.gaussian(0.005);
+
+ //Add some noise for the transformation between the robot base and the gripper
+ Mat rvec_base2gripper_noise;
+ cv::Rodrigues(R_base2gripper_, rvec_base2gripper_noise);
+ rvec_base2gripper_noise.at<double>(0,0) += rng.gaussian(0.001);
+ rvec_base2gripper_noise.at<double>(1,0) += rng.gaussian(0.001);
+ rvec_base2gripper_noise.at<double>(2,0) += rng.gaussian(0.001);
+
+ cv::Rodrigues(rvec_base2gripper_noise, R_base2gripper_);
+
+ t_base2gripper_.at<double>(0,0) += rng.gaussian(0.001);
+ t_base2gripper_.at<double>(1,0) += rng.gaussian(0.001);
+ t_base2gripper_.at<double>(2,0) += rng.gaussian(0.001);
+ }
+
+ R_base2gripper.push_back(R_base2gripper_);
+ t_base2gripper.push_back(t_base2gripper_);
+
+ //Test rvec representation
+ Mat rvec_target2cam;
+ cv::Rodrigues(T_target2cam(Rect(0, 0, 3, 3)), rvec_target2cam);
+ R_target2cam.push_back(rvec_target2cam);
+ t_target2cam.push_back(T_target2cam(Rect(3, 0, 1, 3)));
+ }
}
-double CV_CalibrateHandEyeTest::sign_double(double val)
+static std::string getMethodName(HandEyeCalibrationMethod method)
{
- return (0 < val) - (val < 0);
+ std::string method_name = "";
+ switch (method)
+ {
+ case CALIB_HAND_EYE_TSAI:
+ method_name = "Tsai";
+ break;
+
+ case CALIB_HAND_EYE_PARK:
+ method_name = "Park";
+ break;
+
+ case CALIB_HAND_EYE_HORAUD:
+ method_name = "Horaud";
+ break;
+
+ case CALIB_HAND_EYE_ANDREFF:
+ method_name = "Andreff";
+ break;
+
+ case CALIB_HAND_EYE_DANIILIDIS:
+ method_name = "Daniilidis";
+ break;
+
+ default:
+ break;
+ }
+
+ return method_name;
}
-///////////////////////////////////////////////////////////////////////////////////////////////////
+static std::string getMethodName(RobotWorldHandEyeCalibrationMethod method)
+{
+ std::string method_name = "";
+ switch (method)
+ {
+ case CALIB_ROBOT_WORLD_HAND_EYE_SHAH:
+ method_name = "Shah";
+ break;
-TEST(Calib3d_CalibrateHandEye, regression) { CV_CalibrateHandEyeTest test; test.safe_run(); }
+ case CALIB_ROBOT_WORLD_HAND_EYE_LI:
+ method_name = "Li";
+ break;
-TEST(Calib3d_CalibrateHandEye, regression_17986)
+ default:
+ break;
+ }
+
+ return method_name;
+}
+
+static void printStats(const std::string& methodName, const std::vector<double>& rvec_diff, const std::vector<double>& tvec_diff)
+{
+ double max_rvec_diff = *std::max_element(rvec_diff.begin(), rvec_diff.end());
+ double mean_rvec_diff = std::accumulate(rvec_diff.begin(),
+ rvec_diff.end(), 0.0) / rvec_diff.size();
+ double sq_sum_rvec_diff = std::inner_product(rvec_diff.begin(), rvec_diff.end(),
+ rvec_diff.begin(), 0.0);
+ double std_rvec_diff = std::sqrt(sq_sum_rvec_diff / rvec_diff.size() - mean_rvec_diff * mean_rvec_diff);
+
+ double max_tvec_diff = *std::max_element(tvec_diff.begin(), tvec_diff.end());
+ double mean_tvec_diff = std::accumulate(tvec_diff.begin(),
+ tvec_diff.end(), 0.0) / tvec_diff.size();
+ double sq_sum_tvec_diff = std::inner_product(tvec_diff.begin(), tvec_diff.end(),
+ tvec_diff.begin(), 0.0);
+ double std_tvec_diff = std::sqrt(sq_sum_tvec_diff / tvec_diff.size() - mean_tvec_diff * mean_tvec_diff);
+
+ std::cout << "Method " << methodName << ":\n"
+ << "Max rvec error: " << max_rvec_diff << ", Mean rvec error: " << mean_rvec_diff
+ << ", Std rvec error: " << std_rvec_diff << "\n"
+ << "Max tvec error: " << max_tvec_diff << ", Mean tvec error: " << mean_tvec_diff
+ << ", Std tvec error: " << std_tvec_diff << std::endl;
+}
+
+static void loadDataset(std::vector<Mat>& R_target2cam, std::vector<Mat>& t_target2cam,
+ std::vector<Mat>& R_base2gripper, std::vector<Mat>& t_base2gripper)
{
- const std::string camera_poses_filename = findDataFile("cv/hand_eye_calibration/cali.txt");
- const std::string end_effector_poses = findDataFile("cv/hand_eye_calibration/robot_cali.txt");
+ const std::string camera_poses_filename = findDataFile("cv/robot_world_hand_eye_calibration/cali.txt");
+ const std::string end_effector_poses = findDataFile("cv/robot_world_hand_eye_calibration/robot_cali.txt");
- std::vector<Mat> R_target2cam;
- std::vector<Mat> t_target2cam;
- // Parse camera poses
+ // Parse camera poses, the pose of the chessboard in the camera frame
{
- std::ifstream file(camera_poses_filename.c_str());
+ std::ifstream file(camera_poses_filename);
ASSERT_TRUE(file.is_open());
int ndata = 0;
}
}
- std::vector<Mat> R_gripper2base;
- std::vector<Mat> t_gripper2base;
- // Parse end-effector poses
+ // Parse robot poses, the pose of the robot base in the robot hand frame
{
- std::ifstream file(end_effector_poses.c_str());
+ std::ifstream file(end_effector_poses);
ASSERT_TRUE(file.is_open());
int ndata = 0;
file >> ndata;
- R_gripper2base.reserve(ndata);
- t_gripper2base.reserve(ndata);
+ R_base2gripper.reserve(ndata);
+ t_base2gripper.reserve(ndata);
Matx33d R;
Matx31d t;
R(1,0) >> R(1,1) >> R(1,2) >> t(1) >>
R(2,0) >> R(2,1) >> R(2,2) >> t(2) >>
last_row(0) >> last_row(1) >> last_row(2) >> last_row(3)) {
- R_gripper2base.push_back(Mat(R));
- t_gripper2base.push_back(Mat(t));
+ R_base2gripper.push_back(Mat(R));
+ t_base2gripper.push_back(Mat(t));
+ }
+ }
+}
+
+static void loadResults(Matx33d& wRb, Matx31d& wtb, Matx33d& cRg, Matx31d& ctg)
+{
+ const std::string transformations_filename = findDataFile("cv/robot_world_hand_eye_calibration/rwhe_AA_RPI/transformations.txt");
+ std::ifstream file(transformations_filename);
+ ASSERT_TRUE(file.is_open());
+
+ std::string str;
+ //Parse X
+ file >> str;
+ Matx44d wTb;
+ for (int i = 0; i < 4; i++)
+ {
+ for (int j = 0; j < 4; j++)
+ {
+ file >> wTb(i,j);
}
}
+ //Parse Z
+ file >> str;
+ int cam_num = 0;
+ //Parse camera number
+ file >> cam_num;
+ Matx44d cTg;
+ for (int i = 0; i < 4; i++)
+ {
+ for (int j = 0; j < 4; j++)
+ {
+ file >> cTg(i,j);
+ }
+ }
+
+ for (int i = 0; i < 3; i++)
+ {
+ for (int j = 0; j < 3; j++)
+ {
+ wRb(i,j) = wTb(i,j);
+ cRg(i,j) = cTg(i,j);
+ }
+ wtb(i) = wTb(i,3);
+ ctg(i) = cTg(i,3);
+ }
+}
+
+class CV_CalibrateHandEyeTest : public cvtest::BaseTest
+{
+public:
+ CV_CalibrateHandEyeTest(bool eyeToHand) : eyeToHandConfig(eyeToHand) {
+ eps_rvec[CALIB_HAND_EYE_TSAI] = 1.0e-8;
+ eps_rvec[CALIB_HAND_EYE_PARK] = 1.0e-8;
+ eps_rvec[CALIB_HAND_EYE_HORAUD] = 1.0e-8;
+ eps_rvec[CALIB_HAND_EYE_ANDREFF] = 1.0e-8;
+ eps_rvec[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-8;
+
+ eps_tvec[CALIB_HAND_EYE_TSAI] = 1.0e-8;
+ eps_tvec[CALIB_HAND_EYE_PARK] = 1.0e-8;
+ eps_tvec[CALIB_HAND_EYE_HORAUD] = 1.0e-8;
+ eps_tvec[CALIB_HAND_EYE_ANDREFF] = 1.0e-8;
+ eps_tvec[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-8;
+
+ eps_rvec_noise[CALIB_HAND_EYE_TSAI] = 2.0e-2;
+ eps_rvec_noise[CALIB_HAND_EYE_PARK] = 2.0e-2;
+ eps_rvec_noise[CALIB_HAND_EYE_HORAUD] = 2.0e-2;
+ eps_rvec_noise[CALIB_HAND_EYE_ANDREFF] = 1.0e-2;
+ eps_rvec_noise[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-2;
+
+ eps_tvec_noise[CALIB_HAND_EYE_TSAI] = 5.0e-2;
+ eps_tvec_noise[CALIB_HAND_EYE_PARK] = 5.0e-2;
+ eps_tvec_noise[CALIB_HAND_EYE_HORAUD] = 5.0e-2;
+ if (eyeToHandConfig)
+ {
+ eps_tvec_noise[CALIB_HAND_EYE_ANDREFF] = 7.0e-2;
+ }
+ else
+ {
+ eps_tvec_noise[CALIB_HAND_EYE_ANDREFF] = 5.0e-2;
+ }
+ eps_tvec_noise[CALIB_HAND_EYE_DANIILIDIS] = 5.0e-2;
+ }
+protected:
+ virtual void run(int);
+
+ bool eyeToHandConfig;
+ double eps_rvec[5];
+ double eps_tvec[5];
+ double eps_rvec_noise[5];
+ double eps_tvec_noise[5];
+};
+
+void CV_CalibrateHandEyeTest::run(int)
+{
+ ts->set_failed_test_info(cvtest::TS::OK);
+
+ RNG& rng = ts->get_rng();
+
+ std::vector<std::vector<double> > vec_rvec_diff(5);
+ std::vector<std::vector<double> > vec_tvec_diff(5);
+ std::vector<std::vector<double> > vec_rvec_diff_noise(5);
+ std::vector<std::vector<double> > vec_tvec_diff_noise(5);
+
std::vector<HandEyeCalibrationMethod> methods;
methods.push_back(CALIB_HAND_EYE_TSAI);
methods.push_back(CALIB_HAND_EYE_PARK);
methods.push_back(CALIB_HAND_EYE_ANDREFF);
methods.push_back(CALIB_HAND_EYE_DANIILIDIS);
- for (size_t idx = 0; idx < methods.size(); idx++) {
- SCOPED_TRACE(cv::format("method=%s", getMethodName(methods[idx]).c_str()));
+ const int nTests = 100;
+ for (int i = 0; i < nTests; i++)
+ {
+ const int nPoses = 10;
+ if (eyeToHandConfig)
+ {
+ {
+ //No noise
+ std::vector<Mat> R_base2gripper, t_base2gripper;
+ std::vector<Mat> R_target2cam, t_target2cam;
+ Mat R_cam2base_true, t_cam2base_true;
+
+ const bool noise = false;
+ simulateDataEyeToHand(rng, nPoses, R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, noise,
+ R_cam2base_true, t_cam2base_true);
- Matx33d R_cam2gripper_est;
- Matx31d t_cam2gripper_est;
- calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, methods[idx]);
+ for (size_t idx = 0; idx < methods.size(); idx++)
+ {
+ Mat rvec_cam2base_true;
+ cv::Rodrigues(R_cam2base_true, rvec_cam2base_true);
+
+ Mat R_cam2base_est, t_cam2base_est;
+ calibrateHandEye(R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, R_cam2base_est, t_cam2base_est, methods[idx]);
+
+ Mat rvec_cam2base_est;
+ cv::Rodrigues(R_cam2base_est, rvec_cam2base_est);
+
+ double rvecDiff = cvtest::norm(rvec_cam2base_true, rvec_cam2base_est, NORM_L2);
+ double tvecDiff = cvtest::norm(t_cam2base_true, t_cam2base_est, NORM_L2);
+
+ vec_rvec_diff[idx].push_back(rvecDiff);
+ vec_tvec_diff[idx].push_back(tvecDiff);
+
+ const double epsilon_rvec = eps_rvec[idx];
+ const double epsilon_tvec = eps_tvec[idx];
+
+ //Maybe a better accuracy test would be to compare the mean and std errors with some thresholds?
+ if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec)
+ {
+ ts->printf(cvtest::TS::LOG, "Invalid accuracy (no noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n",
+ getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec);
+ ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
+ }
+ }
+ }
+
+ {
+ //Gaussian noise on transformations between calibration target frame and camera frame and between robot base and gripper frames
+ std::vector<Mat> R_base2gripper, t_base2gripper;
+ std::vector<Mat> R_target2cam, t_target2cam;
+ Mat R_cam2base_true, t_cam2base_true;
+
+ const bool noise = true;
+ simulateDataEyeToHand(rng, nPoses, R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, noise,
+ R_cam2base_true, t_cam2base_true);
+
+ for (size_t idx = 0; idx < methods.size(); idx++)
+ {
+ Mat rvec_cam2base_true;
+ cv::Rodrigues(R_cam2base_true, rvec_cam2base_true);
+
+ Mat R_cam2base_est, t_cam2base_est;
+ calibrateHandEye(R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, R_cam2base_est, t_cam2base_est, methods[idx]);
+
+ Mat rvec_cam2base_est;
+ cv::Rodrigues(R_cam2base_est, rvec_cam2base_est);
+
+ double rvecDiff = cvtest::norm(rvec_cam2base_true, rvec_cam2base_est, NORM_L2);
+ double tvecDiff = cvtest::norm(t_cam2base_true, t_cam2base_est, NORM_L2);
+
+ vec_rvec_diff_noise[idx].push_back(rvecDiff);
+ vec_tvec_diff_noise[idx].push_back(tvecDiff);
+
+ const double epsilon_rvec = eps_rvec_noise[idx];
+ const double epsilon_tvec = eps_tvec_noise[idx];
+
+ //Maybe a better accuracy test would be to compare the mean and std errors with some thresholds?
+ if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec)
+ {
+ ts->printf(cvtest::TS::LOG, "Invalid accuracy (noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n",
+ getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec);
+ ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
+ }
+ }
+ }
+ }
+ else
+ {
+ {
+ //No noise
+ std::vector<Mat> R_gripper2base, t_gripper2base;
+ std::vector<Mat> R_target2cam, t_target2cam;
+ Mat R_cam2gripper_true, t_cam2gripper_true;
+
+ const bool noise = false;
+ simulateDataEyeInHand(rng, nPoses, R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, noise,
+ R_cam2gripper_true, t_cam2gripper_true);
+
+ for (size_t idx = 0; idx < methods.size(); idx++)
+ {
+ Mat rvec_cam2gripper_true;
+ cv::Rodrigues(R_cam2gripper_true, rvec_cam2gripper_true);
+
+ Mat R_cam2gripper_est, t_cam2gripper_est;
+ calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, methods[idx]);
+
+ Mat rvec_cam2gripper_est;
+ cv::Rodrigues(R_cam2gripper_est, rvec_cam2gripper_est);
+
+ double rvecDiff = cvtest::norm(rvec_cam2gripper_true, rvec_cam2gripper_est, NORM_L2);
+ double tvecDiff = cvtest::norm(t_cam2gripper_true, t_cam2gripper_est, NORM_L2);
+
+ vec_rvec_diff[idx].push_back(rvecDiff);
+ vec_tvec_diff[idx].push_back(tvecDiff);
+
+ const double epsilon_rvec = eps_rvec[idx];
+ const double epsilon_tvec = eps_tvec[idx];
+
+ //Maybe a better accuracy test would be to compare the mean and std errors with some thresholds?
+ if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec)
+ {
+ ts->printf(cvtest::TS::LOG, "Invalid accuracy (no noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n",
+ getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec);
+ ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
+ }
+ }
+ }
+
+ {
+ //Gaussian noise on transformations between calibration target frame and camera frame and between gripper and robot base frames
+ std::vector<Mat> R_gripper2base, t_gripper2base;
+ std::vector<Mat> R_target2cam, t_target2cam;
+ Mat R_cam2gripper_true, t_cam2gripper_true;
+
+ const bool noise = true;
+ simulateDataEyeInHand(rng, nPoses, R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, noise,
+ R_cam2gripper_true, t_cam2gripper_true);
+
+ for (size_t idx = 0; idx < methods.size(); idx++)
+ {
+ Mat rvec_cam2gripper_true;
+ cv::Rodrigues(R_cam2gripper_true, rvec_cam2gripper_true);
+
+ Mat R_cam2gripper_est, t_cam2gripper_est;
+ calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, methods[idx]);
+
+ Mat rvec_cam2gripper_est;
+ cv::Rodrigues(R_cam2gripper_est, rvec_cam2gripper_est);
+
+ double rvecDiff = cvtest::norm(rvec_cam2gripper_true, rvec_cam2gripper_est, NORM_L2);
+ double tvecDiff = cvtest::norm(t_cam2gripper_true, t_cam2gripper_est, NORM_L2);
+
+ vec_rvec_diff_noise[idx].push_back(rvecDiff);
+ vec_tvec_diff_noise[idx].push_back(tvecDiff);
+
+ const double epsilon_rvec = eps_rvec_noise[idx];
+ const double epsilon_tvec = eps_tvec_noise[idx];
+
+ //Maybe a better accuracy test would be to compare the mean and std errors with some thresholds?
+ if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec)
+ {
+ ts->printf(cvtest::TS::LOG, "Invalid accuracy (noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n",
+ getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec);
+ ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
+ }
+ }
+ }
+ }
+ }
+
+ for (size_t idx = 0; idx < methods.size(); idx++)
+ {
+ std::cout << std::endl;
+ printStats(getMethodName(methods[idx]), vec_rvec_diff[idx], vec_tvec_diff[idx]);
+ printStats("(noise) " + getMethodName(methods[idx]), vec_rvec_diff_noise[idx], vec_tvec_diff_noise[idx]);
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+
+TEST(Calib3d_CalibrateHandEye, regression_eye_in_hand)
+{
+ //Eye-in-Hand configuration (camera mounted on the robot end-effector observing a static calibration pattern)
+ const bool eyeToHand = false;
+ CV_CalibrateHandEyeTest test(eyeToHand);
+ test.safe_run();
+}
+
+TEST(Calib3d_CalibrateHandEye, regression_eye_to_hand)
+{
+ //Eye-to-Hand configuration (static camera observing a calibration pattern mounted on the robot end-effector)
+ const bool eyeToHand = true;
+ CV_CalibrateHandEyeTest test(eyeToHand);
+ test.safe_run();
+}
+
+TEST(Calib3d_CalibrateHandEye, regression_17986)
+{
+ std::vector<Mat> R_target2cam, t_target2cam;
+ // Dataset contains transformation from base to gripper frame since it contains data for AX = ZB calibration problem
+ std::vector<Mat> R_base2gripper, t_base2gripper;
+ loadDataset(R_target2cam, t_target2cam, R_base2gripper, t_base2gripper);
+
+ 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_cam2base_est;
+ Matx31d t_cam2base_est;
+ calibrateHandEye(R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, R_cam2base_est, t_cam2base_est, method);
+
+ EXPECT_TRUE(checkRange(R_cam2base_est));
+ EXPECT_TRUE(checkRange(t_cam2base_est));
+ }
+}
+
+TEST(Calib3d_CalibrateRobotWorldHandEye, regression)
+{
+ std::vector<Mat> R_world2cam, t_worldt2cam;
+ std::vector<Mat> R_base2gripper, t_base2gripper;
+ loadDataset(R_world2cam, t_worldt2cam, R_base2gripper, t_base2gripper);
+
+ std::vector<Mat> rvec_R_world2cam;
+ rvec_R_world2cam.reserve(R_world2cam.size());
+ for (size_t i = 0; i < R_world2cam.size(); i++)
+ {
+ Mat rvec;
+ cv::Rodrigues(R_world2cam[i], rvec);
+ rvec_R_world2cam.push_back(rvec);
+ }
- EXPECT_TRUE(checkRange(R_cam2gripper_est));
- EXPECT_TRUE(checkRange(t_cam2gripper_est));
+ std::vector<RobotWorldHandEyeCalibrationMethod> methods = {CALIB_ROBOT_WORLD_HAND_EYE_SHAH,
+ CALIB_ROBOT_WORLD_HAND_EYE_LI};
+
+ Matx33d wRb, cRg;
+ Matx31d wtb, ctg;
+ loadResults(wRb, wtb, cRg, ctg);
+
+ for (auto method : methods) {
+ SCOPED_TRACE(cv::format("method=%s", getMethodName(method).c_str()));
+
+ Matx33d wRb_est, cRg_est;
+ Matx31d wtb_est, ctg_est;
+ calibrateRobotWorldHandEye(rvec_R_world2cam, t_worldt2cam, R_base2gripper, t_base2gripper,
+ wRb_est, wtb_est, cRg_est, ctg_est, method);
+
+ EXPECT_TRUE(checkRange(wRb_est));
+ EXPECT_TRUE(checkRange(wtb_est));
+ EXPECT_TRUE(checkRange(cRg_est));
+ EXPECT_TRUE(checkRange(ctg_est));
+
+ //Arbitrary thresholds
+ const double rotation_threshold = 1.0; //1deg
+ const double translation_threshold = 50.0; //5cm
+
+ //X
+ //rotation error
+ Matx33d wRw_est = wRb * wRb_est.t();
+ Matx31d rvec_wRw_est;
+ cv::Rodrigues(wRw_est, rvec_wRw_est);
+ double X_rotation_error = cv::norm(rvec_wRw_est)*180/CV_PI;
+ //translation error
+ double X_t_error = cv::norm(wtb_est - wtb);
+ SCOPED_TRACE(cv::format("X rotation error=%f", X_rotation_error));
+ SCOPED_TRACE(cv::format("X translation error=%f", X_t_error));
+ EXPECT_TRUE(X_rotation_error < rotation_threshold);
+ EXPECT_TRUE(X_t_error < translation_threshold);
+
+ //Z
+ //rotation error
+ Matx33d cRc_est = cRg * cRg_est.t();
+ Matx31d rvec_cMc_est;
+ cv::Rodrigues(cRc_est, rvec_cMc_est);
+ double Z_rotation_error = cv::norm(rvec_cMc_est)*180/CV_PI;
+ //translation error
+ double Z_t_error = cv::norm(ctg_est - ctg);
+ SCOPED_TRACE(cv::format("Z rotation error=%f", Z_rotation_error));
+ SCOPED_TRACE(cv::format("Z translation error=%f", Z_t_error));
+ EXPECT_TRUE(Z_rotation_error < rotation_threshold);
+ EXPECT_TRUE(Z_t_error < translation_threshold);
}
}
projects / platform / upstream / opencv.git / commitdiff
