Add solvePnPRefineLM to refine a pose according to a Levenberg-Marquardt…

Add solvePnPRefineLM to refine a pose according to a Levenberg-Marquardt iterative minimization process. Add solvePnPRefineVVS to refine a pose using a virtual visual servoing scheme.

doc/opencv.bib

@@ -195,6 +195,21 @@
   volume = {9},
   publisher = {Walter de Gruyter}
 }
+@article{Chaumette06,
+  author = {Chaumette, Fran{\c c}ois and Hutchinson, S.},
+  title = {{Visual servo control, Part I: Basic approaches}},
+  url = {https://hal.inria.fr/inria-00350283},
+  journal = {{IEEE Robotics and Automation Magazine}},
+  publisher = {{Institute of Electrical and Electronics Engineers}},
+  volume = {13},
+  number = {4},
+  pages = {82-90},
+  year = {2006},
+  pdf = {https://hal.inria.fr/inria-00350283/file/2006_ieee_ram_chaumette.pdf},
+  hal_id = {inria-00350283},
+  hal_version = {v1},
+}
+
 @article{Daniilidis98,
   author = {Konstantinos Daniilidis},
   title = {Hand-Eye Calibration Using Dual Quaternions},
@@ -242,6 +257,12 @@
   publisher = {IEEE},
   url = {http://alumni.media.mit.edu/~jdavis/Publications/publications_402.pdf}
 }
+@misc{Eade13,
+  author = {Eade, Ethan},
+  title = {Gauss-Newton / Levenberg-Marquardt Optimization},
+  year = {2013},
+  url = {http://ethaneade.com/optimization.pdf}
+}
 @inproceedings{EM11,
   author = {Gastal, Eduardo SL and Oliveira, Manuel M},
   title = {Domain transform for edge-aware image and video processing},
@@ -596,10 +617,14 @@
   title = {ROF and TV-L1 denoising with Primal-Dual algorithm},
   url = {http://znah.net/rof-and-tv-l1-denoising-with-primal-dual-algorithm.html}
 }
-@misc{VandLec,
-  author = {Vandenberghe, Lieven},
-  title = {QR Factorization},
-  url = {http://www.seas.ucla.edu/~vandenbe/133A/lectures/qr.pdf}
+@misc{Madsen04,
+  author = {K. Madsen and H. B. Nielsen and O. Tingleff},
+  title = {Methods for Non-Linear Least Squares Problems (2nd ed.)},
+  year = {2004},
+  pages = {60},
+  publisher = {Informatics and Mathematical Modelling, Technical University of Denmark, {DTU}},
+  address = {Richard Petersens Plads, Building 321, {DK-}2800 Kgs. Lyngby},
+  url = {http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf}
 }
 @article{MHT2011,
   author = {Getreuer, Pascal},
@@ -645,6 +670,23 @@
   title = {Deeper understanding of the homography decomposition for vision-based control},
   year = {2007}
 }
+@article{Marchand16,
+  author = {Marchand, Eric and Uchiyama, Hideaki and Spindler, Fabien},
+  title = {{Pose Estimation for Augmented Reality: A Hands-On Survey}},
+  url = {https://hal.inria.fr/hal-01246370},
+  journal = {{IEEE Transactions on Visualization and Computer Graphics}},
+  publisher = {{Institute of Electrical and Electronics Engineers}},
+  volume = {22},
+  number = {12},
+  pages = {2633 - 2651},
+  year = {2016},
+  month = Dec,
+  doi = {10.1109/TVCG.2015.2513408},
+  keywords = {homography ; SLAM ; motion estimation ; Index Terms-Survey ; augmented reality ; vision-based camera localization ; pose estimation ; PnP ; keypoint matching ; code examples},
+  pdf = {https://hal.inria.fr/hal-01246370/file/survey-ieee-v2.pdf},
+  hal_id = {hal-01246370},
+  hal_version = {v1},
+}
 @article{Matas00,
   author = {Matas, Jiri and Galambos, Charles and Kittler, Josef},
   title = {Robust detection of lines using the progressive probabilistic hough transform},
@@ -915,6 +957,11 @@
   volume = {2},
   publisher = {IEEE}
 }
+@misc{VandLec,
+  author = {Vandenberghe, Lieven},
+  title = {QR Factorization},
+  url = {http://www.seas.ucla.edu/~vandenbe/133A/lectures/qr.pdf}
+}
 @inproceedings{V03,
   author = {Kwatra, Vivek and Sch{\"o}dl, Arno and Essa, Irfan and Turk, Greg and Bobick, Aaron},
   title = {Graphcut textures: image and video synthesis using graph cuts},

modules/calib3d/include/opencv2/calib3d.hpp

@@ -777,6 +777,65 @@ CV_EXPORTS_W int solveP3P( InputArray objectPoints, InputArray imagePoints,
                            OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
                            int flags );
 
+/** @brief Refine a pose (the translation and the rotation that transform a 3D point expressed in the object coordinate frame
+to the camera coordinate frame) from a 3D-2D point correspondences and starting from an initial solution.
+
+@param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or 1xN/Nx1 3-channel,
+where N is the number of points. vector\<Point3f\> can also be passed here.
+@param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel,
+where N is the number of points. vector\<Point2f\> can also be passed here.
+@param cameraMatrix Input camera matrix \f$A = \vecthreethree{fx}{0}{cx}{0}{fy}{cy}{0}{0}{1}\f$ .
+@param distCoeffs Input vector of distortion coefficients
+\f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ of
+4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are
+assumed.
+@param rvec Input/Output rotation vector (see @ref Rodrigues ) that, together with tvec , brings points from
+the model coordinate system to the camera coordinate system. Input values are used as an initial solution.
+@param tvec Input/Output translation vector. Input values are used as an initial solution.
+@param criteria Criteria when to stop the Levenberg-Marquard iterative algorithm.
+
+The function refines the object pose given at least 3 object points, their corresponding image
+projections, an initial solution for the rotation and translation vector,
+as well as the camera matrix and the distortion coefficients.
+The function minimizes the projection error with respect to the rotation and the translation vectors, according
+to a Levenberg-Marquardt iterative minimization @cite Madsen04 @cite Eade13 process.
+ */
+CV_EXPORTS_W void solvePnPRefineLM( InputArray objectPoints, InputArray imagePoints,
+                                    InputArray cameraMatrix, InputArray distCoeffs,
+                                    InputOutputArray rvec, InputOutputArray tvec,
+                                    TermCriteria criteria = TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 20, FLT_EPSILON));
+
+/** @brief Refine a pose (the translation and the rotation that transform a 3D point expressed in the object coordinate frame
+to the camera coordinate frame) from a 3D-2D point correspondences and starting from an initial solution.
+
+@param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or 1xN/Nx1 3-channel,
+where N is the number of points. vector\<Point3f\> can also be passed here.
+@param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel,
+where N is the number of points. vector\<Point2f\> can also be passed here.
+@param cameraMatrix Input camera matrix \f$A = \vecthreethree{fx}{0}{cx}{0}{fy}{cy}{0}{0}{1}\f$ .
+@param distCoeffs Input vector of distortion coefficients
+\f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ of
+4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are
+assumed.
+@param rvec Input/Output rotation vector (see @ref Rodrigues ) that, together with tvec , brings points from
+the model coordinate system to the camera coordinate system. Input values are used as an initial solution.
+@param tvec Input/Output translation vector. Input values are used as an initial solution.
+@param criteria Criteria when to stop the Levenberg-Marquard iterative algorithm.
+@param VVSlambda Gain for the virtual visual servoing control law, equivalent to the \f$\alpha\f$
+gain in the Gauss-Newton formulation.
+
+The function refines the object pose given at least 3 object points, their corresponding image
+projections, an initial solution for the rotation and translation vector,
+as well as the camera matrix and the distortion coefficients.
+The function minimizes the projection error with respect to the rotation and the translation vectors, using a
+virtual visual servoing (VVS) @cite Chaumette06 @cite Marchand16 scheme.
+ */
+CV_EXPORTS_W void solvePnPRefineVVS( InputArray objectPoints, InputArray imagePoints,
+                                     InputArray cameraMatrix, InputArray distCoeffs,
+                                     InputOutputArray rvec, InputOutputArray tvec,
+                                     TermCriteria criteria = TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 20, FLT_EPSILON),
+                                     double VVSlambda = 1);
+
 /** @brief Finds an initial camera matrix from 3D-2D point correspondences.
 
 @param objectPoints Vector of vectors of the calibration pattern points in the calibration pattern

modules/calib3d/src/levmarq.cpp

@@ -81,11 +81,11 @@ class LMSolverImpl CV_FINAL : public LMSolver
 {
 public:
     LMSolverImpl() : maxIters(100) { init(); }
-    LMSolverImpl(const Ptr<LMSolver::Callback>& _cb, int _maxIters) : cb(_cb), maxIters(_maxIters) { init(); }
+    LMSolverImpl(const Ptr<LMSolver::Callback>& _cb, int _maxIters) : cb(_cb), epsx(FLT_EPSILON), epsf(FLT_EPSILON), maxIters(_maxIters) { init(); }
+    LMSolverImpl(const Ptr<LMSolver::Callback>& _cb, int _maxIters, double _eps) : cb(_cb), epsx(_eps), epsf(_eps), maxIters(_maxIters) { init(); }
 
     void init()
     {
-        epsx = epsf = FLT_EPSILON;
         printInterval = 0;
     }
 
@@ -214,4 +214,9 @@ Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters)
     return makePtr<LMSolverImpl>(cb, maxIters);
 }
 
+Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps)
+{
+    return makePtr<LMSolverImpl>(cb, maxIters, eps);
+}
+
 }

modules/calib3d/src/precomp.hpp

@@ -95,6 +95,7 @@ public:
 };
 
 CV_EXPORTS Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters);
+CV_EXPORTS Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps);
 
 class CV_EXPORTS PointSetRegistrator : public Algorithm
 {

modules/calib3d/src/solvepnp.cpp

@@ -456,4 +456,271 @@ int solveP3P( InputArray _opoints, InputArray _ipoints,
     return solutions;
 }
 
+class SolvePnPRefineLMCallback CV_FINAL : public LMSolver::Callback
+{
+public:
+    SolvePnPRefineLMCallback(InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs)
+    {
+        objectPoints = _opoints.getMat();
+        imagePoints = _ipoints.getMat();
+        npoints = std::max(objectPoints.checkVector(3, CV_32F), objectPoints.checkVector(3, CV_64F));
+        imagePoints0 = imagePoints.reshape(1, npoints*2);
+        cameraMatrix = _cameraMatrix.getMat();
+        distCoeffs = _distCoeffs.getMat();
+    }
+
+    bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const CV_OVERRIDE
+    {
+         Mat param = _param.getMat();
+         _err.create(npoints*2, 1, CV_64FC1);
+
+         if(_Jac.needed())
+         {
+             _Jac.create(npoints*2, param.rows, CV_64FC1);
+         }
+
+         Mat rvec = param(Rect(0, 0, 1, 3)), tvec = param(Rect(0, 3, 1, 3));
+
+         Mat J, projectedPts;
+         projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, projectedPts, _Jac.needed() ? J : noArray());
+
+         if (_Jac.needed())
+         {
+             Mat Jac = _Jac.getMat();
+             for (int i = 0; i < Jac.rows; i++)
+             {
+                 for (int j = 0; j < Jac.cols; j++)
+                 {
+                     Jac.at<double>(i,j) = J.at<double>(i,j);
+                 }
+             }
+         }
+
+         Mat err = _err.getMat();
+         projectedPts = projectedPts.reshape(1, npoints*2);
+         err = projectedPts - imagePoints0;
+
+        return true;
+    }
+
+    Mat objectPoints, imagePoints, imagePoints0;
+    Mat cameraMatrix, distCoeffs;
+    int npoints;
+};
+
+/**
+ * @brief Compute the Interaction matrix and the residuals for the current pose.
+ * @param objectPoints 3D object points.
+ * @param R Current estimated rotation matrix.
+ * @param tvec Current estimated translation vector.
+ * @param L Interaction matrix for a vector of point features.
+ * @param s Residuals.
+ */
+static void computeInteractionMatrixAndResiduals(const Mat& objectPoints, const Mat& R, const Mat& tvec,
+                                                 Mat& L, Mat& s)
+{
+    Mat objectPointsInCam;
+
+    int npoints = objectPoints.rows;
+    for (int i = 0; i < npoints; i++)
+    {
+        Mat curPt = objectPoints.row(i);
+        objectPointsInCam = R * curPt.t() + tvec;
+
+        double Zi = objectPointsInCam.at<double>(2,0);
+        double xi = objectPointsInCam.at<double>(0,0) / Zi;
+        double yi = objectPointsInCam.at<double>(1,0) / Zi;
+
+        s.at<double>(2*i,0) = xi;
+        s.at<double>(2*i+1,0) = yi;
+
+        L.at<double>(2*i,0) = -1 / Zi;
+        L.at<double>(2*i,1) = 0;
+        L.at<double>(2*i,2) = xi / Zi;
+        L.at<double>(2*i,3) = xi*yi;
+        L.at<double>(2*i,4) = -(1 + xi*xi);
+        L.at<double>(2*i,5) = yi;
+
+        L.at<double>(2*i+1,0) = 0;
+        L.at<double>(2*i+1,1) = -1 / Zi;
+        L.at<double>(2*i+1,2) = yi / Zi;
+        L.at<double>(2*i+1,3) = 1 + yi*yi;
+        L.at<double>(2*i+1,4) = -xi*yi;
+        L.at<double>(2*i+1,5) = -xi;
+    }
+}
+
+/**
+ * @brief The exponential map from se(3) to SE(3).
+ * @param twist A twist (v, w) represents the velocity of a rigid body as an angular velocity
+ * around an axis and a linear velocity along this axis.
+ * @param R1 Resultant rotation matrix from the twist.
+ * @param t1 Resultant translation vector from the twist.
+ */
+static void exponentialMapToSE3Inv(const Mat& twist, Mat& R1, Mat& t1)
+{
+    //see Exponential Map in http://ethaneade.com/lie.pdf
+    /*
+    \begin{align*}
+    \boldsymbol{\delta} &= \left( \mathbf{u}, \boldsymbol{\omega} \right ) \in se(3) \\
+    \mathbf{u}, \boldsymbol{\omega} &\in \mathbb{R}^3 \\
+    \theta &= \sqrt{ \boldsymbol{\omega}^T \boldsymbol{\omega} } \\
+    A &= \frac{\sin \theta}{\theta} \\
+    B &= \frac{1 - \cos \theta}{\theta^2} \\
+    C &= \frac{1-A}{\theta^2} \\
+    \mathbf{R} &= \mathbf{I} + A \boldsymbol{\omega}_{\times} + B \boldsymbol{\omega}_{\times}^2 \\
+    \mathbf{V} &= \mathbf{I} + B \boldsymbol{\omega}_{\times} + C \boldsymbol{\omega}_{\times}^2 \\
+    \exp \begin{pmatrix}
+    \mathbf{u} \\
+    \boldsymbol{\omega}
+    \end{pmatrix} &=
+    \left(
+    \begin{array}{c|c}
+    \mathbf{R} & \mathbf{V} \mathbf{u} \\ \hline
+    \mathbf{0} & 1
+    \end{array}
+    \right )
+    \end{align*}
+    */
+    double vx = twist.at<double>(0,0);
+    double vy = twist.at<double>(1,0);
+    double vz = twist.at<double>(2,0);
+    double wx = twist.at<double>(3,0);
+    double wy = twist.at<double>(4,0);
+    double wz = twist.at<double>(5,0);
+
+    Matx31d rvec(wx, wy, wz);
+    Mat R;
+    Rodrigues(rvec, R);
+
+    double theta = sqrt(wx*wx + wy*wy + wz*wz);
+    double sinc = std::fabs(theta) < 1e-8 ? 1 : sin(theta) / theta;
+    double mcosc = (std::fabs(theta) < 1e-8) ? 0.5 : (1-cos(theta)) / (theta*theta);
+    double msinc = (std::abs(theta) < 1e-8) ? (1/6.0) : (1-sinc) / (theta*theta);
+
+    Matx31d dt;
+    dt(0) = vx*(sinc + wx*wx*msinc) + vy*(wx*wy*msinc - wz*mcosc) + vz*(wx*wz*msinc + wy*mcosc);
+    dt(1) = vx*(wx*wy*msinc + wz*mcosc) + vy*(sinc + wy*wy*msinc) + vz*(wy*wz*msinc - wx*mcosc);
+    dt(2) = vx*(wx*wz*msinc - wy*mcosc) + vy*(wy*wz*msinc + wx*mcosc) + vz*(sinc + wz*wz*msinc);
+
+    R1 = R.t();
+    t1 = -R1 * dt;
+}
+
+enum SolvePnPRefineMethod {
+    SOLVEPNP_REFINE_LM   = 0,
+    SOLVEPNP_REFINE_VVS  = 1
+};
+
+static void solvePnPRefine(InputArray _objectPoints, InputArray _imagePoints,
+                           InputArray _cameraMatrix, InputArray _distCoeffs,
+                           InputOutputArray _rvec, InputOutputArray _tvec,
+                           SolvePnPRefineMethod _flags,
+                           TermCriteria _criteria=TermCriteria(TermCriteria::EPS+TermCriteria::COUNT, 20, FLT_EPSILON),
+                           double _vvslambda=1)
+{
+    CV_INSTRUMENT_REGION();
+
+    Mat opoints_ = _objectPoints.getMat(), ipoints_ = _imagePoints.getMat();
+    Mat opoints, ipoints;
+    opoints_.convertTo(opoints, CV_64F);
+    ipoints_.convertTo(ipoints, CV_64F);
+    int npoints = opoints.checkVector(3, CV_64F);
+    CV_Assert( npoints >= 3 && npoints == ipoints.checkVector(2, CV_64F) );
+    CV_Assert( !_rvec.empty() && !_tvec.empty() );
+
+    int rtype = _rvec.type(), ttype = _tvec.type();
+    Size rsize = _rvec.size(), tsize = _tvec.size();
+    CV_Assert( (rtype == CV_32FC1 || rtype == CV_64FC1) &&
+               (ttype == CV_32FC1 || ttype == CV_64FC1) );
+    CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) &&
+               (tsize == Size(1, 3) || tsize == Size(3, 1)) );
+
+    Mat cameraMatrix0 = _cameraMatrix.getMat();
+    Mat distCoeffs0 = _distCoeffs.getMat();
+    Mat cameraMatrix = Mat_<double>(cameraMatrix0);
+    Mat distCoeffs = Mat_<double>(distCoeffs0);
+
+    if (_flags == SOLVEPNP_REFINE_LM)
+    {
+        Mat rvec0 = _rvec.getMat(), tvec0 = _tvec.getMat();
+        Mat rvec, tvec;
+        rvec0.convertTo(rvec, CV_64F);
+        tvec0.convertTo(tvec, CV_64F);
+
+        Mat params(6, 1, CV_64FC1);
+        for (int i = 0; i < 3; i++)
+        {
+            params.at<double>(i,0) = rvec.at<double>(i,0);
+            params.at<double>(i+3,0) = tvec.at<double>(i,0);
+        }
+
+        createLMSolver(makePtr<SolvePnPRefineLMCallback>(opoints, ipoints, cameraMatrix, distCoeffs), _criteria.maxCount, _criteria.epsilon)->run(params);
+
+        params.rowRange(0, 3).convertTo(rvec0, rvec0.depth());
+        params.rowRange(3, 6).convertTo(tvec0, tvec0.depth());
+    }
+    else if (_flags == SOLVEPNP_REFINE_VVS)
+    {
+        Mat rvec0 = _rvec.getMat(), tvec0 = _tvec.getMat();
+        Mat rvec, tvec;
+        rvec0.convertTo(rvec, CV_64F);
+        tvec0.convertTo(tvec, CV_64F);
+
+        vector<Point2d> ipoints_normalized;
+        undistortPoints(ipoints, ipoints_normalized, cameraMatrix, distCoeffs);
+        Mat sd = Mat(ipoints_normalized).reshape(1, npoints*2);
+        Mat objectPoints0 = opoints.reshape(1, npoints);
+        Mat imagePoints0 = ipoints.reshape(1, npoints*2);
+        Mat L(npoints*2, 6, CV_64FC1), s(npoints*2, 1, CV_64FC1);
+
+        double residuals_1 = std::numeric_limits<double>::max(), residuals = 0;
+        Mat err;
+        Mat R;
+        Rodrigues(rvec, R);
+        for (int iter = 0; iter < _criteria.maxCount; iter++)
+        {
+            computeInteractionMatrixAndResiduals(objectPoints0, R, tvec, L, s);
+            err = s - sd;
+
+            Mat Lp = L.inv(cv::DECOMP_SVD);
+            Mat dq = -_vvslambda * Lp * err;
+
+            Mat R1, t1;
+            exponentialMapToSE3Inv(dq, R1, t1);
+            R = R1 * R;
+            tvec = R1 * tvec + t1;
+
+            residuals_1 = residuals;
+            Mat res = err.t()*err;
+            residuals = res.at<double>(0,0);
+
+            if (std::fabs(residuals - residuals_1) < _criteria.epsilon)
+                break;
+        }
+
+        Rodrigues(R, rvec);
+        rvec.convertTo(rvec0, rvec0.depth());
+        tvec.convertTo(tvec0, tvec0.depth());
+    }
+}
+
+void solvePnPRefineLM(InputArray _objectPoints, InputArray _imagePoints,
+                      InputArray _cameraMatrix, InputArray _distCoeffs,
+                      InputOutputArray _rvec, InputOutputArray _tvec,
+                      TermCriteria _criteria)
+{
+    CV_INSTRUMENT_REGION();
+    solvePnPRefine(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, SOLVEPNP_REFINE_LM, _criteria);
+}
+
+void solvePnPRefineVVS(InputArray _objectPoints, InputArray _imagePoints,
+                       InputArray _cameraMatrix, InputArray _distCoeffs,
+                       InputOutputArray _rvec, InputOutputArray _tvec,
+                       TermCriteria _criteria, double _VVSlambda)
+{
+    CV_INSTRUMENT_REGION();
+    solvePnPRefine(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, SOLVEPNP_REFINE_VVS, _criteria, _VVSlambda);
+}
+
 }