Merge pull request #8585 from tonyke1993:ap3p

Enable p3p and ap3p in solvePnPRansac (#8585)

* add paper info

* allow p3p and ap3p being RANSAC kernel

* keep previous code

* apply catrees comment

* fix getMat

* add comment

* add solvep3p test

* test return value

* fix warnings

modules/calib3d/include/opencv2/calib3d.hpp

@@ -561,6 +561,9 @@ F.Moreno-Noguer. "Exhaustive Linearization for Robust Camera Pose and Focal Leng
 Estimation" (@cite penate2013exhaustive). In this case the function also estimates the parameters \f$f_x\f$ and \f$f_y\f$
 assuming that both have the same value. Then the cameraMatrix is updated with the estimated
 focal length.
+-   **SOLVEPNP_AP3P** Method is based on the paper of Tong Ke and Stergios I. Roumeliotis.
+"An Efficient Algebraic Solution to the Perspective-Three-Point Problem". In this case the
+function requires exactly four object and image points.
 
 The function estimates the object pose given a set of object points, their corresponding image
 projections, as well as the camera matrix and the distortion coefficients.
@@ -631,6 +634,33 @@ CV_EXPORTS_W bool solvePnPRansac( InputArray objectPoints, InputArray imagePoint
                                   bool useExtrinsicGuess = false, int iterationsCount = 100,
                                   float reprojectionError = 8.0, double confidence = 0.99,
                                   OutputArray inliers = noArray(), int flags = SOLVEPNP_ITERATIVE );
+/** @brief Finds an object pose from 3 3D-2D point correspondences.
+
+@param objectPoints Array of object points in the object coordinate space, 3x3 1-channel or
+1x3/3x1 3-channel. vector\<Point3f\> can be also passed here.
+@param imagePoints Array of corresponding image points, 3x2 1-channel or 1x3/3x1 2-channel.
+ vector\<Point2f\> can be also 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 rvecs Output rotation vectors (see Rodrigues ) that, together with tvecs , brings points from
+the model coordinate system to the camera coordinate system. A P3P problem has up to 4 solutions.
+@param tvecs Output translation vectors.
+@param flags Method for solving a P3P problem:
+-   **SOLVEPNP_P3P** Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang
+"Complete Solution Classification for the Perspective-Three-Point Problem".
+-   **SOLVEPNP_AP3P** Method is based on the paper of Tong Ke and Stergios I. Roumeliotis.
+"An Efficient Algebraic Solution to the Perspective-Three-Point Problem".
+
+The function estimates the object pose given 3 object points, their corresponding image
+projections, as well as the camera matrix and the distortion coefficients.
+ */
+CV_EXPORTS_W int solveP3P( InputArray objectPoints, InputArray imagePoints,
+                           InputArray cameraMatrix, InputArray distCoeffs,
+                           OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
+                           int flags );
 
 /** @brief Finds an initial camera matrix from 3D-2D point correspondences.
 

modules/calib3d/src/ap3p.cpp

@@ -313,6 +313,38 @@ bool ap3p::solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Ma
     return result;
 }
 
+int ap3p::solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, const cv::Mat &opoints, const cv::Mat &ipoints) {
+    CV_INSTRUMENT_REGION()
+
+    double rotation_matrix[4][3][3], translation[4][3];
+    std::vector<double> points;
+    if (opoints.depth() == ipoints.depth()) {
+        if (opoints.depth() == CV_32F)
+            extract_points<cv::Point3f, cv::Point2f>(opoints, ipoints, points);
+        else
+            extract_points<cv::Point3d, cv::Point2d>(opoints, ipoints, points);
+    } else if (opoints.depth() == CV_32F)
+        extract_points<cv::Point3f, cv::Point2d>(opoints, ipoints, points);
+    else
+        extract_points<cv::Point3d, cv::Point2f>(opoints, ipoints, points);
+
+    int solutions = solve(rotation_matrix, translation,
+                          points[0], points[1], points[2], points[3], points[4],
+                          points[5], points[6], points[7], points[8], points[9],
+                          points[10], points[11], points[12], points[13], points[14]);
+
+    for (int i = 0; i < solutions; i++) {
+        cv::Mat R, tvec;
+        cv::Mat(3, 1, CV_64F, translation[i]).copyTo(tvec);
+        cv::Mat(3, 3, CV_64F, rotation_matrix[i]).copyTo(R);
+
+        Rs.push_back(R);
+        tvecs.push_back(tvec);
+    }
+
+    return solutions;
+}
+
 bool
 ap3p::solve(double R[3][3], double t[3], double mu0, double mv0, double X0, double Y0, double Z0, double mu1,
             double mv1,

modules/calib3d/src/ap3p.h

@@ -17,8 +17,9 @@ private:
     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++) {
+        int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
+        points.resize(5*npoints);
+        for (int i = 0; i < npoints; 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;
@@ -39,6 +40,7 @@ public:
     ap3p(cv::Mat cameraMatrix);
 
     bool solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Mat &ipoints);
+    int solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, 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,

modules/calib3d/src/p3p.cpp

@@ -57,6 +57,41 @@ bool p3p::solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat
     return result;
 }
 
+int p3p::solve(std::vector<cv::Mat>& Rs, std::vector<cv::Mat>& tvecs, const cv::Mat& opoints, const cv::Mat& ipoints)
+{
+    CV_INSTRUMENT_REGION()
+
+    double rotation_matrix[4][3][3], translation[4][3];
+    std::vector<double> points;
+    if (opoints.depth() == ipoints.depth())
+    {
+        if (opoints.depth() == CV_32F)
+            extract_points<cv::Point3f,cv::Point2f>(opoints, ipoints, points);
+        else
+            extract_points<cv::Point3d,cv::Point2d>(opoints, ipoints, points);
+    }
+    else if (opoints.depth() == CV_32F)
+        extract_points<cv::Point3f,cv::Point2d>(opoints, ipoints, points);
+    else
+        extract_points<cv::Point3d,cv::Point2f>(opoints, ipoints, points);
+
+    int solutions = solve(rotation_matrix, translation,
+                          points[0], points[1], points[2], points[3], points[4],
+                          points[5], points[6], points[7], points[8], points[9],
+                          points[10], points[11], points[12], points[13], points[14]);
+
+    for (int i = 0; i < solutions; i++) {
+        cv::Mat R, tvec;
+        cv::Mat(3, 1, CV_64F, translation[i]).copyTo(tvec);
+        cv::Mat(3, 3, CV_64F, rotation_matrix[i]).copyTo(R);
+
+        Rs.push_back(R);
+        tvecs.push_back(tvec);
+    }
+
+    return solutions;
+}
+
 bool p3p::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,

modules/calib3d/src/p3p.h

@@ -11,6 +11,7 @@ class p3p
   p3p(cv::Mat cameraMatrix);
 
   bool solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat& ipoints);
+  int solve(std::vector<cv::Mat>& Rs, std::vector<cv::Mat>& tvecs, 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,
@@ -34,8 +35,9 @@ class p3p
   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++)
+      int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
+      points.resize(5*npoints);
+      for(int i = 0; i < npoints; i++)
       {
           points[i*5] = ipoints.at<IpointType>(i).x*fx + cx;
           points[i*5+1] = ipoints.at<IpointType>(i).y*fy + cy;

modules/calib3d/src/solvepnp.cpp

@@ -268,7 +268,12 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
     int model_points = 5;
     int ransac_kernel_method = SOLVEPNP_EPNP;
 
-    if( npoints == 4 )
+    if( flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P)
+    {
+        model_points = 4;
+        ransac_kernel_method = flags;
+    }
+    else if( npoints == 4 )
     {
         model_points = 4;
         ransac_kernel_method = SOLVEPNP_P3P;
@@ -337,4 +342,57 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
     return true;
 }
 
+int solveP3P( InputArray _opoints, InputArray _ipoints,
+              InputArray _cameraMatrix, InputArray _distCoeffs,
+              OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, int flags) {
+    CV_INSTRUMENT_REGION()
+
+    Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
+    int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
+    CV_Assert( npoints == 3 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
+    CV_Assert( flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P );
+
+    Mat cameraMatrix0 = _cameraMatrix.getMat();
+    Mat distCoeffs0 = _distCoeffs.getMat();
+    Mat cameraMatrix = Mat_<double>(cameraMatrix0);
+    Mat distCoeffs = Mat_<double>(distCoeffs0);
+
+    Mat undistortedPoints;
+    undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
+    std::vector<Mat> Rs, ts;
+
+    int solutions = 0;
+    if (flags == SOLVEPNP_P3P)
+    {
+        p3p P3Psolver(cameraMatrix);
+        solutions = P3Psolver.solve(Rs, ts, opoints, undistortedPoints);
+    }
+    else if (flags == SOLVEPNP_AP3P)
+    {
+        ap3p P3Psolver(cameraMatrix);
+        solutions = P3Psolver.solve(Rs, ts, opoints, undistortedPoints);
+    }
+
+    if (solutions == 0) {
+        return 0;
+    }
+
+    if (_rvecs.needed()) {
+        _rvecs.create(solutions, 1, CV_64F);
+    }
+
+    if (_tvecs.needed()) {
+        _tvecs.create(solutions, 1, CV_64F);
+    }
+
+    for (int i = 0; i < solutions; i++) {
+        Mat rvec;
+        Rodrigues(Rs[i], rvec);
+        _tvecs.getMatRef(i) = ts[i];
+        _rvecs.getMatRef(i) = rvec;
+    }
+
+    return solutions;
+}
+
 }

modules/calib3d/test/test_solvepnp_ransac.cpp

@@ -153,7 +153,7 @@ protected:
         return isTestSuccess;
     }
 
-    void run(int)
+    virtual void run(int)
     {
         ts->set_failed_test_info(cvtest::TS::OK);
 
@@ -253,6 +253,100 @@ protected:
     }
 };
 
+class CV_solveP3P_Test : public CV_solvePnPRansac_Test
+{
+ public:
+  CV_solveP3P_Test()
+  {
+    eps[SOLVEPNP_P3P] = 1.0e-4;
+    eps[SOLVEPNP_AP3P] = 1.0e-4;
+    totalTestsCount = 1000;
+  }
+
+  ~CV_solveP3P_Test() {}
+ protected:
+  virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, const double* epsilon, double& maxError)
+  {
+    std::vector<Mat> rvecs, tvecs;
+    Mat trueRvec, trueTvec;
+    Mat intrinsics, distCoeffs;
+    generateCameraMatrix(intrinsics, rng);
+    if (mode == 0)
+      distCoeffs = Mat::zeros(4, 1, CV_64FC1);
+    else
+      generateDistCoeffs(distCoeffs, rng);
+    generatePose(trueRvec, trueTvec, rng);
+
+    std::vector<Point3f> opoints;
+    opoints = std::vector<Point3f>(points.begin(), points.begin()+3);
+
+    vector<Point2f> projectedPoints;
+    projectedPoints.resize(opoints.size());
+    projectPoints(Mat(opoints), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);
+
+    int num_of_solutions = solveP3P(opoints, projectedPoints, intrinsics, distCoeffs, rvecs, tvecs, method);
+    if (num_of_solutions != (int) rvecs.size() || num_of_solutions != (int) tvecs.size() || num_of_solutions == 0)
+      return false;
+
+    double min_rvecDiff = DBL_MAX, min_tvecDiff = DBL_MAX;
+    for (unsigned int i = 0; i < rvecs.size(); ++i) {
+      double rvecDiff = norm(rvecs[i]-trueRvec);
+      min_rvecDiff = std::min(rvecDiff, min_rvecDiff);
+    }
+    for (unsigned int i = 0; i < tvecs.size(); ++i) {
+      double tvecDiff = norm(tvecs[i]-trueTvec);
+      min_tvecDiff = std::min(tvecDiff, min_tvecDiff);
+    }
+    bool isTestSuccess = min_rvecDiff < epsilon[method] && min_tvecDiff < epsilon[method];
+
+    double error = std::max(min_rvecDiff, min_tvecDiff);
+    if (error > maxError)
+      maxError = error;
+
+    return isTestSuccess;
+  }
+
+  virtual void run(int)
+  {
+    ts->set_failed_test_info(cvtest::TS::OK);
+
+    vector<Point3f> points, points_dls;
+    const int pointsCount = 500;
+    points.resize(pointsCount);
+    generate3DPointCloud(points);
+
+    const int methodsCount = 2;
+    int methods[methodsCount] = {SOLVEPNP_P3P, SOLVEPNP_AP3P};
+    RNG rng = ts->get_rng();
+
+    for (int mode = 0; mode < 2; mode++)
+    {
+      for (int method = 0; method < methodsCount; method++)
+      {
+        double maxError = 0;
+        int successfulTestsCount = 0;
+        for (int testIndex = 0; testIndex < totalTestsCount; testIndex++)
+        {
+          if (runTest(rng, mode, methods[method], points, eps, maxError))
+            successfulTestsCount++;
+        }
+        //cout <<  maxError << " " << successfulTestsCount << endl;
+        if (successfulTestsCount < 0.7*totalTestsCount)
+        {
+          ts->printf( cvtest::TS::LOG, "Invalid accuracy for method %d, failed %d tests from %d, maximum error equals %f, distortion mode equals %d\n",
+                      method, totalTestsCount - successfulTestsCount, totalTestsCount, maxError, mode);
+          ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
+        }
+        cout << "mode: " << mode << ", method: " << method << " -> "
+             << ((double)successfulTestsCount / totalTestsCount) * 100 << "%"
+             << " (err < " << maxError << ")" << endl;
+      }
+    }
+  }
+};
+
+
+TEST(Calib3d_SolveP3P, accuracy) { CV_solveP3P_Test test; test.safe_run();}
 TEST(Calib3d_SolvePnPRansac, accuracy) { CV_solvePnPRansac_Test test; test.safe_run(); }
 TEST(Calib3d_SolvePnP, accuracy) { CV_solvePnP_Test test; test.safe_run(); }