From 8088d6785a753f6ac8aca6f8ccbe25ffcb2a4bc0 Mon Sep 17 00:00:00 2001 From: Tong Ke Date: Wed, 28 Jun 2017 05:27:30 -0700 Subject: [PATCH] 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 | 30 ++++++ modules/calib3d/src/ap3p.cpp | 34 ++++++- modules/calib3d/src/ap3p.h | 6 +- modules/calib3d/src/p3p.cpp | 35 +++++++ modules/calib3d/src/p3p.h | 6 +- modules/calib3d/src/solvepnp.cpp | 60 +++++++++++- modules/calib3d/test/test_solvepnp_ransac.cpp | 96 ++++++++++++++++++- 7 files changed, 260 insertions(+), 7 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index c151c6c3b4..0cc265ff44 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/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\ can be also passed here. +@param imagePoints Array of corresponding image points, 3x2 1-channel or 1x3/3x1 2-channel. + vector\ 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. diff --git a/modules/calib3d/src/ap3p.cpp b/modules/calib3d/src/ap3p.cpp index db88a02d04..6aa771dcba 100644 --- a/modules/calib3d/src/ap3p.cpp +++ b/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 &Rs, std::vector &tvecs, const cv::Mat &opoints, const cv::Mat &ipoints) { + CV_INSTRUMENT_REGION() + + double rotation_matrix[4][3][3], translation[4][3]; + std::vector points; + if (opoints.depth() == ipoints.depth()) { + if (opoints.depth() == CV_32F) + extract_points(opoints, ipoints, points); + else + extract_points(opoints, ipoints, points); + } else if (opoints.depth() == CV_32F) + extract_points(opoints, ipoints, points); + else + extract_points(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, @@ -383,4 +415,4 @@ int ap3p::solve(double R[4][3][3], double t[4][3], double mu0, double mv0, doubl return computePoses(featureVectors, worldPoints, R, t); } -} \ No newline at end of file +} diff --git a/modules/calib3d/src/ap3p.h b/modules/calib3d/src/ap3p.h index 37c7be12b4..241e3e681b 100644 --- a/modules/calib3d/src/ap3p.h +++ b/modules/calib3d/src/ap3p.h @@ -17,8 +17,9 @@ private: template void extract_points(const cv::Mat &opoints, const cv::Mat &ipoints, std::vector &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(i).x * fx + cx; points[i * 5 + 1] = ipoints.at(i).y * fy + cy; points[i * 5 + 2] = opoints.at(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 &Rs, std::vector &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, diff --git a/modules/calib3d/src/p3p.cpp b/modules/calib3d/src/p3p.cpp index f91925b219..8bf0c35945 100644 --- a/modules/calib3d/src/p3p.cpp +++ b/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& Rs, std::vector& tvecs, const cv::Mat& opoints, const cv::Mat& ipoints) +{ + CV_INSTRUMENT_REGION() + + double rotation_matrix[4][3][3], translation[4][3]; + std::vector points; + if (opoints.depth() == ipoints.depth()) + { + if (opoints.depth() == CV_32F) + extract_points(opoints, ipoints, points); + else + extract_points(opoints, ipoints, points); + } + else if (opoints.depth() == CV_32F) + extract_points(opoints, ipoints, points); + else + extract_points(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, diff --git a/modules/calib3d/src/p3p.h b/modules/calib3d/src/p3p.h index 00d99ae36e..9c7f7ec987 100644 --- a/modules/calib3d/src/p3p.h +++ b/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& Rs, std::vector& 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& 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(i).x*fx + cx; points[i*5+1] = ipoints.at(i).y*fy + cy; diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 60d74f415a..b9bc4ae5b3 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/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_(cameraMatrix0); + Mat distCoeffs = Mat_(distCoeffs0); + + Mat undistortedPoints; + undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); + std::vector 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; +} + } diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp index 86634a0b45..69d1e32f64 100644 --- a/modules/calib3d/test/test_solvepnp_ransac.cpp +++ b/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& points, const double* epsilon, double& maxError) + { + std::vector 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 opoints; + opoints = std::vector(points.begin(), points.begin()+3); + + vector 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 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(); }