From dac31e84fbacbbc5fab7e6a3ba078a4e01b9b468 Mon Sep 17 00:00:00 2001 From: catree Date: Fri, 26 Apr 2019 19:43:42 +0200 Subject: [PATCH] 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 | 55 ++- modules/calib3d/include/opencv2/calib3d.hpp | 59 ++++ modules/calib3d/src/levmarq.cpp | 9 +- modules/calib3d/src/precomp.hpp | 1 + modules/calib3d/src/solvepnp.cpp | 267 ++++++++++++++ modules/calib3d/test/test_solvepnp_ransac.cpp | 326 ++++++++++++++++++ 6 files changed, 711 insertions(+), 6 deletions(-) diff --git a/doc/opencv.bib b/doc/opencv.bib index e2af456532..fd1b60dfd1 100644 --- a/doc/opencv.bib +++ b/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}, diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 53b23d6035..16ab3342b7 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/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\ 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\ 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\ 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\ 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 diff --git a/modules/calib3d/src/levmarq.cpp b/modules/calib3d/src/levmarq.cpp index 0bec8680c6..a0172b2ecb 100644 --- a/modules/calib3d/src/levmarq.cpp +++ b/modules/calib3d/src/levmarq.cpp @@ -81,11 +81,11 @@ class LMSolverImpl CV_FINAL : public LMSolver { public: LMSolverImpl() : maxIters(100) { init(); } - LMSolverImpl(const Ptr& _cb, int _maxIters) : cb(_cb), maxIters(_maxIters) { init(); } + LMSolverImpl(const Ptr& _cb, int _maxIters) : cb(_cb), epsx(FLT_EPSILON), epsf(FLT_EPSILON), maxIters(_maxIters) { init(); } + LMSolverImpl(const Ptr& _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 createLMSolver(const Ptr& cb, int maxIters) return makePtr(cb, maxIters); } +Ptr createLMSolver(const Ptr& cb, int maxIters, double eps) +{ + return makePtr(cb, maxIters, eps); +} + } diff --git a/modules/calib3d/src/precomp.hpp b/modules/calib3d/src/precomp.hpp index 329692eb01..86bf9cc5cb 100644 --- a/modules/calib3d/src/precomp.hpp +++ b/modules/calib3d/src/precomp.hpp @@ -95,6 +95,7 @@ public: }; CV_EXPORTS Ptr createLMSolver(const Ptr& cb, int maxIters); +CV_EXPORTS Ptr createLMSolver(const Ptr& cb, int maxIters, double eps); class CV_EXPORTS PointSetRegistrator : public Algorithm { diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index b544595bb5..82a1604e46 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/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(i,j) = J.at(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(2,0); + double xi = objectPointsInCam.at(0,0) / Zi; + double yi = objectPointsInCam.at(1,0) / Zi; + + s.at(2*i,0) = xi; + s.at(2*i+1,0) = yi; + + L.at(2*i,0) = -1 / Zi; + L.at(2*i,1) = 0; + L.at(2*i,2) = xi / Zi; + L.at(2*i,3) = xi*yi; + L.at(2*i,4) = -(1 + xi*xi); + L.at(2*i,5) = yi; + + L.at(2*i+1,0) = 0; + L.at(2*i+1,1) = -1 / Zi; + L.at(2*i+1,2) = yi / Zi; + L.at(2*i+1,3) = 1 + yi*yi; + L.at(2*i+1,4) = -xi*yi; + L.at(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(0,0); + double vy = twist.at(1,0); + double vz = twist.at(2,0); + double wx = twist.at(3,0); + double wy = twist.at(4,0); + double wz = twist.at(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_(cameraMatrix0); + Mat distCoeffs = Mat_(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(i,0) = rvec.at(i,0); + params.at(i+3,0) = tvec.at(i,0); + } + + createLMSolver(makePtr(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 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::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(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); +} + } diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp index 2359fa9282..adf7758c92 100644 --- a/modules/calib3d/test/test_solvepnp_ransac.cpp +++ b/modules/calib3d/test/test_solvepnp_ransac.cpp @@ -589,4 +589,330 @@ TEST(Calib3d_SolvePnP, iterativeInitialGuess3pts) } } +TEST(Calib3d_SolvePnP, refine3pts) +{ + { + Matx33d intrinsics(605.4, 0.0, 317.35, + 0.0, 601.2, 242.63, + 0.0, 0.0, 1.0); + + double L = 0.1; + vector p3d; + p3d.push_back(Point3d(-L, -L, 0.0)); + p3d.push_back(Point3d(L, -L, 0.0)); + p3d.push_back(Point3d(L, L, 0.0)); + + Mat rvec_ground_truth = (Mat_(3,1) << 0.3, -0.2, 0.75); + Mat tvec_ground_truth = (Mat_(3,1) << 0.15, -0.2, 1.5); + + vector p2d; + projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d); + + { + Mat rvec_est = (Mat_(3,1) << 0.2, -0.1, 0.6); + Mat tvec_est = (Mat_(3,1) << 0.05, -0.05, 1.0); + + solvePnPRefineLM(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est); + + cout << "\nmethod: Levenberg-Marquardt" << endl; + cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl; + cout << "rvec_est: " << rvec_est.t() << std::endl; + cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl; + cout << "tvec_est: " << tvec_est.t() << std::endl; + + EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6); + EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6); + } + { + Mat rvec_est = (Mat_(3,1) << 0.2, -0.1, 0.6); + Mat tvec_est = (Mat_(3,1) << 0.05, -0.05, 1.0); + + solvePnPRefineVVS(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est); + + cout << "\nmethod: Virtual Visual Servoing" << endl; + cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl; + cout << "rvec_est: " << rvec_est.t() << std::endl; + cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl; + cout << "tvec_est: " << tvec_est.t() << std::endl; + + EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6); + EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6); + } + } + + { + Matx33f intrinsics(605.4f, 0.0f, 317.35f, + 0.0f, 601.2f, 242.63f, + 0.0f, 0.0f, 1.0f); + + float L = 0.1f; + vector p3d; + p3d.push_back(Point3f(-L, -L, 0.0f)); + p3d.push_back(Point3f(L, -L, 0.0f)); + p3d.push_back(Point3f(L, L, 0.0f)); + + Mat rvec_ground_truth = (Mat_(3,1) << -0.75f, 0.4f, 0.34f); + Mat tvec_ground_truth = (Mat_(3,1) << -0.15f, 0.35f, 1.58f); + + vector p2d; + projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d); + + { + Mat rvec_est = (Mat_(3,1) << -0.5f, 0.2f, 0.2f); + Mat tvec_est = (Mat_(3,1) << 0.0f, 0.2f, 1.0f); + + solvePnPRefineLM(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est); + + cout << "\nmethod: Levenberg-Marquardt" << endl; + cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl; + cout << "rvec_est: " << rvec_est.t() << std::endl; + cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl; + cout << "tvec_est: " << tvec_est.t() << std::endl; + + EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6); + EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6); + } + { + Mat rvec_est = (Mat_(3,1) << -0.5f, 0.2f, 0.2f); + Mat tvec_est = (Mat_(3,1) << 0.0f, 0.2f, 1.0f); + + solvePnPRefineVVS(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est); + + cout << "\nmethod: Virtual Visual Servoing" << endl; + cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl; + cout << "rvec_est: " << rvec_est.t() << std::endl; + cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl; + cout << "tvec_est: " << tvec_est.t() << std::endl; + + EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6); + EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6); + } + } +} + +TEST(Calib3d_SolvePnP, refine) +{ + //double + { + Matx33d intrinsics(605.4, 0.0, 317.35, + 0.0, 601.2, 242.63, + 0.0, 0.0, 1.0); + + double L = 0.1; + vector p3d; + p3d.push_back(Point3d(-L, -L, 0.0)); + p3d.push_back(Point3d(L, -L, 0.0)); + p3d.push_back(Point3d(L, L, 0.0)); + p3d.push_back(Point3d(-L, L, L/2)); + p3d.push_back(Point3d(0, 0, -L/2)); + + Mat rvec_ground_truth = (Mat_(3,1) << 0.3, -0.2, 0.75); + Mat tvec_ground_truth = (Mat_(3,1) << 0.15, -0.2, 1.5); + + vector p2d; + projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d); + + { + Mat rvec_est = (Mat_(3,1) << 0.1, -0.1, 0.1); + Mat tvec_est = (Mat_(3,1) << 0.0, -0.5, 1.0); + + solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, true, SOLVEPNP_ITERATIVE); + + cout << "\nmethod: Levenberg-Marquardt (C API)" << endl; + cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl; + cout << "rvec_est: " << rvec_est.t() << std::endl; + cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl; + cout << "tvec_est: " << tvec_est.t() << std::endl; + + EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6); + EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6); + } + { + Mat rvec_est = (Mat_(3,1) << 0.1, -0.1, 0.1); + Mat tvec_est = (Mat_(3,1) << 0.0, -0.5, 1.0); + + solvePnPRefineLM(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est); + + cout << "\nmethod: Levenberg-Marquardt (C++ API)" << endl; + cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl; + cout << "rvec_est: " << rvec_est.t() << std::endl; + cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl; + cout << "tvec_est: " << tvec_est.t() << std::endl; + + EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6); + EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6); + } + { + Mat rvec_est = (Mat_(3,1) << 0.1, -0.1, 0.1); + Mat tvec_est = (Mat_(3,1) << 0.0, -0.5, 1.0); + + solvePnPRefineVVS(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est); + + cout << "\nmethod: Virtual Visual Servoing" << endl; + cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl; + cout << "rvec_est: " << rvec_est.t() << std::endl; + cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl; + cout << "tvec_est: " << tvec_est.t() << std::endl; + + EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6); + EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6); + } + } + + //float + { + Matx33f intrinsics(605.4f, 0.0f, 317.35f, + 0.0f, 601.2f, 242.63f, + 0.0f, 0.0f, 1.0f); + + float L = 0.1f; + vector p3d; + p3d.push_back(Point3f(-L, -L, 0.0f)); + p3d.push_back(Point3f(L, -L, 0.0f)); + p3d.push_back(Point3f(L, L, 0.0f)); + p3d.push_back(Point3f(-L, L, L/2)); + p3d.push_back(Point3f(0, 0, -L/2)); + + Mat rvec_ground_truth = (Mat_(3,1) << -0.75f, 0.4f, 0.34f); + Mat tvec_ground_truth = (Mat_(3,1) << -0.15f, 0.35f, 1.58f); + + vector p2d; + projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d); + + { + Mat rvec_est = (Mat_(3,1) << -0.1f, 0.1f, 0.1f); + Mat tvec_est = (Mat_(3,1) << 0.0f, 0.0f, 1.0f); + + solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, true, SOLVEPNP_ITERATIVE); + + cout << "\nmethod: Levenberg-Marquardt (C API)" << endl; + cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl; + cout << "rvec_est: " << rvec_est.t() << std::endl; + cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl; + cout << "tvec_est: " << tvec_est.t() << std::endl; + + EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6); + EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6); + } + { + Mat rvec_est = (Mat_(3,1) << -0.1f, 0.1f, 0.1f); + Mat tvec_est = (Mat_(3,1) << 0.0f, 0.0f, 1.0f); + + solvePnPRefineLM(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est); + + cout << "\nmethod: Levenberg-Marquardt (C++ API)" << endl; + cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl; + cout << "rvec_est: " << rvec_est.t() << std::endl; + cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl; + cout << "tvec_est: " << tvec_est.t() << std::endl; + + EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6); + EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6); + } + { + Mat rvec_est = (Mat_(3,1) << -0.1f, 0.1f, 0.1f); + Mat tvec_est = (Mat_(3,1) << 0.0f, 0.0f, 1.0f); + + solvePnPRefineVVS(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est); + + cout << "\nmethod: Virtual Visual Servoing" << endl; + cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl; + cout << "rvec_est: " << rvec_est.t() << std::endl; + cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl; + cout << "tvec_est: " << tvec_est.t() << std::endl; + + EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6); + EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6); + } + } + + //refine after solvePnP + { + Matx33d intrinsics(605.4, 0.0, 317.35, + 0.0, 601.2, 242.63, + 0.0, 0.0, 1.0); + + double L = 0.1; + vector p3d; + p3d.push_back(Point3d(-L, -L, 0.0)); + p3d.push_back(Point3d(L, -L, 0.0)); + p3d.push_back(Point3d(L, L, 0.0)); + p3d.push_back(Point3d(-L, L, L/2)); + p3d.push_back(Point3d(0, 0, -L/2)); + + Mat rvec_ground_truth = (Mat_(3,1) << 0.3, -0.2, 0.75); + Mat tvec_ground_truth = (Mat_(3,1) << 0.15, -0.2, 1.5); + + vector p2d; + projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d); + + //add small Gaussian noise + RNG& rng = theRNG(); + for (size_t i = 0; i < p2d.size(); i++) + { + p2d[i].x += rng.gaussian(5e-2); + p2d[i].y += rng.gaussian(5e-2); + } + + Mat rvec_est, tvec_est; + solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, false, SOLVEPNP_EPNP); + + { + + Mat rvec_est_refine = rvec_est.clone(), tvec_est_refine = tvec_est.clone(); + solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est_refine, tvec_est_refine, true, SOLVEPNP_ITERATIVE); + + cout << "\nmethod: Levenberg-Marquardt (C API)" << endl; + cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl; + cout << "rvec_est (EPnP): " << rvec_est.t() << std::endl; + cout << "rvec_est_refine: " << rvec_est_refine.t() << std::endl; + cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl; + cout << "tvec_est (EPnP): " << tvec_est.t() << std::endl; + cout << "tvec_est_refine: " << tvec_est_refine.t() << std::endl; + + EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-2); + EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-3); + + EXPECT_LT(cvtest::norm(rvec_ground_truth, rvec_est_refine, NORM_INF), cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF)); + EXPECT_LT(cvtest::norm(tvec_ground_truth, tvec_est_refine, NORM_INF), cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF)); + } + { + Mat rvec_est_refine = rvec_est.clone(), tvec_est_refine = tvec_est.clone(); + solvePnPRefineLM(p3d, p2d, intrinsics, noArray(), rvec_est_refine, tvec_est_refine); + + cout << "\nmethod: Levenberg-Marquardt (C++ API)" << endl; + cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl; + cout << "rvec_est: " << rvec_est.t() << std::endl; + cout << "rvec_est_refine: " << rvec_est_refine.t() << std::endl; + cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl; + cout << "tvec_est: " << tvec_est.t() << std::endl; + cout << "tvec_est_refine: " << tvec_est_refine.t() << std::endl; + + EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-2); + EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-3); + + EXPECT_LT(cvtest::norm(rvec_ground_truth, rvec_est_refine, NORM_INF), cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF)); + EXPECT_LT(cvtest::norm(tvec_ground_truth, tvec_est_refine, NORM_INF), cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF)); + } + { + Mat rvec_est_refine = rvec_est.clone(), tvec_est_refine = tvec_est.clone(); + solvePnPRefineVVS(p3d, p2d, intrinsics, noArray(), rvec_est_refine, tvec_est_refine); + + cout << "\nmethod: Virtual Visual Servoing" << endl; + cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl; + cout << "rvec_est: " << rvec_est.t() << std::endl; + cout << "rvec_est_refine: " << rvec_est_refine.t() << std::endl; + cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl; + cout << "tvec_est: " << tvec_est.t() << std::endl; + cout << "tvec_est_refine: " << tvec_est_refine.t() << std::endl; + + EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-2); + EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-3); + + EXPECT_LT(cvtest::norm(rvec_ground_truth, rvec_est_refine, NORM_INF), cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF)); + EXPECT_LT(cvtest::norm(tvec_ground_truth, tvec_est_refine, NORM_INF), cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF)); + } + } +} + }} // namespace