diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index cac04c4869..ad5c85b222 100644 --- 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 index fb0e2965e6..43b90dff92 100644 --- 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_(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 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.,