Skip to content

O
opencv
Commit bbf39b09 authored by catree's avatar catree
Browse files
Options

Add Hand-Eye calibration methods (Tsai, Park, Horaud, Andreff, Daniilidis).

parent e36a3acb
Hide whitespace changes
Inline Side-by-side
Showing with 1358 additions and 1 deletion
doc/opencv.bib
View file @ bbf39b09
......@@ -17,6 +17,21 @@
number = {7},
url = {http://www.bmva.org/bmvc/2013/Papers/paper0013/paper0013.pdf}
}
@inproceedings{Andreff99,
author = {Andreff, Nicolas and Horaud, Radu and Espiau, Bernard},
title = {On-line Hand-eye Calibration},
booktitle = {Proceedings of the 2Nd International Conference on 3-D Digital Imaging and Modeling},
series = {3DIM'99},
year = {1999},
isbn = {0-7695-0062-5},
location = {Ottawa, Canada},
pages = {430--436},
numpages = {7},
url = {http://dl.acm.org/citation.cfm?id=1889712.1889775},
acmid = {1889775},
publisher = {IEEE Computer Society},
address = {Washington, DC, USA},
}
@inproceedings{Arandjelovic:2012:TTE:2354409.2355123,
author = {Arandjelovic, Relja},
title = {Three Things Everyone Should Know to Improve Object Retrieval},
......@@ -180,6 +195,14 @@
volume = {9},
publisher = {Walter de Gruyter}
}
@article{Daniilidis98,
author = {Konstantinos Daniilidis},
title = {Hand-Eye Calibration Using Dual Quaternions},
journal = {International Journal of Robotics Research},
year = {1998},
volume = {18},
pages = {286--298}
}
@inproceedings{DM03,
author = {Drago, Fr{\'e}d{\'e}ric and Myszkowski, Karol and Annen, Thomas and Chiba, Norishige},
title = {Adaptive logarithmic mapping for displaying high contrast scenes},
......@@ -431,6 +454,24 @@
publisher = {Cambridge university press},
url = {http://cds.cern.ch/record/1598612/files/0521540518_TOC.pdf}
}
@article{Horaud95,
author = {Horaud, Radu and Dornaika, Fadi},
title = {Hand-eye Calibration},
journal = {Int. J. Rob. Res.},
issue_date = {June 1995},
volume = {14},
number = {3},
month = jun,
year = {1995},
issn = {0278-3649},
pages = {195--210},
numpages = {16},
url = {http://dx.doi.org/10.1177/027836499501400301},
doi = {10.1177/027836499501400301},
acmid = {208622},
publisher = {Sage Publications, Inc.},
address = {Thousand Oaks, CA, USA}
}
@article{Horn81,
author = {Horn, Berthold KP and Schunck, Brian G},
title = {Determining Optical Flow},
......@@ -667,6 +708,18 @@
number = {2},
publisher = {Elsevier}
}
@article{Park94,
author = {F. C. Park and B. J. Martin},
journal = {IEEE Transactions on Robotics and Automation},
title = {Robot sensor calibration: solving AX=XB on the Euclidean group},
year = {1994},
volume = {10},
number = {5},
pages = {717-721},
doi = {10.1109/70.326576},
ISSN = {1042-296X},
month = {Oct}
}
@inproceedings{PM03,
author = {P{\'e}rez, Patrick and Gangnet, Michel and Blake, Andrew},
title = {Poisson image editing},
......@@ -841,6 +894,18 @@
number = {1},
publisher = {Taylor \& Francis}
}
@article{Tsai89,
author = {R. Y. Tsai and R. K. Lenz},
journal = {IEEE Transactions on Robotics and Automation},
title = {A new technique for fully autonomous and efficient 3D robotics hand/eye calibration},
year = {1989},
volume = {5},
number = {3},
pages = {345-358},
doi = {10.1109/70.34770},
ISSN = {1042-296X},
month = {June}
}
@inproceedings{UES01,
author = {Uyttendaele, Matthew and Eden, Ashley and Skeliski, R},
title = {Eliminating ghosting and exposure artifacts in image mosaics},
......
modules/calib3d/include/opencv2/calib3d.hpp
View file @ bbf39b09
......@@ -277,7 +277,7 @@ enum { CALIB_USE_INTRINSIC_GUESS = 0x00001,
// for stereo rectification
CALIB_ZERO_DISPARITY = 0x00400,
CALIB_USE_LU = (1 << 17), //!< use LU instead of SVD decomposition for solving. much faster but potentially less precise
CALIB_USE_EXTRINSIC_GUESS = (1 << 22), //!< for stereoCalibrate
CALIB_USE_EXTRINSIC_GUESS = (1 << 22) //!< for stereoCalibrate
};
//! the algorithm for finding fundamental matrix
......@@ -287,6 +287,14 @@ enum { FM_7POINT = 1, //!< 7-point algorithm
FM_RANSAC = 8 //!< RANSAC algorithm. It needs at least 15 points. 7-point algorithm is used.
};
enum HandEyeCalibrationMethod
{
CALIB_HAND_EYE_TSAI = 0, //!< A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration @cite Tsai89
CALIB_HAND_EYE_PARK = 1, //!< Robot Sensor Calibration: Solving AX = XB on the Euclidean Group @cite Park94
CALIB_HAND_EYE_HORAUD = 2, //!< Hand-eye Calibration @cite Horaud95
CALIB_HAND_EYE_ANDREFF = 3, //!< On-line Hand-Eye Calibration @cite Andreff99
CALIB_HAND_EYE_DANIILIDIS = 4 //!< Hand-Eye Calibration Using Dual Quaternions @cite Daniilidis98
};
/** @brief Converts a rotation matrix to a rotation vector or vice versa.
......@@ -1402,6 +1410,139 @@ CV_EXPORTS_W Mat getOptimalNewCameraMatrix( InputArray cameraMatrix, InputArray
CV_OUT Rect* validPixROI = 0,
bool centerPrincipalPoint = false);
/** @brief Computes Hand-Eye calibration: \f$_{}^{g}\textrm{T}_c\f$
@param[in] R_gripper2base Rotation part extracted from the homogeneous matrix that transforms a point
expressed in the gripper frame to the robot base frame (\f$_{}^{b}\textrm{T}_g\f$).
This is a vector (`vector<Mat>`) that contains the rotation matrices for all the transformations
from gripper frame to robot base frame.
@param[in] t_gripper2base Translation part extracted from the homogeneous matrix that transforms a point
expressed in the gripper frame to the robot base frame (\f$_{}^{b}\textrm{T}_g\f$).
This is a vector (`vector<Mat>`) that contains the translation vectors for all the transformations
from gripper frame to robot base frame.
@param[in] R_target2cam Rotation part extracted from the homogeneous matrix that transforms a point
expressed in the target frame to the camera frame (\f$_{}^{c}\textrm{T}_t\f$).
This is a vector (`vector<Mat>`) that contains the rotation matrices for all the transformations
from calibration target frame to camera frame.
@param[in] t_target2cam Rotation part extracted from the homogeneous matrix that transforms a point
expressed in the target frame to the camera frame (\f$_{}^{c}\textrm{T}_t\f$).
This is a vector (`vector<Mat>`) that contains the translation vectors for all the transformations
from calibration target frame to camera frame.
@param[out] R_cam2gripper Estimated rotation part extracted from the homogeneous matrix that transforms a point
expressed in the camera frame to the gripper frame (\f$_{}^{g}\textrm{T}_c\f$).
@param[out] t_cam2gripper Estimated translation part extracted from the homogeneous matrix that transforms a point
expressed in the camera frame to the gripper frame (\f$_{}^{g}\textrm{T}_c\f$).
@param[in] method One of the implemented Hand-Eye calibration method, see cv::HandEyeCalibrationMethod
The function performs the Hand-Eye calibration using various methods. One approach consists in estimating the
rotation then the translation (separable solutions) and the following methods are implemented:
- R. Tsai, R. Lenz A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/EyeCalibration \cite Tsai89
- F. Park, B. Martin Robot Sensor Calibration: Solving AX = XB on the Euclidean Group \cite Park94
- R. Horaud, F. Dornaika Hand-Eye Calibration \cite Horaud95
Another approach consists in estimating simultaneously the rotation and the translation (simultaneous solutions),
with the following implemented method:
- N. Andreff, R. Horaud, B. Espiau On-line Hand-Eye Calibration \cite Andreff99
- K. Daniilidis Hand-Eye Calibration Using Dual Quaternions \cite Daniilidis98
The following picture describes the Hand-Eye calibration problem where the transformation between a camera ("eye")
mounted on a robot gripper ("hand") has to be estimated.
![](pics/hand-eye_figure.png)
The calibration procedure is the following:
- a static calibration pattern is used to estimate the transformation between the target frame
and the camera frame
- the robot gripper is moved in order to acquire several poses
- for each pose, the homogeneous transformation between the gripper frame and the robot base frame is recorded using for
instance the robot kinematics
\f[
\begin{bmatrix}
X_b\\
Y_b\\
Z_b\\
1
\end{bmatrix}
=
\begin{bmatrix}
_{}^{b}\textrm{R}_g & _{}^{b}\textrm{t}_g \\
0_{1 \times 3} & 1
\end{bmatrix}
\begin{bmatrix}
X_g\\
Y_g\\
Z_g\\
1
\end{bmatrix}
\f]
- for each pose, the homogeneous transformation between the calibration target frame and the camera frame is recorded using
for instance a pose estimation method (PnP) from 2D-3D point correspondences
\f[
\begin{bmatrix}
X_c\\
Y_c\\
Z_c\\
1
\end{bmatrix}
=
\begin{bmatrix}
_{}^{c}\textrm{R}_t & _{}^{c}\textrm{t}_t \\
0_{1 \times 3} & 1
\end{bmatrix}
\begin{bmatrix}
X_t\\
Y_t\\
Z_t\\
1
\end{bmatrix}
\f]
The Hand-Eye calibration procedure returns the following homogeneous transformation
\f[
\begin{bmatrix}
X_g\\
Y_g\\
Z_g\\
1
\end{bmatrix}
=
\begin{bmatrix}
_{}^{g}\textrm{R}_c & _{}^{g}\textrm{t}_c \\
0_{1 \times 3} & 1
\end{bmatrix}
\begin{bmatrix}
X_c\\
Y_c\\
Z_c\\
1
\end{bmatrix}
\f]
This problem is also known as solving the \f$\mathbf{A}\mathbf{X}=\mathbf{X}\mathbf{B}\f$ equation:
\f[
\begin{align*}
^{b}{\textrm{T}_g}^{(1)} \hspace{0.2em} ^{g}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(1)} &=
\hspace{0.1em} ^{b}{\textrm{T}_g}^{(2)} \hspace{0.2em} ^{g}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(2)} \\
(^{b}{\textrm{T}_g}^{(2)})^{-1} \hspace{0.2em} ^{b}{\textrm{T}_g}^{(1)} \hspace{0.2em} ^{g}\textrm{T}_c &=
\hspace{0.1em} ^{g}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(2)} (^{c}{\textrm{T}_t}^{(1)})^{-1} \\
\textrm{A}_i \textrm{X} &= \textrm{X} \textrm{B}_i \\
\end{align*}
\f]
\note
Additional information can be found on this [website](http://campar.in.tum.de/Chair/HandEyeCalibration).
\note
A minimum of 2 motions with non parallel rotation axes are necessary to determine the hand-eye transformation.
So at least 3 different poses are required, but it is strongly recommended to use many more poses.
*/
CV_EXPORTS_W void calibrateHandEye( InputArrayOfArrays R_gripper2base, InputArrayOfArrays t_gripper2base,
InputArrayOfArrays R_target2cam, InputArrayOfArrays t_target2cam,
OutputArray R_cam2gripper, OutputArray t_cam2gripper,
HandEyeCalibrationMethod method=CALIB_HAND_EYE_TSAI );
/** @brief Converts points from Euclidean to homogeneous space.
@param src Input vector of N-dimensional points.
......
modules/calib3d/src/calibration_handeye.cpp 0 → 100644
View file @ bbf39b09
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "precomp.hpp"
#include "opencv2/calib3d.hpp"
namespace cv {
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;
}
// q = rot2quatMinimal(R)
//
// R - 3x3 rotation matrix, or 4x4 homogeneous matrix
// q - 3x1 unit quaternion <qx, qy, qz>
// q = sin(theta/2) * v
// theta - rotation angle
// v - unit rotation axis, |v| = 1
static Mat rot2quatMinimal(const Mat& R)
{
CV_Assert(R.type() == CV_64FC1 && R.rows >= 3 && R.cols >= 3);
double m00 = R.at<double>(0,0), m01 = R.at<double>(0,1), m02 = R.at<double>(0,2);
double m10 = R.at<double>(1,0), m11 = R.at<double>(1,1), m12 = R.at<double>(1,2);
double m20 = R.at<double>(2,0), m21 = R.at<double>(2,1), m22 = R.at<double>(2,2);
double trace = m00 + m11 + m22;
double qx, qy, qz;
if (trace > 0) {
double S = sqrt(trace + 1.0) * 2; // S=4*qw
qx = (m21 - m12) / S;
qy = (m02 - m20) / S;
qz = (m10 - m01) / S;
} else if ((m00 > m11)&(m00 > m22)) {
double S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx
qx = 0.25 * S;
qy = (m01 + m10) / S;
qz = (m02 + m20) / S;
} else if (m11 > m22) {
double S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy
qx = (m01 + m10) / S;
qy = 0.25 * S;
qz = (m12 + m21) / S;
} else {
double S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz
qx = (m02 + m20) / S;
qy = (m12 + m21) / S;
qz = 0.25 * S;
}
return (Mat_<double>(3,1) << qx, qy, qz);
}
static Mat skew(const Mat& v)
{
CV_Assert(v.type() == CV_64FC1 && v.rows == 3 && v.cols == 1);
double vx = v.at<double>(0,0);
double vy = v.at<double>(1,0);
double vz = v.at<double>(2,0);
return (Mat_<double>(3,3) << 0, -vz, vy,
vz, 0, -vx,
-vy, vx, 0);
}
// R = quatMinimal2rot(q)
//
// q - 3x1 unit quaternion <qx, qy, qz>
// R - 3x3 rotation matrix
// q = sin(theta/2) * v
// theta - rotation angle
// v - unit rotation axis, |v| = 1
static Mat quatMinimal2rot(const Mat& q)
{
CV_Assert(q.type() == CV_64FC1 && q.rows == 3 && q.cols == 1);
Mat p = q.t()*q;
double w = sqrt(1 - p.at<double>(0,0));
Mat diag_p = Mat::eye(3,3,CV_64FC1)*p.at<double>(0,0);
return 2*q*q.t() + 2*w*skew(q) + Mat::eye(3,3,CV_64FC1) - 2*diag_p;
}
// q = rot2quat(R)
//
// q - 4x1 unit quaternion <qw, qx, qy, qz>
// R - 3x3 rotation matrix
static Mat rot2quat(const Mat& R)
{
CV_Assert(R.type() == CV_64FC1 && R.rows >= 3 && R.cols >= 3);
double m00 = R.at<double>(0,0), m01 = R.at<double>(0,1), m02 = R.at<double>(0,2);
double m10 = R.at<double>(1,0), m11 = R.at<double>(1,1), m12 = R.at<double>(1,2);
double m20 = R.at<double>(2,0), m21 = R.at<double>(2,1), m22 = R.at<double>(2,2);
double trace = m00 + m11 + m22;
double qw, qx, qy, qz;
if (trace > 0) {
double S = sqrt(trace + 1.0) * 2; // S=4*qw
qw = 0.25 * S;
qx = (m21 - m12) / S;
qy = (m02 - m20) / S;
qz = (m10 - m01) / S;
} else if ((m00 > m11)&(m00 > m22)) {
double S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx
qw = (m21 - m12) / S;
qx = 0.25 * S;
qy = (m01 + m10) / S;
qz = (m02 + m20) / S;
} else if (m11 > m22) {
double S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy
qw = (m02 - m20) / S;
qx = (m01 + m10) / S;
qy = 0.25 * S;
qz = (m12 + m21) / S;
} else {
double S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz
qw = (m10 - m01) / S;
qx = (m02 + m20) / S;
qy = (m12 + m21) / S;
qz = 0.25 * S;
}
return (Mat_<double>(4,1) << qw, qx, qy, qz);
}
// R = quat2rot(q)
//
// q - 4x1 unit quaternion <qw, qx, qy, qz>
// R - 3x3 rotation matrix
static Mat quat2rot(const Mat& q)
{
CV_Assert(q.type() == CV_64FC1 && q.rows == 4 && q.cols == 1);
double qw = q.at<double>(0,0);
double qx = q.at<double>(1,0);
double qy = q.at<double>(2,0);
double qz = q.at<double>(3,0);
Mat R(3, 3, CV_64FC1);
R.at<double>(0, 0) = 1 - 2*qy*qy - 2*qz*qz;
R.at<double>(0, 1) = 2*qx*qy - 2*qz*qw;
R.at<double>(0, 2) = 2*qx*qz + 2*qy*qw;
R.at<double>(1, 0) = 2*qx*qy + 2*qz*qw;
R.at<double>(1, 1) = 1 - 2*qx*qx - 2*qz*qz;
R.at<double>(1, 2) = 2*qy*qz - 2*qx*qw;
R.at<double>(2, 0) = 2*qx*qz - 2*qy*qw;
R.at<double>(2, 1) = 2*qy*qz + 2*qx*qw;
R.at<double>(2, 2) = 1 - 2*qx*qx - 2*qy*qy;
return R;
}
// Kronecker product or tensor product
// https://stackoverflow.com/a/36552682
static Mat kron(const Mat& A, const Mat& B)
{
CV_Assert(A.channels() == 1 && B.channels() == 1);
Mat1d Ad, Bd;
A.convertTo(Ad, CV_64F);
B.convertTo(Bd, CV_64F);
Mat1d Kd(Ad.rows * Bd.rows, Ad.cols * Bd.cols, 0.0);
for (int ra = 0; ra < Ad.rows; ra++)
{
for (int ca = 0; ca < Ad.cols; ca++)
{
Kd(Range(ra*Bd.rows, (ra + 1)*Bd.rows), Range(ca*Bd.cols, (ca + 1)*Bd.cols)) = Bd.mul(Ad(ra, ca));
}
}
Mat K;
Kd.convertTo(K, A.type());
return K;
}
// quaternion multiplication
static Mat qmult(const Mat& s, const Mat& t)
{
CV_Assert(s.type() == CV_64FC1 && t.type() == CV_64FC1);
CV_Assert(s.rows == 4 && s.cols == 1);
CV_Assert(t.rows == 4 && t.cols == 1);
double s0 = s.at<double>(0,0);
double s1 = s.at<double>(1,0);
double s2 = s.at<double>(2,0);
double s3 = s.at<double>(3,0);
double t0 = t.at<double>(0,0);
double t1 = t.at<double>(1,0);
double t2 = t.at<double>(2,0);
double t3 = t.at<double>(3,0);
Mat q(4, 1, CV_64FC1);
q.at<double>(0,0) = s0*t0 - s1*t1 - s2*t2 - s3*t3;
q.at<double>(1,0) = s0*t1 + s1*t0 + s2*t3 - s3*t2;
q.at<double>(2,0) = s0*t2 - s1*t3 + s2*t0 + s3*t1;
q.at<double>(3,0) = s0*t3 + s1*t2 - s2*t1 + s3*t0;
return q;
}
// dq = homogeneous2dualQuaternion(H)
//
// H - 4x4 homogeneous transformation: [R | t; 0 0 0 | 1]
// dq - 8x1 dual quaternion: <q (rotation part), qprime (translation part)>
static Mat homogeneous2dualQuaternion(const Mat& H)
{
CV_Assert(H.type() == CV_64FC1 && H.rows == 4 && H.cols == 4);
Mat dualq(8, 1, CV_64FC1);
Mat R = H(Rect(0, 0, 3, 3));
Mat t = H(Rect(3, 0, 1, 3));
Mat q = rot2quat(R);
Mat qt = Mat::zeros(4, 1, CV_64FC1);
t.copyTo(qt(Rect(0, 1, 1, 3)));
Mat qprime = 0.5 * qmult(qt, q);
q.copyTo(dualq(Rect(0, 0, 1, 4)));
qprime.copyTo(dualq(Rect(0, 4, 1, 4)));
return dualq;
}
// H = dualQuaternion2homogeneous(dq)
//
// H - 4x4 homogeneous transformation: [R | t; 0 0 0 | 1]
// dq - 8x1 dual quaternion: <q (rotation part), qprime (translation part)>
static Mat dualQuaternion2homogeneous(const Mat& dualq)
{
CV_Assert(dualq.type() == CV_64FC1 && dualq.rows == 8 && dualq.cols == 1);
Mat q = dualq(Rect(0, 0, 1, 4));
Mat qprime = dualq(Rect(0, 4, 1, 4));
Mat R = quat2rot(q);
q.at<double>(1,0) = -q.at<double>(1,0);
q.at<double>(2,0) = -q.at<double>(2,0);
q.at<double>(3,0) = -q.at<double>(3,0);
Mat qt = 2*qmult(qprime, q);
Mat t = qt(Rect(0, 1, 1, 3));
Mat H = Mat::eye(4, 4, CV_64FC1);
R.copyTo(H(Rect(0, 0, 3, 3)));
t.copyTo(H(Rect(3, 0, 1, 3)));
return H;
}
//Reference:
//R. Y. Tsai and R. K. Lenz, "A new technique for fully autonomous and efficient 3D robotics hand/eye calibration."
//In IEEE Transactions on Robotics and Automation, vol. 5, no. 3, pp. 345-358, June 1989.
//C++ code converted from Zoran Lazarevic's Matlab code:
//http://lazax.com/www.cs.columbia.edu/~laza/html/Stewart/matlab/handEye.m
static void calibrateHandEyeTsai(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
Mat& R_cam2gripper, Mat& t_cam2gripper)
{
//Number of unique camera position pairs
int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
//Will store: skew(Pgij+Pcij)
Mat A(3*K, 3, CV_64FC1);
//Will store: Pcij - Pgij
Mat B(3*K, 1, CV_64FC1);
std::vector<Mat> vec_Hgij, vec_Hcij;
vec_Hgij.reserve(static_cast<size_t>(K));
vec_Hcij.reserve(static_cast<size_t>(K));
int idx = 0;
for (size_t i = 0; i < Hg.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 = 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);
//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)));
}
}
Mat Pcg_;
//Rotation from camera to gripper is obtained from the set of equations:
// skew(Pgij+Pcij) * Pcg_ = Pcij - Pgij (eq 12)
solve(A, B, Pcg_, DECOMP_SVD);
Mat Pcg_norm = Pcg_.t() * Pcg_;
//Obtained non-unit quaternion is scaled back to unit value that
//designates camera-gripper rotation
Mat Pcg = 2 * Pcg_ / sqrt(1 + Pcg_norm.at<double>(0,0)); //eq 14
Mat Rcg = quatMinimal2rot(Pcg/2.0);
idx = 0;
for (size_t i = 0; i < Hg.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)));
}
}
Mat Tcg;
//Translation from camera to gripper is obtained from the set of equations:
// (Rgij - I) * Tcg = Rcg*Tcij - Tgij (eq 15)
solve(A, B, Tcg, DECOMP_SVD);
R_cam2gripper = Rcg;
t_cam2gripper = Tcg;
}
//Reference:
//F. Park, B. Martin, "Robot Sensor Calibration: Solving AX = XB on the Euclidean Group."
//In IEEE Transactions on Robotics and Automation, 10(5): 717-721, 1994.
//Matlab code: http://math.loyola.edu/~mili/Calibration/
static void calibrateHandEyePark(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
Mat& R_cam2gripper, Mat& t_cam2gripper)
{
Mat M = Mat::zeros(3, 3, CV_64FC1);
for (size_t i = 0; i < Hg.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++)
{
Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
Mat Rgij = Hgij(Rect(0, 0, 3, 3));
Mat Rcij = Hcij(Rect(0, 0, 3, 3));
Mat a, b;
Rodrigues(Rgij, a);
Rodrigues(Rcij, b);
M += b * a.t();
}
}
Mat eigenvalues, eigenvectors;
eigen(M.t()*M, eigenvalues, eigenvectors);
Mat v = Mat::zeros(3, 3, CV_64FC1);
for (int i = 0; i < 3; i++) {
v.at<double>(i,i) = 1.0 / sqrt(eigenvalues.at<double>(i,0));
}
Mat R = eigenvectors.t() * v * eigenvectors * M.t();
R_cam2gripper = R;
int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
Mat C(3*K, 3, CV_64FC1);
Mat d(3*K, 1, CV_64FC1);
Mat I3 = Mat::eye(3, 3, CV_64FC1);
int idx = 0;
for (size_t i = 0; i < Hg.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++, idx++)
{
Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
Mat Rgij = Hgij(Rect(0, 0, 3, 3));
Mat tgij = Hgij(Rect(3, 0, 1, 3));
Mat tcij = Hcij(Rect(3, 0, 1, 3));
Mat I_tgij = I3 - Rgij;
I_tgij.copyTo(C(Rect(0, 3*idx, 3, 3)));
Mat A_RB = tgij - R*tcij;
A_RB.copyTo(d(Rect(0, 3*idx, 1, 3)));
}
}
Mat t;
solve(C, d, t, DECOMP_SVD);
t_cam2gripper = t;
}
//Reference:
//R. Horaud, F. Dornaika, "Hand-Eye Calibration"
//In International Journal of Robotics Research, 14(3): 195-210, 1995.
//Matlab code: http://math.loyola.edu/~mili/Calibration/
static void calibrateHandEyeHoraud(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
Mat& R_cam2gripper, Mat& t_cam2gripper)
{
Mat A = Mat::zeros(4, 4, CV_64FC1);
for (size_t i = 0; i < Hg.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++)
{
Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
Mat Rgij = Hgij(Rect(0, 0, 3, 3));
Mat Rcij = Hcij(Rect(0, 0, 3, 3));
Mat qgij = rot2quat(Rgij);
double r0 = qgij.at<double>(0,0);
double rx = qgij.at<double>(1,0);
double ry = qgij.at<double>(2,0);
double rz = qgij.at<double>(3,0);
// Q(r) Appendix A
Matx44d Qvi(r0, -rx, -ry, -rz,
rx, r0, -rz, ry,
ry, rz, r0, -rx,
rz, -ry, rx, r0);
Mat qcij = rot2quat(Rcij);
r0 = qcij.at<double>(0,0);
rx = qcij.at<double>(1,0);
ry = qcij.at<double>(2,0);
rz = qcij.at<double>(3,0);
// W(r) Appendix A
Matx44d Wvi(r0, -rx, -ry, -rz,
rx, r0, rz, -ry,
ry, -rz, r0, rx,
rz, ry, -rx, r0);
// Ai = (Q(vi') - W(vi))^T (Q(vi') - W(vi))
A += (Qvi - Wvi).t() * (Qvi - Wvi);
}
}
Mat eigenvalues, eigenvectors;
eigen(A, eigenvalues, eigenvectors);
Mat R = quat2rot(eigenvectors.row(3).t());
R_cam2gripper = R;
int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
Mat C(3*K, 3, CV_64FC1);
Mat d(3*K, 1, CV_64FC1);
Mat I3 = Mat::eye(3, 3, CV_64FC1);
int idx = 0;
for (size_t i = 0; i < Hg.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++, idx++)
{
Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
Mat Rgij = Hgij(Rect(0, 0, 3, 3));
Mat tgij = Hgij(Rect(3, 0, 1, 3));
Mat tcij = Hcij(Rect(3, 0, 1, 3));
Mat I_tgij = I3 - Rgij;
I_tgij.copyTo(C(Rect(0, 3*idx, 3, 3)));
Mat A_RB = tgij - R*tcij;
A_RB.copyTo(d(Rect(0, 3*idx, 1, 3)));
}
}
Mat t;
solve(C, d, t, DECOMP_SVD);
t_cam2gripper = t;
}
// sign function, return -1 if negative values, +1 otherwise
static int sign_double(double val)
{
return (0 < val) - (val < 0);
}
//Reference:
//N. Andreff, R. Horaud, B. Espiau, "On-line Hand-Eye Calibration."
//In Second International Conference on 3-D Digital Imaging and Modeling (3DIM'99), pages 430-436, 1999.
//Matlab code: http://math.loyola.edu/~mili/Calibration/
static void calibrateHandEyeAndreff(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
Mat& R_cam2gripper, Mat& t_cam2gripper)
{
int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
Mat A(12*K, 12, CV_64FC1);
Mat B(12*K, 1, CV_64FC1);
Mat I9 = Mat::eye(9, 9, CV_64FC1);
Mat I3 = Mat::eye(3, 3, CV_64FC1);
Mat O9x3 = Mat::zeros(9, 3, CV_64FC1);
Mat O9x1 = Mat::zeros(9, 1, CV_64FC1);
int idx = 0;
for (size_t i = 0; i < Hg.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++, idx++)
{
Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
Mat Rgij = Hgij(Rect(0, 0, 3, 3));
Mat Rcij = Hcij(Rect(0, 0, 3, 3));
Mat tgij = Hgij(Rect(3, 0, 1, 3));
Mat tcij = Hcij(Rect(3, 0, 1, 3));
//Eq 10
Mat a00 = I9 - kron(Rgij, Rcij);
Mat a01 = O9x3;
Mat a10 = kron(I3, tcij.t());
Mat a11 = I3 - Rgij;
a00.copyTo(A(Rect(0, idx*12, 9, 9)));
a01.copyTo(A(Rect(9, idx*12, 3, 9)));
a10.copyTo(A(Rect(0, idx*12 + 9, 9, 3)));
a11.copyTo(A(Rect(9, idx*12 + 9, 3, 3)));
O9x1.copyTo(B(Rect(0, idx*12, 1, 9)));
tgij.copyTo(B(Rect(0, idx*12 + 9, 1, 3)));
}
}
Mat X;
solve(A, B, X, DECOMP_SVD);
Mat R = X(Rect(0, 0, 1, 9));
int newSize[] = {3, 3};
R = R.reshape(1, 2, newSize);
//Eq 15
double det = determinant(R);
R = pow(sign_double(det) / abs(det), 1.0/3.0) * 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;
}
//Reference:
//K. Daniilidis, "Hand-Eye Calibration Using Dual Quaternions."
//In The International Journal of Robotics Research,18(3): 286-298, 1998.
//Matlab code: http://math.loyola.edu/~mili/Calibration/
static void calibrateHandEyeDaniilidis(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
Mat& R_cam2gripper, Mat& t_cam2gripper)
{
int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
Mat T = Mat::zeros(6*K, 8, CV_64FC1);
int idx = 0;
for (size_t i = 0; i < Hg.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++, idx++)
{
Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
Mat dualqa = homogeneous2dualQuaternion(Hgij);
Mat dualqb = homogeneous2dualQuaternion(Hcij);
Mat a = dualqa(Rect(0, 1, 1, 3));
Mat b = dualqb(Rect(0, 1, 1, 3));
Mat aprime = dualqa(Rect(0, 5, 1, 3));
Mat bprime = dualqb(Rect(0, 5, 1, 3));
//Eq 31
Mat s00 = a - b;
Mat s01 = skew(a + b);
Mat s10 = aprime - bprime;
Mat s11 = skew(aprime + bprime);
Mat s12 = a - b;
Mat s13 = skew(a + b);
s00.copyTo(T(Rect(0, idx*6, 1, 3)));
s01.copyTo(T(Rect(1, idx*6, 3, 3)));
s10.copyTo(T(Rect(0, idx*6 + 3, 1, 3)));
s11.copyTo(T(Rect(1, idx*6 + 3, 3, 3)));
s12.copyTo(T(Rect(4, idx*6 + 3, 1, 3)));
s13.copyTo(T(Rect(5, idx*6 + 3, 3, 3)));
}
}
Mat w, u, vt;
SVDecomp(T, w, u, vt);
Mat v = vt.t();
Mat u1 = v(Rect(6, 0, 1, 4));
Mat v1 = v(Rect(6, 4, 1, 4));
Mat u2 = v(Rect(7, 0, 1, 4));
Mat v2 = v(Rect(7, 4, 1, 4));
//Solves Eq 34, Eq 35
Mat ma = u1.t()*v1;
Mat mb = u1.t()*v2 + u2.t()*v1;
Mat mc = u2.t()*v2;
double a = ma.at<double>(0,0);
double b = mb.at<double>(0,0);
double c = mc.at<double>(0,0);
double s1 = (-b + sqrt(b*b - 4*a*c)) / (2*a);
double s2 = (-b - sqrt(b*b - 4*a*c)) / (2*a);
Mat sol1 = s1*s1*u1.t()*u1 + 2*s1*u1.t()*u2 + u2.t()*u2;
Mat sol2 = s2*s2*u1.t()*u1 + 2*s2*u1.t()*u2 + u2.t()*u2;
double s, val;
if (sol1.at<double>(0,0) > sol2.at<double>(0,0))
{
s = s1;
val = sol1.at<double>(0,0);
}
else
{
s = s2;
val = sol2.at<double>(0,0);
}
double lambda2 = sqrt(1.0 / val);
double lambda1 = s * lambda2;
Mat dualq = lambda1 * v(Rect(6, 0, 1, 8)) + lambda2*v(Rect(7, 0, 1, 8));
Mat X = dualQuaternion2homogeneous(dualq);
Mat R = X(Rect(0, 0, 3, 3));
Mat t = X(Rect(3, 0, 1, 3));
R_cam2gripper = R;
t_cam2gripper = t;
}
void calibrateHandEye(InputArrayOfArrays R_gripper2base, InputArrayOfArrays t_gripper2base,
InputArrayOfArrays R_target2cam, InputArrayOfArrays t_target2cam,
OutputArray R_cam2gripper, OutputArray t_cam2gripper,
HandEyeCalibrationMethod method)
{
CV_Assert(R_gripper2base.isMatVector() && t_gripper2base.isMatVector() &&
R_target2cam.isMatVector() && t_target2cam.isMatVector());
std::vector<Mat> R_gripper2base_, t_gripper2base_;
R_gripper2base.getMatVector(R_gripper2base_);
t_gripper2base.getMatVector(t_gripper2base_);
std::vector<Mat> R_target2cam_, t_target2cam_;
R_target2cam.getMatVector(R_target2cam_);
t_target2cam.getMatVector(t_target2cam_);
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);
//Notation used in Tsai paper
//Defines coordinate transformation from G (gripper) to RW (robot base)
std::vector<Mat> Hg;
Hg.reserve(R_gripper2base_.size());
for (size_t i = 0; i < R_gripper2base_.size(); i++)
{
Mat m = Mat::eye(4, 4, CV_64FC1);
Mat R = m(Rect(0, 0, 3, 3));
R_gripper2base_[i].convertTo(R, CV_64F);
Mat t = m(Rect(3, 0, 1, 3));
t_gripper2base_[i].convertTo(t, CV_64F);
Hg.push_back(m);
}
//Defines coordinate transformation from CW (calibration target) to C (camera)
std::vector<Mat> Hc;
Hc.reserve(R_target2cam_.size());
for (size_t i = 0; i < R_target2cam_.size(); i++)
{
Mat m = Mat::eye(4, 4, CV_64FC1);
Mat R = m(Rect(0, 0, 3, 3));
R_target2cam_[i].convertTo(R, CV_64F);
Mat t = m(Rect(3, 0, 1, 3));
t_target2cam_[i].convertTo(t, CV_64F);
Hc.push_back(m);
}
Mat Rcg = Mat::eye(3, 3, CV_64FC1);
Mat Tcg = Mat::zeros(3, 1, CV_64FC1);
switch (method)
{
case CALIB_HAND_EYE_TSAI:
calibrateHandEyeTsai(Hg, Hc, Rcg, Tcg);
break;
case CALIB_HAND_EYE_PARK:
calibrateHandEyePark(Hg, Hc, Rcg, Tcg);
break;
case CALIB_HAND_EYE_HORAUD:
calibrateHandEyeHoraud(Hg, Hc, Rcg, Tcg);
break;
case CALIB_HAND_EYE_ANDREFF:
calibrateHandEyeAndreff(Hg, Hc, Rcg, Tcg);
break;
case CALIB_HAND_EYE_DANIILIDIS:
calibrateHandEyeDaniilidis(Hg, Hc, Rcg, Tcg);
break;
default:
break;
}
Rcg.copyTo(R_cam2gripper);
Tcg.copyTo(t_cam2gripper);
}
}
modules/calib3d/test/test_calibration_hand_eye.cpp 0 → 100644
View file @ bbf39b09
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "test_precomp.hpp"
#include "opencv2/calib3d.hpp"
namespace opencv_test { namespace {
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);
std::string getMethodName(HandEyeCalibrationMethod method);
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)
{
Mat axis(3, 1, CV_64FC1);
for (int i = 0; i < 3; i++)
{
axis.at<double>(i,0) = rng.uniform(-1.0, 1.0);
}
double theta = rng.uniform(min_theta, max_theta);
if (random_sign)
{
theta *= sign_double(rng.uniform(-1.0, 1.0));
}
Mat rvec(3, 1, CV_64FC1);
rvec.at<double>(0,0) = theta*axis.at<double>(0,0);
rvec.at<double>(1,0) = theta*axis.at<double>(1,0);
rvec.at<double>(2,0) = theta*axis.at<double>(2,0);
tvec.create(3, 1, CV_64FC1);
tvec.at<double>(0,0) = rng.uniform(min_tx, max_tx);
tvec.at<double>(1,0) = rng.uniform(min_ty, max_ty);
tvec.at<double>(2,0) = rng.uniform(min_tz, max_tz);
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));
}
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)
{
//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.05, 0.5, 0.05, 0.5, 0.05, 0.5,
R_cam2gripper, t_cam2gripper, random_sign);
Mat R_target2base, t_target2base;
generatePose(rng, 5.0*CV_PI/180.0, 85.0*CV_PI/180.0,
0.5, 3.5, 0.5, 3.5, 0.5, 3.5,
R_target2base, t_target2base, random_sign);
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);
R_gripper2base.push_back(R_gripper2base_);
t_gripper2base.push_back(t_gripper2base_);
Mat T_cam2gripper = Mat::eye(4, 4, CV_64FC1);
R_cam2gripper.copyTo(T_cam2gripper(Rect(0, 0, 3, 3)));
t_cam2gripper.copyTo(T_cam2gripper(Rect(3, 0, 1, 3)));
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_base2cam = homogeneousInverse(T_cam2gripper) * homogeneousInverse(T_gripper2base);
Mat T_target2base = Mat::eye(4, 4, CV_64FC1);
R_target2base.copyTo(T_target2base(Rect(0, 0, 3, 3)));
t_target2base.copyTo(T_target2base(Rect(3, 0, 1, 3)));
Mat T_target2cam = T_base2cam * T_target2base;
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 gripper and the robot base
Mat R_gripper2base_noise = T_gripper2base(Rect(0, 0, 3, 3));
Mat rvec_gripper2base_noise;
cv::Rodrigues(R_gripper2base_noise, rvec_gripper2base_noise);
rvec_gripper2base_noise.at<double>(0,0) += rng.gaussian(0.001);
rvec_gripper2base_noise.at<double>(1,0) += rng.gaussian(0.001);
rvec_gripper2base_noise.at<double>(2,0) += rng.gaussian(0.001);
cv::Rodrigues(rvec_gripper2base_noise, R_gripper2base_noise);
Mat t_gripper2base_noise = T_gripper2base(Rect(3, 0, 1, 3));
t_gripper2base_noise.at<double>(0,0) += rng.gaussian(0.001);
t_gripper2base_noise.at<double>(1,0) += rng.gaussian(0.001);
t_gripper2base_noise.at<double>(2,0) += rng.gaussian(0.001);
}
R_target2cam.push_back(T_target2cam(Rect(0, 0, 3, 3)));
t_target2cam.push_back(T_target2cam(Rect(3, 0, 1, 3)));
}
}
Mat CV_CalibrateHandEyeTest::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;
}
std::string CV_CalibrateHandEyeTest::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;
}
double CV_CalibrateHandEyeTest::sign_double(double val)
{
return (0 < val) - (val < 0);
}
///////////////////////////////////////////////////////////////////////////////////////////////////
TEST(Calib3d_CalibrateHandEye, regression) { CV_CalibrateHandEyeTest test; test.safe_run(); }
}} // namespace
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment