From 0a63ab36bb10b133ded9496aca0058ff630fa2a8 Mon Sep 17 00:00:00 2001 From: Tong Ke Date: Fri, 7 Apr 2017 01:48:34 -0500 Subject: [PATCH] Merge pull request #8301 from tonyke1993:p3p_alg New p3p algorithm (accepted by CVPR 2017) (#8301) * add p3p source code * indent 4 * update publication info * fix filename * interface done * plug in done, test needed * debugging * for test * a working version * clean p3p code * test * test * fix warning, blank line * apply patch from @catree * add reference info * namespace, indent 4 * static solveQuartic * put small functions to anonymous namespace --- doc/opencv.bib | 7 + modules/calib3d/include/opencv2/calib3d.hpp | 4 +- modules/calib3d/src/ap3p.cpp | 383 ++++++++++++++++++ modules/calib3d/src/ap3p.h | 65 +++ modules/calib3d/src/solvepnp.cpp | 16 +- modules/calib3d/test/test_solvepnp_ransac.cpp | 8 +- 6 files changed, 477 insertions(+), 6 deletions(-) create mode 100644 modules/calib3d/src/ap3p.cpp create mode 100644 modules/calib3d/src/ap3p.h diff --git a/doc/opencv.bib b/doc/opencv.bib index 29a2ae4512..f4bb2512d1 100644 --- a/doc/opencv.bib +++ b/doc/opencv.bib @@ -897,3 +897,10 @@ year={2010}, publisher={Springer} } +@INPROCEEDINGS{Ke17, + author = {Ke, Tong and Roumeliotis, Stergios}, + title = {An Efficient Algebraic Solution to the Perspective-Three-Point Problem}, + booktitle = {Computer Vision and Pattern Recognition (CVPR), 2017 IEEE Conference on}, + year = {2017}, + organization = {IEEE} +} diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index b5a56ec1b6..c58daab027 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -236,8 +236,8 @@ enum { SOLVEPNP_ITERATIVE = 0, SOLVEPNP_EPNP = 1, //!< EPnP: Efficient Perspective-n-Point Camera Pose Estimation @cite lepetit2009epnp SOLVEPNP_P3P = 2, //!< Complete Solution Classification for the Perspective-Three-Point Problem @cite gao2003complete SOLVEPNP_DLS = 3, //!< A Direct Least-Squares (DLS) Method for PnP @cite hesch2011direct - SOLVEPNP_UPNP = 4 //!< Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation @cite penate2013exhaustive - + SOLVEPNP_UPNP = 4, //!< Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation @cite penate2013exhaustive + SOLVEPNP_AP3P = 5 //!< An Efficient Algebraic Solution to the Perspective-Three-Point Problem @cite Ke17 }; enum { CALIB_CB_ADAPTIVE_THRESH = 1, diff --git a/modules/calib3d/src/ap3p.cpp b/modules/calib3d/src/ap3p.cpp new file mode 100644 index 0000000000..ad2841fe98 --- /dev/null +++ b/modules/calib3d/src/ap3p.cpp @@ -0,0 +1,383 @@ +#include "ap3p.h" + +#include +#include + +using namespace std; + +namespace { +void solveQuartic(const double *factors, double *realRoots) { + const double &a4 = factors[0]; + const double &a3 = factors[1]; + const double &a2 = factors[2]; + const double &a1 = factors[3]; + const double &a0 = factors[4]; + + double a4_2 = a4 * a4; + double a3_2 = a3 * a3; + double a4_3 = a4_2 * a4; + double a2a4 = a2 * a4; + + double p4 = (8 * a2a4 - 3 * a3_2) / (8 * a4_2); + double q4 = (a3_2 * a3 - 4 * a2a4 * a3 + 8 * a1 * a4_2) / (8 * a4_3); + double r4 = (256 * a0 * a4_3 - 3 * (a3_2 * a3_2) - 64 * a1 * a3 * a4_2 + 16 * a2a4 * a3_2) / (256 * (a4_3 * a4)); + + double p3 = ((p4 * p4) / 12 + r4) / 3; // /=-3 + double q3 = (72 * r4 * p4 - 2 * p4 * p4 * p4 - 27 * q4 * q4) / 432; // /=2 + + double t; // *=2 + complex w; + if (q3 >= 0) + w = -sqrt(static_cast >(q3 * q3 - p3 * p3 * p3)) - q3; + else + w = sqrt(static_cast >(q3 * q3 - p3 * p3 * p3)) - q3; + if (w.imag() == 0.0) { + w.real(cbrt(w.real())); + t = 2.0 * (w.real() + p3 / w.real()); + } else { + w = pow(w, 1.0 / 3); + t = 4.0 * w.real(); + } + + complex sqrt_2m = sqrt(static_cast >(-2 * p4 / 3 + t)); + double B_4A = -a3 / (4 * a4); + double complex1 = 4 * p4 / 3 + t; + complex complex2 = 2 * q4 / sqrt_2m; + double sqrt_2m_rh = sqrt_2m.real() / 2; + double sqrt1 = sqrt(-(complex1 + complex2)).real() / 2; + realRoots[0] = B_4A + sqrt_2m_rh + sqrt1; + realRoots[1] = B_4A + sqrt_2m_rh - sqrt1; + double sqrt2 = sqrt(-(complex1 - complex2)).real() / 2; + realRoots[2] = B_4A - sqrt_2m_rh + sqrt2; + realRoots[3] = B_4A - sqrt_2m_rh - sqrt2; +} + +void polishQuarticRoots(const double *coeffs, double *roots) { + const int iterations = 2; + for (int i = 0; i < iterations; ++i) { + for (int j = 0; j < 4; ++j) { + double error = + (((coeffs[0] * roots[j] + coeffs[1]) * roots[j] + coeffs[2]) * roots[j] + coeffs[3]) * roots[j] + + coeffs[4]; + double + derivative = + ((4 * coeffs[0] * roots[j] + 3 * coeffs[1]) * roots[j] + 2 * coeffs[2]) * roots[j] + coeffs[3]; + roots[j] -= error / derivative; + } + } +} + +inline void vect_cross(const double *a, const double *b, double *result) { + result[0] = a[1] * b[2] - a[2] * b[1]; + result[1] = -(a[0] * b[2] - a[2] * b[0]); + result[2] = a[0] * b[1] - a[1] * b[0]; +} + +inline double vect_dot(const double *a, const double *b) { + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; +} + +inline double vect_norm(const double *a) { + return sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]); +} + +inline void vect_scale(const double s, const double *a, double *result) { + result[0] = a[0] * s; + result[1] = a[1] * s; + result[2] = a[2] * s; +} + +inline void vect_sub(const double *a, const double *b, double *result) { + result[0] = a[0] - b[0]; + result[1] = a[1] - b[1]; + result[2] = a[2] - b[2]; +} + +inline void vect_divide(const double *a, const double d, double *result) { + result[0] = a[0] / d; + result[1] = a[1] / d; + result[2] = a[2] / d; +} + +inline void mat_mult(const double a[3][3], const double b[3][3], double result[3][3]) { + result[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0] + a[0][2] * b[2][0]; + result[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1] + a[0][2] * b[2][1]; + result[0][2] = a[0][0] * b[0][2] + a[0][1] * b[1][2] + a[0][2] * b[2][2]; + + result[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0] + a[1][2] * b[2][0]; + result[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1] + a[1][2] * b[2][1]; + result[1][2] = a[1][0] * b[0][2] + a[1][1] * b[1][2] + a[1][2] * b[2][2]; + + result[2][0] = a[2][0] * b[0][0] + a[2][1] * b[1][0] + a[2][2] * b[2][0]; + result[2][1] = a[2][0] * b[0][1] + a[2][1] * b[1][1] + a[2][2] * b[2][1]; + result[2][2] = a[2][0] * b[0][2] + a[2][1] * b[1][2] + a[2][2] * b[2][2]; +} +} + +namespace cv { +void ap3p::init_inverse_parameters() { + inv_fx = 1. / fx; + inv_fy = 1. / fy; + cx_fx = cx / fx; + cy_fy = cy / fy; +} + +ap3p::ap3p(cv::Mat cameraMatrix) { + if (cameraMatrix.depth() == CV_32F) + init_camera_parameters(cameraMatrix); + else + init_camera_parameters(cameraMatrix); + init_inverse_parameters(); +} + +ap3p::ap3p(double _fx, double _fy, double _cx, double _cy) { + fx = _fx; + fy = _fy; + cx = _cx; + cy = _cy; + init_inverse_parameters(); +} + +// This algorithm is from "Tong Ke, Stergios Roumeliotis, An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (Accepted by CVPR 2017) +// See https://arxiv.org/pdf/1701.08237.pdf +// featureVectors: The 3 bearing measurements (normalized) stored as column vectors +// worldPoints: The positions of the 3 feature points stored as column vectors +// solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame +// solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame +int ap3p::computePoses(const double featureVectors[3][3], + const double worldPoints[3][3], + double solutionsR[4][3][3], + double solutionsT[4][3]) { + + //world point vectors + double w1[3] = {worldPoints[0][0], worldPoints[1][0], worldPoints[2][0]}; + double w2[3] = {worldPoints[0][1], worldPoints[1][1], worldPoints[2][1]}; + double w3[3] = {worldPoints[0][2], worldPoints[1][2], worldPoints[2][2]}; + // k1 + double u0[3]; + vect_sub(w1, w2, u0); + + double nu0 = vect_norm(u0); + double k1[3]; + vect_divide(u0, nu0, k1); + // bi + double b1[3] = {featureVectors[0][0], featureVectors[1][0], featureVectors[2][0]}; + double b2[3] = {featureVectors[0][1], featureVectors[1][1], featureVectors[2][1]}; + double b3[3] = {featureVectors[0][2], featureVectors[1][2], featureVectors[2][2]}; + // k3,tz + double k3[3]; + vect_cross(b1, b2, k3); + double nk3 = vect_norm(k3); + vect_divide(k3, nk3, k3); + + double tz[3]; + vect_cross(b1, k3, tz); + // ui,vi + double v1[3]; + vect_cross(b1, b3, v1); + double v2[3]; + vect_cross(b2, b3, v2); + + double u1[3]; + vect_sub(w1, w3, u1); + // coefficients related terms + double u1k1 = vect_dot(u1, k1); + double k3b3 = vect_dot(k3, b3); + // f1i + double f11 = k3b3; + double f13 = vect_dot(k3, v1); + double f15 = -u1k1 * f11; + //delta + double nl[3]; + vect_cross(u1, k1, nl); + double delta = vect_norm(nl); + vect_divide(nl, delta, nl); + f11 *= delta; + f13 *= delta; + // f2i + double u2k1 = u1k1 - nu0; + double f21 = vect_dot(tz, v2); + double f22 = nk3 * k3b3; + double f23 = vect_dot(k3, v2); + double f24 = u2k1 * f22; + double f25 = -u2k1 * f21; + f21 *= delta; + f22 *= delta; + f23 *= delta; + double g1 = f13 * f22; + double g2 = f13 * f25 - f15 * f23; + double g3 = f11 * f23 - f13 * f21; + double g4 = -f13 * f24; + double g5 = f11 * f22; + double g6 = f11 * f25 - f15 * f21; + double g7 = -f15 * f24; + double coeffs[5] = {g5 * g5 + g1 * g1 + g3 * g3, + 2 * (g5 * g6 + g1 * g2 + g3 * g4), + g6 * g6 + 2 * g5 * g7 + g2 * g2 + g4 * g4 - g1 * g1 - g3 * g3, + 2 * (g6 * g7 - g1 * g2 - g3 * g4), + g7 * g7 - g2 * g2 - g4 * g4}; + double s[4]; + solveQuartic(coeffs, s); + polishQuarticRoots(coeffs, s); + + double temp[3]; + vect_cross(k1, nl, temp); + + double Ck1nl[3][3] = + {{k1[0], nl[0], temp[0]}, + {k1[1], nl[1], temp[1]}, + {k1[2], nl[2], temp[2]}}; + + double Cb1k3tzT[3][3] = + {{b1[0], b1[1], b1[2]}, + {k3[0], k3[1], k3[2]}, + {tz[0], tz[1], tz[2]}}; + + double b3p[3]; + vect_scale((delta / k3b3), b3, b3p); + + int nb_solutions = 0; + for (int i = 0; i < 4; ++i) { + double ctheta1p = s[i]; + if (abs(ctheta1p) > 1) + continue; + double stheta1p = sqrt(1 - ctheta1p * ctheta1p); + stheta1p = (k3b3 > 0) ? stheta1p : -stheta1p; + double ctheta3 = g1 * ctheta1p + g2; + double stheta3 = g3 * ctheta1p + g4; + double ntheta3 = stheta1p / ((g5 * ctheta1p + g6) * ctheta1p + g7); + ctheta3 *= ntheta3; + stheta3 *= ntheta3; + + double C13[3][3] = + {{ctheta3, 0, -stheta3}, + {stheta1p * stheta3, ctheta1p, stheta1p * ctheta3}, + {ctheta1p * stheta3, -stheta1p, ctheta1p * ctheta3}}; + + double temp_matrix[3][3]; + double R[3][3]; + mat_mult(Ck1nl, C13, temp_matrix); + mat_mult(temp_matrix, Cb1k3tzT, R); + + // R' * p3 + double rp3[3] = + {w3[0] * R[0][0] + w3[1] * R[1][0] + w3[2] * R[2][0], + w3[0] * R[0][1] + w3[1] * R[1][1] + w3[2] * R[2][1], + w3[0] * R[0][2] + w3[1] * R[1][2] + w3[2] * R[2][2]}; + + double pxstheta1p[3]; + vect_scale(stheta1p, b3p, pxstheta1p); + + vect_sub(pxstheta1p, rp3, solutionsT[nb_solutions]); + + solutionsR[nb_solutions][0][0] = R[0][0]; + solutionsR[nb_solutions][1][0] = R[0][1]; + solutionsR[nb_solutions][2][0] = R[0][2]; + solutionsR[nb_solutions][0][1] = R[1][0]; + solutionsR[nb_solutions][1][1] = R[1][1]; + solutionsR[nb_solutions][2][1] = R[1][2]; + solutionsR[nb_solutions][0][2] = R[2][0]; + solutionsR[nb_solutions][1][2] = R[2][1]; + solutionsR[nb_solutions][2][2] = R[2][2]; + + nb_solutions++; + } + + return nb_solutions; +} + +bool ap3p::solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Mat &ipoints) { + CV_INSTRUMENT_REGION() + + double rotation_matrix[3][3], translation[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); + + bool result = 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], + points[15], points[16], points[17], points[18], points[19]); + cv::Mat(3, 1, CV_64F, translation).copyTo(tvec); + cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R); + return result; +} + +bool +ap3p::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, double mu2, double mv2, double X2, double Y2, double Z2, double mu3, + double mv3, double X3, double Y3, double Z3) { + double Rs[4][3][3], ts[4][3]; + + int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2); + if (n == 0) + return false; + + int ns = 0; + double min_reproj = 0; + for (int i = 0; i < n; i++) { + double X3p = Rs[i][0][0] * X3 + Rs[i][0][1] * Y3 + Rs[i][0][2] * Z3 + ts[i][0]; + double Y3p = Rs[i][1][0] * X3 + Rs[i][1][1] * Y3 + Rs[i][1][2] * Z3 + ts[i][1]; + double Z3p = Rs[i][2][0] * X3 + Rs[i][2][1] * Y3 + Rs[i][2][2] * Z3 + ts[i][2]; + double mu3p = cx + fx * X3p / Z3p; + double mv3p = cy + fy * Y3p / Z3p; + double reproj = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3); + if (i == 0 || min_reproj > reproj) { + ns = i; + min_reproj = reproj; + } + } + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) + R[i][j] = Rs[ns][i][j]; + t[i] = ts[ns][i]; + } + + return true; +} + +int ap3p::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, double mu2, double mv2, double X2, double Y2, double Z2) { + double mk0, mk1, mk2; + double norm; + + mu0 = inv_fx * mu0 - cx_fx; + mv0 = inv_fy * mv0 - cy_fy; + norm = sqrt(mu0 * mu0 + mv0 * mv0 + 1); + mk0 = 1. / norm; + mu0 *= mk0; + mv0 *= mk0; + + mu1 = inv_fx * mu1 - cx_fx; + mv1 = inv_fy * mv1 - cy_fy; + norm = sqrt(mu1 * mu1 + mv1 * mv1 + 1); + mk1 = 1. / norm; + mu1 *= mk1; + mv1 *= mk1; + + mu2 = inv_fx * mu2 - cx_fx; + mv2 = inv_fy * mv2 - cy_fy; + norm = sqrt(mu2 * mu2 + mv2 * mv2 + 1); + mk2 = 1. / norm; + mu2 *= mk2; + mv2 *= mk2; + + double featureVectors[3][3] = {{mu0, mu1, mu2}, + {mv0, mv1, mv2}, + {mk0, mk1, mk2}}; + double worldPoints[3][3] = {{X0, X1, X2}, + {Y0, Y1, Y2}, + {Z0, Z1, Z2}}; + + 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 new file mode 100644 index 0000000000..37c7be12b4 --- /dev/null +++ b/modules/calib3d/src/ap3p.h @@ -0,0 +1,65 @@ +#ifndef P3P_P3P_H +#define P3P_P3P_H + +#include "precomp.hpp" + +namespace cv { +class ap3p { +private: + template + void init_camera_parameters(const cv::Mat &cameraMatrix) { + cx = cameraMatrix.at(0, 2); + cy = cameraMatrix.at(1, 2); + fx = cameraMatrix.at(0, 0); + fy = cameraMatrix.at(1, 1); + } + + 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++) { + 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; + points[i * 5 + 3] = opoints.at(i).y; + points[i * 5 + 4] = opoints.at(i).z; + } + } + + void init_inverse_parameters(); + + double fx, fy, cx, cy; + double inv_fx, inv_fy, cx_fx, cy_fy; +public: + ap3p() {} + + ap3p(double fx, double fy, double cx, double cy); + + ap3p(cv::Mat cameraMatrix); + + bool solve(cv::Mat &R, cv::Mat &tvec, 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, + double mu2, double mv2, double X2, double Y2, double Z2); + + bool 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, + double mu2, double mv2, double X2, double Y2, double Z2, + double mu3, double mv3, double X3, double Y3, double Z3); + + // This algorithm is from "Tong Ke, Stergios Roumeliotis, An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (Accepted by CVPR 2017) + // See https://arxiv.org/pdf/1701.08237.pdf + // featureVectors: 3 bearing measurements (normalized) stored as column vectors + // worldPoints: Positions of the 3 feature points stored as column vectors + // solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame + // solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame + int computePoses(const double featureVectors[3][3], const double worldPoints[3][3], double solutionsR[4][3][3], + double solutionsT[4][3]); + +}; +} +#endif //P3P_P3P_H diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 1e9b8ec6e4..a9be4c848d 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -45,6 +45,7 @@ #include "dls.h" #include "epnp.h" #include "p3p.h" +#include "ap3p.h" #include "opencv2/calib3d/calib3d_c.h" #include @@ -118,6 +119,18 @@ bool solvePnP( InputArray _opoints, InputArray _ipoints, if (result) Rodrigues(R, rvec); } + else if (flags == SOLVEPNP_AP3P) + { + CV_Assert( npoints == 4); + Mat undistortedPoints; + undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); + ap3p P3Psolver(cameraMatrix); + + Mat R; + result = P3Psolver.solve(R, tvec, opoints, undistortedPoints); + if (result) + Rodrigues(R, rvec); + } else if (flags == SOLVEPNP_ITERATIVE) { CvMat c_objectPoints = opoints, c_imagePoints = ipoints; @@ -291,7 +304,8 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints, opoints_inliers.resize(npoints1); ipoints_inliers.resize(npoints1); result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix, - distCoeffs, rvec, tvec, false, flags == SOLVEPNP_P3P ? SOLVEPNP_EPNP : flags) ? 1 : -1; + distCoeffs, rvec, tvec, false, + (flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P) ? SOLVEPNP_EPNP : flags) ? 1 : -1; } if( result <= 0 || _local_model.rows <= 0) diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp index e6e9b6b22f..86634a0b45 100644 --- a/modules/calib3d/test/test_solvepnp_ransac.cpp +++ b/modules/calib3d/test/test_solvepnp_ransac.cpp @@ -57,6 +57,7 @@ public: eps[SOLVEPNP_ITERATIVE] = 1.0e-2; eps[SOLVEPNP_EPNP] = 1.0e-2; eps[SOLVEPNP_P3P] = 1.0e-2; + eps[SOLVEPNP_AP3P] = 1.0e-2; eps[SOLVEPNP_DLS] = 1.0e-2; eps[SOLVEPNP_UPNP] = 1.0e-2; totalTestsCount = 10; @@ -161,7 +162,7 @@ protected: points.resize(pointsCount); generate3DPointCloud(points); - const int methodsCount = 5; + const int methodsCount = 6; RNG rng = ts->get_rng(); @@ -189,7 +190,7 @@ protected: } } } - double eps[5]; + double eps[6]; int totalTestsCount; }; @@ -201,6 +202,7 @@ public: eps[SOLVEPNP_ITERATIVE] = 1.0e-6; eps[SOLVEPNP_EPNP] = 1.0e-6; eps[SOLVEPNP_P3P] = 1.0e-4; + eps[SOLVEPNP_AP3P] = 1.0e-4; eps[SOLVEPNP_DLS] = 1.0e-4; eps[SOLVEPNP_UPNP] = 1.0e-4; totalTestsCount = 1000; @@ -222,7 +224,7 @@ protected: generatePose(trueRvec, trueTvec, rng); std::vector opoints; - if (method == 2) + if (method == 2 || method == 5) { opoints = std::vector(points.begin(), points.begin()+4); } -- GitLab