Merge pull request #13880 from catree:add_hand_eye_calibration

doc/opencv.bib

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

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