Merge pull request #8301 from tonyke1993:p3p_alg

New p3p algorithm (accepted by CVPR 2017) (#8301)

* add p3p source code

* indent 4

* update publication info

* fix filename

* interface done

* plug in done, test needed

* debugging

* for test

* a working version

* clean p3p code

* test

* test

* fix warning, blank line

* apply patch from @catree

* add reference info

* namespace, indent 4

* static solveQuartic

* put small functions to anonymous namespace

doc/opencv.bib

@@ -897,3 +897,10 @@
   year={2010},
   publisher={Springer}
 }
+@INPROCEEDINGS{Ke17,
+  author = {Ke, Tong and Roumeliotis, Stergios},
+  title = {An Efficient Algebraic Solution to the Perspective-Three-Point Problem},
+  booktitle = {Computer Vision and Pattern Recognition (CVPR), 2017 IEEE Conference on},
+  year = {2017},
+  organization = {IEEE}
+}

modules/calib3d/include/opencv2/calib3d.hpp

@@ -236,8 +236,8 @@ enum { SOLVEPNP_ITERATIVE = 0,
        SOLVEPNP_EPNP      = 1, //!< EPnP: Efficient Perspective-n-Point Camera Pose Estimation @cite lepetit2009epnp
        SOLVEPNP_P3P       = 2, //!< Complete Solution Classification for the Perspective-Three-Point Problem @cite gao2003complete
        SOLVEPNP_DLS       = 3, //!< A Direct Least-Squares (DLS) Method for PnP  @cite hesch2011direct
-       SOLVEPNP_UPNP      = 4  //!< Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation @cite penate2013exhaustive
-
+       SOLVEPNP_UPNP      = 4, //!< Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation @cite penate2013exhaustive
+       SOLVEPNP_AP3P      = 5  //!< An Efficient Algebraic Solution to the Perspective-Three-Point Problem @cite Ke17
 };
 
 enum { CALIB_CB_ADAPTIVE_THRESH = 1,

modules/calib3d/src/ap3p.h

+#ifndef P3P_P3P_H
+#define P3P_P3P_H
+
+#include "precomp.hpp"
+
+namespace cv {
+class ap3p {
+private:
+    template<typename T>
+    void init_camera_parameters(const cv::Mat &cameraMatrix) {
+        cx = cameraMatrix.at<T>(0, 2);
+        cy = cameraMatrix.at<T>(1, 2);
+        fx = cameraMatrix.at<T>(0, 0);
+        fy = cameraMatrix.at<T>(1, 1);
+    }
+
+    template<typename OpointType, typename IpointType>
+    void extract_points(const cv::Mat &opoints, const cv::Mat &ipoints, std::vector<double> &points) {
+        points.clear();
+        points.resize(20);
+        for (int i = 0; i < 4; i++) {
+            points[i * 5] = ipoints.at<IpointType>(i).x * fx + cx;
+            points[i * 5 + 1] = ipoints.at<IpointType>(i).y * fy + cy;
+            points[i * 5 + 2] = opoints.at<OpointType>(i).x;
+            points[i * 5 + 3] = opoints.at<OpointType>(i).y;
+            points[i * 5 + 4] = opoints.at<OpointType>(i).z;
+        }
+    }
+
+    void init_inverse_parameters();
+
+    double fx, fy, cx, cy;
+    double inv_fx, inv_fy, cx_fx, cy_fy;
+public:
+    ap3p() {}
+
+    ap3p(double fx, double fy, double cx, double cy);
+
+    ap3p(cv::Mat cameraMatrix);
+
+    bool solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Mat &ipoints);
+
+    int solve(double R[4][3][3], double t[4][3],
+              double mu0, double mv0, double X0, double Y0, double Z0,
+              double mu1, double mv1, double X1, double Y1, double Z1,
+              double mu2, double mv2, double X2, double Y2, double Z2);
+
+    bool solve(double R[3][3], double t[3],
+               double mu0, double mv0, double X0, double Y0, double Z0,
+               double mu1, double mv1, double X1, double Y1, double Z1,
+               double mu2, double mv2, double X2, double Y2, double Z2,
+               double mu3, double mv3, double X3, double Y3, double Z3);
+
+    // This algorithm is from "Tong Ke, Stergios Roumeliotis, An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (Accepted by CVPR 2017)
+    // See https://arxiv.org/pdf/1701.08237.pdf
+    // featureVectors: 3 bearing measurements (normalized) stored as column vectors
+    // worldPoints: Positions of the 3 feature points stored as column vectors
+    // solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame
+    // solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame
+    int computePoses(const double featureVectors[3][3], const double worldPoints[3][3], double solutionsR[4][3][3],
+                     double solutionsT[4][3]);
+
+};
+}
+#endif //P3P_P3P_H

modules/calib3d/src/solvepnp.cpp

@@ -45,6 +45,7 @@
 #include "dls.h"
 #include "epnp.h"
 #include "p3p.h"
+#include "ap3p.h"
 #include "opencv2/calib3d/calib3d_c.h"
 
 #include <iostream>
@@ -118,6 +119,18 @@ bool solvePnP( InputArray _opoints, InputArray _ipoints,
         if (result)
             Rodrigues(R, rvec);
     }
+    else if (flags == SOLVEPNP_AP3P)
+    {
+        CV_Assert( npoints == 4);
+        Mat undistortedPoints;
+        undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
+        ap3p P3Psolver(cameraMatrix);
+
+        Mat R;
+        result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
+        if (result)
+            Rodrigues(R, rvec);
+    }
     else if (flags == SOLVEPNP_ITERATIVE)
     {
         CvMat c_objectPoints = opoints, c_imagePoints = ipoints;
@@ -291,7 +304,8 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
         opoints_inliers.resize(npoints1);
         ipoints_inliers.resize(npoints1);
         result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix,
-                          distCoeffs, rvec, tvec, false, flags == SOLVEPNP_P3P ? SOLVEPNP_EPNP : flags) ? 1 : -1;
+                          distCoeffs, rvec, tvec, false,
+                          (flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P) ? SOLVEPNP_EPNP : flags) ? 1 : -1;
     }
 
     if( result <= 0 || _local_model.rows <= 0)

modules/calib3d/test/test_solvepnp_ransac.cpp

@@ -57,6 +57,7 @@ public:
         eps[SOLVEPNP_ITERATIVE] = 1.0e-2;
         eps[SOLVEPNP_EPNP] = 1.0e-2;
         eps[SOLVEPNP_P3P] = 1.0e-2;
+        eps[SOLVEPNP_AP3P] = 1.0e-2;
         eps[SOLVEPNP_DLS] = 1.0e-2;
         eps[SOLVEPNP_UPNP] = 1.0e-2;
         totalTestsCount = 10;
@@ -161,7 +162,7 @@ protected:
         points.resize(pointsCount);
         generate3DPointCloud(points);
 
-        const int methodsCount = 5;
+        const int methodsCount = 6;
         RNG rng = ts->get_rng();
 
 
@@ -189,7 +190,7 @@ protected:
             }
         }
     }
-    double eps[5];
+    double eps[6];
     int totalTestsCount;
 };
 
@@ -201,6 +202,7 @@ public:
         eps[SOLVEPNP_ITERATIVE] = 1.0e-6;
         eps[SOLVEPNP_EPNP] = 1.0e-6;
         eps[SOLVEPNP_P3P] = 1.0e-4;
+        eps[SOLVEPNP_AP3P] = 1.0e-4;
         eps[SOLVEPNP_DLS] = 1.0e-4;
         eps[SOLVEPNP_UPNP] = 1.0e-4;
         totalTestsCount = 1000;
@@ -222,7 +224,7 @@ protected:
         generatePose(trueRvec, trueTvec, rng);
 
         std::vector<Point3f> opoints;
-        if (method == 2)
+        if (method == 2 || method == 5)
         {
             opoints = std::vector<Point3f>(points.begin(), points.begin()+4);
         }