Merge pull request opencv#19253 from mightbxg:bugfix_PnPRansac

* fix unexpected Exception in solvePnPRansac caused by input points * calib3d: solvePnPRansac - keep minimal changes to handle DLT 6 points requirement Co-authored-by: Alexander Alekhin <[email protected]>
figo · Mar 11, 2021 · b995de4 · b995de4
1 parent 825dadf
commit b995de4
Show file tree

Hide file tree

Showing 2 changed files with 66 additions and 5 deletions.
diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp
@@ -311,18 +311,42 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
 
     opoints_inliers.resize(npoints1);
     ipoints_inliers.resize(npoints1);
-    result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix,
-                      distCoeffs, rvec, tvec, useExtrinsicGuess,
-                      (flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P) ? SOLVEPNP_EPNP : flags) ? 1 : -1;
+    try
+    {
+        result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix,
+                          distCoeffs, rvec, tvec, useExtrinsicGuess,
+                          (flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P) ? SOLVEPNP_EPNP : flags) ? 1 : -1;
+    }
+    catch (const cv::Exception& e)
+    {
+        if (flags == SOLVEPNP_ITERATIVE &&
+            npoints1 == 5 &&
+            e.what() &&
+            std::string(e.what()).find("DLT algorithm needs at least 6 points") != std::string::npos
+        )
+        {
+            CV_LOG_INFO(NULL, "solvePnPRansac(): solvePnP stage to compute the final pose using points "
+                "in the consensus set raised DLT 6 points exception, use result from MSS (Minimal Sample Sets) stage instead.");
+            rvec = _local_model.col(0);    // output rotation vector
+            tvec = _local_model.col(1);    // output translation vector
+            result = 1;
+        }
+        else
+        {
+            // raise other exceptions
+            throw;
+        }
+    }
 
-    if( result <= 0 )
+    if (result <= 0)
     {
         _rvec.assign(_local_model.col(0));    // output rotation vector
         _tvec.assign(_local_model.col(1));    // output translation vector
 
-        if( _inliers.needed() )
+        if (_inliers.needed())
             _inliers.release();
 
+        CV_LOG_DEBUG(NULL, "solvePnPRansac(): solvePnP stage to compute the final pose using points in the consensus set failed. Return false");
         return false;
     }
     else

diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp
@@ -837,6 +837,43 @@ TEST(Calib3d_SolvePnPRansac, double_support)
     EXPECT_LE(cvtest::norm(t, Mat_<double>(tF), NORM_INF), 1e-3);
 }
 
+TEST(Calib3d_SolvePnPRansac, bad_input_points_19253)
+{
+    // with this specific data
+    // when computing the final pose using points in the consensus set with SOLVEPNP_ITERATIVE and solvePnP()
+    // an exception is thrown from solvePnP because there are 5 non-coplanar 3D points and the DLT algorithm needs at least 6 non-coplanar 3D points
+    // with PR #19253 we choose to return true, with the pose estimated from the MSS stage instead of throwing the exception
+
+    float pts2d_[] = {
+        -5.38358629e-01f, -5.09638414e-02f,
+        -5.07192254e-01f, -2.20743284e-01f,
+        -5.43107152e-01f, -4.90474701e-02f,
+        -5.54325163e-01f, -1.86715424e-01f,
+        -5.59334219e-01f, -4.01909500e-02f,
+        -5.43504596e-01f, -4.61776406e-02f
+    };
+    Mat pts2d(6, 2, CV_32FC1, pts2d_);
+
+    float pts3d_[] = {
+        -3.01153604e-02f, -1.55665115e-01f, 4.50000018e-01f,
+        4.27827090e-01f, 4.28645730e-01f, 1.08600008e+00f,
+        -3.14165242e-02f, -1.52656138e-01f, 4.50000018e-01f,
+        -1.46217480e-01f, 5.57961613e-02f, 7.17000008e-01f,
+        -4.89348806e-02f, -1.38795510e-01f, 4.47000027e-01f,
+        -3.13065052e-02f, -1.52636901e-01f, 4.51000035e-01f
+    };
+    Mat pts3d(6, 3, CV_32FC1, pts3d_);
+
+    Mat camera_mat = Mat::eye(3, 3, CV_64FC1);
+    Mat rvec, tvec;
+    vector<int> inliers;
+
+    // solvePnPRansac will return true with 5 inliers, which means the result is from MSS stage.
+    bool result = solvePnPRansac(pts3d, pts2d, camera_mat, noArray(), rvec, tvec, false, 100, 4.f / 460.f, 0.99, inliers);
+    EXPECT_EQ(inliers.size(), size_t(5));
+    EXPECT_TRUE(result);
+}
+
 TEST(Calib3d_SolvePnP, input_type)
 {
     Matx33d intrinsics(5.4794130238156129e+002, 0., 2.9835545700043139e+002, 0.,