From 2e56a47f8cdac7cad9b530a2efad8b8517bfd5ea Mon Sep 17 00:00:00 2001 From: catree Date: Tue, 28 Nov 2017 15:04:59 +0100 Subject: [PATCH] Allow to use 3 points in SolvePnP if SOLVEPNP_ITERATIVE and useExtrinsicGuess==true. Add bibtex citations for P3P. Update SolvPnP tests. --- modules/calib3d/include/opencv2/calib3d.hpp | 11 ++-- modules/calib3d/src/solvepnp.cpp | 3 +- modules/calib3d/test/test_solvepnp_ransac.cpp | 80 ++++++++++++++++++++++++--- 3 files changed, 80 insertions(+), 14 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index d8f4f9c..8d6fedf 100644 --- 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 index a4420f2..0f4687c 100644 --- 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 index 0adbbc9..a7cc6ff 100644 --- 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 points, points_dls; + vector 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 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_(3,1) << 0.3, -0.2, 0.75); + Mat tvec_ground_truth = (Mat_(3,1) << 0.15, -0.2, 1.5); + + vector p2d; + projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d); + + Mat rvec_est = (Mat_(3,1) << 0.2, -0.1, 0.6); + Mat tvec_est = (Mat_(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 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_(3,1) << -0.75f, 0.4f, 0.34f); + Mat tvec_ground_truth = (Mat_(3,1) << -0.15f, 0.35f, 1.58f); + + vector p2d; + projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d); + + Mat rvec_est = (Mat_(3,1) << -0.5f, 0.2f, 0.2f); + Mat tvec_est = (Mat_(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); + } +} -- 2.7.4