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.

catree · catree · commit dac31e84fbac · 2019-04-30T14:31:11.000+02:00
diff --git a/doc/opencv.bib b/doc/opencv.bib
@@ -195,6 +195,21 @@ @article{ChambolleEtAl
   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 @@ @inproceedings{Davis97
   publisher = {IEEE},
   url = {https://alumni.media.mit.edu/~jdavis/Publications/publications_402.pdf}
 }
+@misc{Eade13,
+  author = {Eade, Ethan},
+  title = {Gauss-Newton / Levenberg-Marquardt Optimization},
+  year = {2013},
+  url = {https://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 @@ @misc{MA13
   title = {ROF and TV-L1 denoising with Primal-Dual algorithm},
   url = {https://znah.net/rof-and-tv-l1-denoising-with-primal-dual-algorithm.html}
 }
-@misc{VandLec,
-  author = {Vandenberghe, Lieven},
-  title = {QR Factorization},
-  url = {https://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 = {https://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf}
 }
 @article{MHT2011,
   author = {Getreuer, Pascal},
@@ -645,6 +670,23 @@ @article{Malis
   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 @@ @inproceedings{UES01
   volume = {2},
   publisher = {IEEE}
 }
+@misc{VandLec,
+  author = {Vandenberghe, Lieven},
+  title = {QR Factorization},
+  url = {https://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
@@ -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
diff --git 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<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);
+}
+
 }
diff --git a/modules/calib3d/src/precomp.hpp b/modules/calib3d/src/precomp.hpp
@@ -95,6 +95,7 @@ class CV_EXPORTS LMSolver : public Algorithm
 };
 
 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
 {
diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp
diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp

Original file line number	Diff line number	Diff line change
`@@ -81,11 +81,11 @@ class LMSolverImpl CV_FINAL : public LMSolver`
`81`	`81`	`{`
`82`	`82`	`public:`
`83`	`83`	`LMSolverImpl() : maxIters(100) { init(); }`
`84`		`- LMSolverImpl(const Ptr<LMSolver::Callback>& _cb, int _maxIters) : cb(_cb), maxIters(_maxIters) { init(); }`
	`84`	`+ LMSolverImpl(const Ptr<LMSolver::Callback>& _cb, int _maxIters) : cb(_cb), epsx(FLT_EPSILON), epsf(FLT_EPSILON), maxIters(_maxIters) { init(); }`
	`85`	`+ LMSolverImpl(const Ptr<LMSolver::Callback>& _cb, int _maxIters, double _eps) : cb(_cb), epsx(_eps), epsf(_eps), maxIters(_maxIters) { init(); }`
`85`	`86`
`86`	`87`	`void init()`
`87`	`88`	`{`
`88`		`- epsx = epsf = FLT_EPSILON;`
`89`	`89`	`printInterval = 0;`
`90`	`90`	`}`
`91`	`91`
`@@ -214,4 +214,9 @@ Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters)`
`214`	`214`	`return makePtr<LMSolverImpl>(cb, maxIters);`
`215`	`215`	`}`
`216`	`216`
	`217`	`+Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps)`
	`218`	`+{`
	`219`	`+ return makePtr<LMSolverImpl>(cb, maxIters, eps);`
	`220`	`+}`
	`221`	`+`
`217`	`222`	`}`
Original file line number	Diff line number	Diff line change
`@@ -95,6 +95,7 @@ class CV_EXPORTS LMSolver : public Algorithm`
`95`	`95`	`};`
`96`	`96`
`97`	`97`	`CV_EXPORTS Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters);`
	`98`	`+CV_EXPORTS Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps);`
`98`	`99`
`99`	`100`	`class CV_EXPORTS PointSetRegistrator : public Algorithm`
`100`	`101`	`{`