Merge pull request opencv#10184 from catree:solvePnP_iterative_guess_3_pts

alalek · alalek · commit e703c3090a45 · 2017-11-29T18:24:04.000Z
diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp
@@ -563,7 +563,7 @@ Estimation" (@cite penate2013exhaustive). In this case the function also estimat
 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
+"An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (@cite Ke17). 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
@@ -585,9 +585,12 @@ projections, as well as the camera matrix and the distortion coefficients.
    -   The methods **SOLVEPNP_DLS** and **SOLVEPNP_UPNP** cannot be used as the current implementations are
        unstable and sometimes give completely wrong results. If you pass one of these two
        flags, **SOLVEPNP_EPNP** method will be used instead.
-   -   The minimum number of points is 4. In the case of **SOLVEPNP_P3P** and **SOLVEPNP_AP3P**
+   -   The minimum number of points is 4 in the general case. In the case of **SOLVEPNP_P3P** and **SOLVEPNP_AP3P**
        methods, it is required to use exactly 4 points (the first 3 points are used to estimate all the solutions
        of the P3P problem, the last one is used to retain the best solution that minimizes the reprojection error).
+   -   With **SOLVEPNP_ITERATIVE** method and `useExtrinsicGuess=true`, the minimum number of points is 3 (3 points
+       are sufficient to compute a pose but there are up to 4 solutions). The initial solution should be close to the
+       global solution to converge.
  */
 CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints,
                             InputArray cameraMatrix, InputArray distCoeffs,
@@ -658,9 +661,9 @@ the model coordinate system to the camera coordinate system. A P3P problem has u
 @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".
+"Complete Solution Classification for the Perspective-Three-Point Problem" (@cite gao2003complete).
 -   **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".
+"An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (@cite Ke17).
 
 The function estimates the object pose given 3 object points, their corresponding image
 projections, as well as the camera matrix and the distortion coefficients.
diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp
@@ -61,7 +61,8 @@ bool solvePnP( 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 == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
+    CV_Assert( ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) )
+               && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
 
     Mat rvec, tvec;
     if( flags != SOLVEPNP_ITERATIVE )
diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp
@@ -302,18 +302,15 @@ class CV_solveP3P_Test : public CV_solvePnPRansac_Test
     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) {
+    bool isTestSuccess = false;
+    double error = DBL_MAX;
+    for (unsigned int i = 0; i < rvecs.size() && !isTestSuccess; ++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);
+      isTestSuccess = rvecDiff < epsilon[method] && tvecDiff < epsilon[method];
+      error = std::min(error, std::max(rvecDiff, tvecDiff));
     }
-    bool isTestSuccess = min_rvecDiff < epsilon[method] && min_tvecDiff < epsilon[method];
 
-    double error = std::max(min_rvecDiff, min_tvecDiff);
     if (error > maxError)
       maxError = error;
 
@@ -324,7 +321,7 @@ class CV_solveP3P_Test : public CV_solvePnPRansac_Test
   {
     ts->set_failed_test_info(cvtest::TS::OK);
 
-    vector<Point3f> points, points_dls;
+    vector<Point3f> points;
     points.resize(pointsCount);
     generate3DPointCloud(points);
 
@@ -529,3 +526,68 @@ TEST(Calib3d_SolvePnP, translation)
     EXPECT_TRUE(checkRange(rvec));
     EXPECT_TRUE(checkRange(tvec));
 }
+
+TEST(Calib3d_SolvePnP, iterativeInitialGuess3pts)
+{
+    {
+        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<Point3d> 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_<double>(3,1) << 0.3, -0.2, 0.75);
+        Mat tvec_ground_truth = (Mat_<double>(3,1) << 0.15, -0.2, 1.5);
+
+        vector<Point2d> p2d;
+        projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d);
+
+        Mat rvec_est = (Mat_<double>(3,1) << 0.2, -0.1, 0.6);
+        Mat tvec_est = (Mat_<double>(3,1) << 0.05, -0.05, 1.0);
+
+        solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, true, SOLVEPNP_ITERATIVE);
+
+        std::cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
+        std::cout << "rvec_est: " << rvec_est.t() << std::endl;
+        std::cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
+        std::cout << "tvec_est: " << tvec_est.t() << std::endl;
+
+        EXPECT_LE(norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
+        EXPECT_LE(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<Point3f> 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_<float>(3,1) << -0.75f, 0.4f, 0.34f);
+        Mat tvec_ground_truth = (Mat_<float>(3,1) << -0.15f, 0.35f, 1.58f);
+
+        vector<Point2f> p2d;
+        projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d);
+
+        Mat rvec_est = (Mat_<float>(3,1) << -0.5f, 0.2f, 0.2f);
+        Mat tvec_est = (Mat_<float>(3,1) << 0.0f, 0.2f, 1.0f);
+
+        solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, true, SOLVEPNP_ITERATIVE);
+
+        std::cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
+        std::cout << "rvec_est: " << rvec_est.t() << std::endl;
+        std::cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
+        std::cout << "tvec_est: " << tvec_est.t() << std::endl;
+
+        EXPECT_LE(norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
+        EXPECT_LE(norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
+    }
+}
