Merge pull request opencv#18371 from nathanrgodwin:sqpnp_dev

Added SQPnP algorithm to SolvePnP

* Added sqpnp

* Fixed test case

* Added fix for duplicate point checking and inverse func reuse

* Changes for 3x speedup

Changed norm method (significant speed increase), changed nearest rotation computation to FOAM

* Added symmetric 3x3 inverse and unrolled loops

* Fixed error with SVD

* Fixed error from with indices

Indices were initialized negative. When nullspace is large, points coplanar, and rotation near 0, indices not changed.

modules/calib3d/doc/calib3d.bib

@@ -39,3 +39,11 @@ @article{penate2013exhaustive
   year={2013},
   publisher={IEEE}
 }
+
+@inproceedings{Terzakis20,
+  author = {Terzakis, George and Lourakis, Manolis},
+  year = {2020},
+  month = {09},
+  pages = {},
+  title = {A Consistently Fast and Globally Optimal Solution to the Perspective-n-Point Problem}
+}

modules/calib3d/include/opencv2/calib3d.hpp

@@ -464,6 +464,7 @@ enum SolvePnPMethod {
                               //!<   - point 1: [ squareLength / 2,  squareLength / 2, 0]
                               //!<   - point 2: [ squareLength / 2, -squareLength / 2, 0]
                               //!<   - point 3: [-squareLength / 2, -squareLength / 2, 0]
+    SOLVEPNP_SQPNP       = 8, //!< SQPnP: A Consistently Fast and Globally OptimalSolution to the Perspective-n-Point Problem @cite Terzakis20
 #ifndef CV_DOXYGEN
     SOLVEPNP_MAX_COUNT        //!< Used for count
 #endif
@@ -835,6 +836,9 @@ It requires 4 coplanar object points defined in the following order:
   - point 1: [ squareLength / 2,  squareLength / 2, 0]
   - point 2: [ squareLength / 2, -squareLength / 2, 0]
   - point 3: [-squareLength / 2, -squareLength / 2, 0]
+-   **SOLVEPNP_SQPNP** Method is based on the paper "A Consistently Fast and Globally Optimal Solution to the
+Perspective-n-Point Problem" by G. Terzakis and M.Lourakis (@cite Terzakis20). It requires 3 or more points.
+
 
 The function estimates the object pose given a set of object points, their corresponding image
 projections, as well as the camera intrinsic matrix and the distortion coefficients, see the figure below
@@ -958,6 +962,7 @@ a 3D point expressed in the world frame into the camera frame:
          - point 1: [ squareLength / 2,  squareLength / 2, 0]
          - point 2: [ squareLength / 2, -squareLength / 2, 0]
          - point 3: [-squareLength / 2, -squareLength / 2, 0]
+    -  With **SOLVEPNP_SQPNP** input points must be >= 3
  */
 CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints,
                             InputArray cameraMatrix, InputArray distCoeffs,

modules/calib3d/src/solvepnp.cpp

@@ -47,6 +47,7 @@
 #include "p3p.h"
 #include "ap3p.h"
 #include "ippe.hpp"
+#include "sqpnp.hpp"
 #include "opencv2/calib3d/calib3d_c.h"
 #include <opencv2/core/utils/logger.hpp>
 
@@ -751,7 +752,8 @@ int solvePnPGeneric( InputArray _opoints, InputArray _ipoints,
 
     Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
     int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
-    CV_Assert( ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) )
+    CV_Assert( ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess)
+                || (npoints >= 3 && flags == SOLVEPNP_SQPNP) )
                && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
 
     opoints = opoints.reshape(3, npoints);
@@ -936,6 +938,14 @@ int solvePnPGeneric( InputArray _opoints, InputArray _ipoints,
             }
         } catch (...) { }
     }
+    else if (flags == SOLVEPNP_SQPNP)
+    {
+        Mat undistortedPoints;
+        undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
+
+        sqpnp::PoseSolver solver;
+        solver.solve(opoints, undistortedPoints, vec_rvecs, vec_tvecs);
+    }
     /*else if (flags == SOLVEPNP_DLS)
     {
         Mat undistortedPoints;
@@ -963,7 +973,8 @@ int solvePnPGeneric( InputArray _opoints, InputArray _ipoints,
         vec_tvecs.push_back(tvec);
     }*/
     else
-        CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS");
+        CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, "
+            "SOLVEPNP_EPNP, SOLVEPNP_DLS, SOLVEPNP_UPNP, SOLVEPNP_AP3P, SOLVEPNP_IPPE, SOLVEPNP_IPPE_SQUARE or SOLVEPNP_SQPNP");
 
     CV_Assert(vec_rvecs.size() == vec_tvecs.size());