From c11551a510d3c45dfb2a75bff12863de3dd14c82 Mon Sep 17 00:00:00 2001 From: Alexander Shishkov Date: Mon, 26 Dec 2011 12:59:07 +0000 Subject: [PATCH] added P3P method added test for solvePnP changed test for solvePnPRansac fixed bug with mutex solvePnPRansac --- .../include/opencv2/calib3d/calib3d.hpp | 26 +- modules/calib3d/src/epnp.h | 46 +- modules/calib3d/src/p3p.cpp | 416 ++++++++++++++++++ modules/calib3d/src/p3p.h | 62 +++ modules/calib3d/src/polynom_solver.cpp | 170 +++++++ modules/calib3d/src/polynom_solver.h | 12 + modules/calib3d/src/solvepnp.cpp | 104 +++-- modules/calib3d/test/test_solvepnp_ransac.cpp | 232 +++++++--- 8 files changed, 941 insertions(+), 127 deletions(-) create mode 100644 modules/calib3d/src/p3p.cpp create mode 100644 modules/calib3d/src/p3p.h create mode 100644 modules/calib3d/src/polynom_solver.cpp create mode 100644 modules/calib3d/src/polynom_solver.h diff --git a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp index 889be59db5..edf31a0f04 100644 --- a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp @@ -86,6 +86,13 @@ CVAPI(void) cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst ); #define CV_FM_RANSAC_ONLY CV_RANSAC #define CV_FM_LMEDS CV_LMEDS #define CV_FM_RANSAC CV_RANSAC + +enum +{ + CV_ITERATIVE = 0, + CV_EPNP = 1, // F.Moreno-Noguer, V.Lepetit and P.Fua "EPnP: Efficient Perspective-n-Point Camera Pose Estimation" + CV_P3P = 2 // X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem" +}; CVAPI(int) cvFindFundamentalMat( const CvMat* points1, const CvMat* points2, CvMat* fundamental_matrix, @@ -431,10 +438,6 @@ public: namespace cv { -CV_EXPORTS_W double ePnP( InputArray _opoints, InputArray _ipoints, - InputArray _cameraMatrix, InputArray _distCoeffs, - OutputArray _rvec, OutputArray _tvec); - //! converts rotation vector to rotation matrix or vice versa using Rodrigues transformation CV_EXPORTS_W void Rodrigues(InputArray src, OutputArray dst, OutputArray jacobian=noArray()); @@ -491,10 +494,16 @@ CV_EXPORTS_W void projectPoints( InputArray objectPoints, double aspectRatio=0 ); //! computes the camera pose from a few 3D points and the corresponding projections. The outliers are not handled. -CV_EXPORTS_W void solvePnP( InputArray objectPoints, InputArray imagePoints, +enum +{ + ITERATIVE=CV_ITERATIVE, + EPNP=CV_EPNP, + P3P=CV_P3P +}; +CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, - bool useExtrinsicGuess=false ); + bool useExtrinsicGuess=false, int flags=0); //! computes the camera pose from a few 3D points and the corresponding projections. The outliers are possible. CV_EXPORTS_W void solvePnPRansac( InputArray objectPoints, @@ -503,11 +512,12 @@ CV_EXPORTS_W void solvePnPRansac( InputArray objectPoints, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, - bool useExtrinsicGuess = false, + bool useExtrinsicGuess = false, int iterationsCount = 100, float reprojectionError = 8.0, int minInliersCount = 100, - OutputArray inliers = noArray() ); + OutputArray inliers = noArray(), + int flags = 0); //! initializes camera matrix from a few 3D points and the corresponding projections. CV_EXPORTS_W Mat initCameraMatrix2D( InputArrayOfArrays objectPoints, diff --git a/modules/calib3d/src/epnp.h b/modules/calib3d/src/epnp.h index a7c8cfeeed..71f3aa9366 100644 --- a/modules/calib3d/src/epnp.h +++ b/modules/calib3d/src/epnp.h @@ -5,27 +5,36 @@ class epnp { public: - epnp(void); + epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints); ~epnp(); - void set_internal_parameters(const double uc, const double vc, - const double fu, const double fv); - - void set_maximum_number_of_correspondences(const int n); - void reset_correspondences(void); void add_correspondence(const double X, const double Y, const double Z, const double u, const double v); - double compute_pose(double R[3][3], double T[3]); - - void relative_error(double & rot_err, double & transl_err, - const double Rtrue[3][3], const double ttrue[3], - const double Rest[3][3], const double test[3]); - - void print_pose(const double R[3][3], const double t[3]); - double reprojection_error(const double R[3][3], const double t[3]); - + void compute_pose(cv::Mat& R, cv::Mat& t); private: + template + void init_camera_parameters(const cv::Mat& cameraMatrix) + { + uc = cameraMatrix.at (0, 2); + vc = cameraMatrix.at (1, 2); + fu = cameraMatrix.at (0, 0); + fv = cameraMatrix.at (1, 1); + } + template + void init_points(const cv::Mat& opoints, const cv::Mat& ipoints) + { + for(int i = 0; i < number_of_correspondences; i++) + { + pws[3 * i ] = opoints.at(0,i).x; + pws[3 * i + 1] = opoints.at(0,i).y; + pws[3 * i + 2] = opoints.at(0,i).z; + + us[2 * i ] = ipoints.at(0,i).x*fu + uc; + us[2 * i + 1] = ipoints.at(0,i).y*fv + vc; + } + } + double reprojection_error(const double R[3][3], const double t[3]); void choose_control_points(void); void compute_barycentric_coordinates(void); void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v); @@ -47,7 +56,7 @@ class epnp { void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]); void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho, - double cb[4], CvMat * A, CvMat * b); + const double cb[4], CvMat * A, CvMat * b); double compute_R_and_t(const double * ut, const double * betas, double R[3][3], double t[3]); @@ -57,13 +66,10 @@ class epnp { void copy_R_and_t(const double R_dst[3][3], const double t_dst[3], double R_src[3][3], double t_src[3]); - void mat_to_quat(const double R[3][3], double q[4]); - double uc, vc, fu, fv; - double * pws, * us, * alphas, * pcs; - int maximum_number_of_correspondences; + std::vector pws, us, alphas, pcs; int number_of_correspondences; double cws[4][3], ccs[4][3]; diff --git a/modules/calib3d/src/p3p.cpp b/modules/calib3d/src/p3p.cpp new file mode 100644 index 0000000000..300f2307f5 --- /dev/null +++ b/modules/calib3d/src/p3p.cpp @@ -0,0 +1,416 @@ +#include +#include +#include + +#include "polynom_solver.h" +#include "p3p.h" + +using namespace std; + +void p3p::init_inverse_parameters() +{ + inv_fx = 1. / fx; + inv_fy = 1. / fy; + cx_fx = cx / fx; + cy_fy = cy / fy; +} + +p3p::p3p(cv::Mat cameraMatrix) +{ + if (cameraMatrix.depth() == CV_32F) + init_camera_parameters(cameraMatrix); + else + init_camera_parameters(cameraMatrix); + init_inverse_parameters(); +} + +p3p::p3p(double _fx, double _fy, double _cx, double _cy) +{ + fx = _fx; + fy = _fy; + cx = _cx; + cy = _cy; + init_inverse_parameters(); +} + +bool p3p::solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat& ipoints) +{ + 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 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, + 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 p3p::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 distances[3]; + distances[0] = sqrt( (X1 - X2) * (X1 - X2) + (Y1 - Y2) * (Y1 - Y2) + (Z1 - Z2) * (Z1 - Z2) ); + distances[1] = sqrt( (X0 - X2) * (X0 - X2) + (Y0 - Y2) * (Y0 - Y2) + (Z0 - Z2) * (Z0 - Z2) ); + distances[2] = sqrt( (X0 - X1) * (X0 - X1) + (Y0 - Y1) * (Y0 - Y1) + (Z0 - Z1) * (Z0 - Z1) ); + + // Calculate angles + double cosines[3]; + cosines[0] = mu1 * mu2 + mv1 * mv2 + mk1 * mk2; + cosines[1] = mu0 * mu2 + mv0 * mv2 + mk0 * mk2; + cosines[2] = mu0 * mu1 + mv0 * mv1 + mk0 * mk1; + + double lengths[4][3]; + int n = solve_for_lengths(lengths, distances, cosines); + + int nb_solutions = 0; + for(int i = 0; i < n; i++) { + double M_orig[3][3]; + + M_orig[0][0] = lengths[i][0] * mu0; + M_orig[0][1] = lengths[i][0] * mv0; + M_orig[0][2] = lengths[i][0] * mk0; + + M_orig[1][0] = lengths[i][1] * mu1; + M_orig[1][1] = lengths[i][1] * mv1; + M_orig[1][2] = lengths[i][1] * mk1; + + M_orig[2][0] = lengths[i][2] * mu2; + M_orig[2][1] = lengths[i][2] * mv2; + M_orig[2][2] = lengths[i][2] * mk2; + + if (!align(M_orig, X0, Y0, Z0, X1, Y1, Z1, X2, Y2, Z2, R[nb_solutions], t[nb_solutions])) + continue; + + nb_solutions++; + } + + return nb_solutions; +} + +/// Given 3D distances between three points and cosines of 3 angles at the apex, calculates +/// the lentghs of the line segments connecting projection center (P) and the three 3D points (A, B, C). +/// Returned distances are for |PA|, |PB|, |PC| respectively. +/// Only the solution to the main branch. +/// Reference : X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem" +/// IEEE Trans. on PAMI, vol. 25, No. 8, August 2003 +/// \param lengths3D Lengths of line segments up to four solutions. +/// \param dist3D Distance between 3D points in pairs |BC|, |AC|, |AB|. +/// \param cosines Cosine of the angles /_BPC, /_APC, /_APB. +/// \returns Number of solutions. +/// WARNING: NOT ALL THE DEGENERATE CASES ARE IMPLEMENTED + +int p3p::solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3]) +{ + double p = cosines[0] * 2; + double q = cosines[1] * 2; + double r = cosines[2] * 2; + + double inv_d22 = 1. / (distances[2] * distances[2]); + double a = inv_d22 * (distances[0] * distances[0]); + double b = inv_d22 * (distances[1] * distances[1]); + + double a2 = a * a, b2 = b * b, p2 = p * p, q2 = q * q, r2 = r * r; + double pr = p * r, pqr = q * pr; + + // Check reality condition (the four points should not be coplanar) + if (p2 + q2 + r2 - pqr - 1 == 0) + return 0; + + double ab = a * b, a_2 = 2*a; + + double A = -2 * b + b2 + a2 + 1 + ab*(2 - r2) - a_2; + + // Check reality condition + if (A == 0) return 0; + + double a_4 = 4*a; + + double B = q*(-2*(ab + a2 + 1 - b) + r2*ab + a_4) + pr*(b - b2 + ab); + double C = q2 + b2*(r2 + p2 - 2) - b*(p2 + pqr) - ab*(r2 + pqr) + (a2 - a_2)*(2 + q2) + 2; + double D = pr*(ab-b2+b) + q*((p2-2)*b + 2 * (ab - a2) + a_4 - 2); + double E = 1 + 2*(b - a - ab) + b2 - b*p2 + a2; + + double temp = (p2*(a-1+b) + r2*(a-1-b) + pqr - a*pqr); + double b0 = b * temp * temp; + // Check reality condition + if (b0 == 0) + return 0; + + double real_roots[4]; + int n = solve_deg4(A, B, C, D, E, real_roots[0], real_roots[1], real_roots[2], real_roots[3]); + + if (n == 0) + return 0; + + int nb_solutions = 0; + double r3 = r2*r, pr2 = p*r2, r3q = r3 * q; + double inv_b0 = 1. / b0; + + // For each solution of x + for(int i = 0; i < n; i++) { + double x = real_roots[i]; + + // Check reality condition + if (x <= 0) + continue; + + double x2 = x*x; + + double b1 = + ((1-a-b)*x2 + (q*a-q)*x + 1 - a + b) * + (((r3*(a2 + ab*(2 - r2) - a_2 + b2 - 2*b + 1)) * x + + + (r3q*(2*(b-a2) + a_4 + ab*(r2 - 2) - 2) + pr2*(1 + a2 + 2*(ab-a-b) + r2*(b - b2) + b2))) * x2 + + + (r3*(q2*(1-2*a+a2) + r2*(b2-ab) - a_4 + 2*(a2 - b2) + 2) + r*p2*(b2 + 2*(ab - b - a) + 1 + a2) + pr2*q*(a_4 + 2*(b - ab - a2) - 2 - r2*b)) * x + + + 2*r3q*(a_2 - b - a2 + ab - 1) + pr2*(q2 - a_4 + 2*(a2 - b2) + r2*b + q2*(a2 - a_2) + 2) + + p2*(p*(2*(ab - a - b) + a2 + b2 + 1) + 2*q*r*(b + a_2 - a2 - ab - 1))); + + // Check reality condition + if (b1 <= 0) + continue; + + double y = inv_b0 * b1; + double v = x2 + y*y - x*y*r; + + if (v <= 0) + continue; + + double Z = distances[2] / sqrt(v); + double X = x * Z; + double Y = y * Z; + + lengths[nb_solutions][0] = X; + lengths[nb_solutions][1] = Y; + lengths[nb_solutions][2] = Z; + + nb_solutions++; + } + + return nb_solutions; +} + +bool p3p::align(double M_end[3][3], + double X0, double Y0, double Z0, + double X1, double Y1, double Z1, + double X2, double Y2, double Z2, + double R[3][3], double T[3]) +{ + // Centroids: + double C_start[3], C_end[3]; + for(int i = 0; i < 3; i++) C_end[i] = (M_end[0][i] + M_end[1][i] + M_end[2][i]) / 3; + C_start[0] = (X0 + X1 + X2) / 3; + C_start[1] = (Y0 + Y1 + Y2) / 3; + C_start[2] = (Z0 + Z1 + Z2) / 3; + + // Covariance matrix s: + double s[3 * 3]; + for(int j = 0; j < 3; j++) { + s[0 * 3 + j] = (X0 * M_end[0][j] + X1 * M_end[1][j] + X2 * M_end[2][j]) / 3 - C_end[j] * C_start[0]; + s[1 * 3 + j] = (Y0 * M_end[0][j] + Y1 * M_end[1][j] + Y2 * M_end[2][j]) / 3 - C_end[j] * C_start[1]; + s[2 * 3 + j] = (Z0 * M_end[0][j] + Z1 * M_end[1][j] + Z2 * M_end[2][j]) / 3 - C_end[j] * C_start[2]; + } + + double Qs[16], evs[4], U[16]; + + Qs[0 * 4 + 0] = s[0 * 3 + 0] + s[1 * 3 + 1] + s[2 * 3 + 2]; + Qs[1 * 4 + 1] = s[0 * 3 + 0] - s[1 * 3 + 1] - s[2 * 3 + 2]; + Qs[2 * 4 + 2] = s[1 * 3 + 1] - s[2 * 3 + 2] - s[0 * 3 + 0]; + Qs[3 * 4 + 3] = s[2 * 3 + 2] - s[0 * 3 + 0] - s[1 * 3 + 1]; + + Qs[1 * 4 + 0] = Qs[0 * 4 + 1] = s[1 * 3 + 2] - s[2 * 3 + 1]; + Qs[2 * 4 + 0] = Qs[0 * 4 + 2] = s[2 * 3 + 0] - s[0 * 3 + 2]; + Qs[3 * 4 + 0] = Qs[0 * 4 + 3] = s[0 * 3 + 1] - s[1 * 3 + 0]; + Qs[2 * 4 + 1] = Qs[1 * 4 + 2] = s[1 * 3 + 0] + s[0 * 3 + 1]; + Qs[3 * 4 + 1] = Qs[1 * 4 + 3] = s[2 * 3 + 0] + s[0 * 3 + 2]; + Qs[3 * 4 + 2] = Qs[2 * 4 + 3] = s[2 * 3 + 1] + s[1 * 3 + 2]; + + jacobi_4x4(Qs, evs, U); + + // Looking for the largest eigen value: + int i_ev = 0; + double ev_max = evs[i_ev]; + for(int i = 1; i < 4; i++) + if (evs[i] > ev_max) + ev_max = evs[i_ev = i]; + + // Quaternion: + double q[4]; + for(int i = 0; i < 4; i++) + q[i] = U[i * 4 + i_ev]; + + double q02 = q[0] * q[0], q12 = q[1] * q[1], q22 = q[2] * q[2], q32 = q[3] * q[3]; + double q0_1 = q[0] * q[1], q0_2 = q[0] * q[2], q0_3 = q[0] * q[3]; + double q1_2 = q[1] * q[2], q1_3 = q[1] * q[3]; + double q2_3 = q[2] * q[3]; + + R[0][0] = q02 + q12 - q22 - q32; + R[0][1] = 2. * (q1_2 - q0_3); + R[0][2] = 2. * (q1_3 + q0_2); + + R[1][0] = 2. * (q1_2 + q0_3); + R[1][1] = q02 + q22 - q12 - q32; + R[1][2] = 2. * (q2_3 - q0_1); + + R[2][0] = 2. * (q1_3 - q0_2); + R[2][1] = 2. * (q2_3 + q0_1); + R[2][2] = q02 + q32 - q12 - q22; + + for(int i = 0; i < 3; i++) + T[i] = C_end[i] - (R[i][0] * C_start[0] + R[i][1] * C_start[1] + R[i][2] * C_start[2]); + + return true; +} + +bool p3p::jacobi_4x4(double * A, double * D, double * U) +{ + double B[4], Z[4]; + double Id[16] = {1., 0., 0., 0., + 0., 1., 0., 0., + 0., 0., 1., 0., + 0., 0., 0., 1.}; + + memcpy(U, Id, 16 * sizeof(double)); + + B[0] = A[0]; B[1] = A[5]; B[2] = A[10]; B[3] = A[15]; + memcpy(D, B, 4 * sizeof(double)); + memset(Z, 0, 4 * sizeof(double)); + + for(int iter = 0; iter < 50; iter++) { + double sum = fabs(A[1]) + fabs(A[2]) + fabs(A[3]) + fabs(A[6]) + fabs(A[7]) + fabs(A[11]); + + if (sum == 0.0) + return true; + + double tresh = (iter < 3) ? 0.2 * sum / 16. : 0.0; + for(int i = 0; i < 3; i++) { + double * pAij = A + 5 * i + 1; + for(int j = i + 1 ; j < 4; j++) { + double Aij = *pAij; + double eps_machine = 100.0 * fabs(Aij); + + if ( iter > 3 && fabs(D[i]) + eps_machine == fabs(D[i]) && fabs(D[j]) + eps_machine == fabs(D[j]) ) + *pAij = 0.0; + else if (fabs(Aij) > tresh) { + double h = D[j] - D[i], t; + if (fabs(h) + eps_machine == fabs(h)) + t = Aij / h; + else { + double theta = 0.5 * h / Aij; + t = 1.0 / (fabs(theta) + sqrt(1.0 + theta * theta)); + if (theta < 0.0) t = -t; + } + + h = t * Aij; + Z[i] -= h; + Z[j] += h; + D[i] -= h; + D[j] += h; + *pAij = 0.0; + + double c = 1.0 / sqrt(1 + t * t); + double s = t * c; + double tau = s / (1.0 + c); + for(int k = 0; k <= i - 1; k++) { + double g = A[k * 4 + i], h = A[k * 4 + j]; + A[k * 4 + i] = g - s * (h + g * tau); + A[k * 4 + j] = h + s * (g - h * tau); + } + for(int k = i + 1; k <= j - 1; k++) { + double g = A[i * 4 + k], h = A[k * 4 + j]; + A[i * 4 + k] = g - s * (h + g * tau); + A[k * 4 + j] = h + s * (g - h * tau); + } + for(int k = j + 1; k < 4; k++) { + double g = A[i * 4 + k], h = A[j * 4 + k]; + A[i * 4 + k] = g - s * (h + g * tau); + A[j * 4 + k] = h + s * (g - h * tau); + } + for(int k = 0; k < 4; k++) { + double g = U[k * 4 + i], h = U[k * 4 + j]; + U[k * 4 + i] = g - s * (h + g * tau); + U[k * 4 + j] = h + s * (g - h * tau); + } + } + pAij++; + } + } + + for(int i = 0; i < 4; i++) B[i] += Z[i]; + memcpy(D, B, 4 * sizeof(double)); + memset(Z, 0, 4 * sizeof(double)); + } + + return false; +} + diff --git a/modules/calib3d/src/p3p.h b/modules/calib3d/src/p3p.h new file mode 100644 index 0000000000..a2bee0e01d --- /dev/null +++ b/modules/calib3d/src/p3p.h @@ -0,0 +1,62 @@ +#ifndef P3P_H +#define P3P_H + + +#include "precomp.hpp" + +class p3p +{ + public: + p3p(double fx, double fy, double cx, double cy); + p3p(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); + + 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(0,i).x*fx + cx; + points[i*5+1] = ipoints.at(0,i).y*fy + cy; + points[i*5+2] = opoints.at(0,i).x; + points[i*5+3] = opoints.at(0,i).y; + points[i*5+4] = opoints.at(0,i).z; + } + } + void init_inverse_parameters(); + int solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3]); + bool align(double M_start[3][3], + double X0, double Y0, double Z0, + double X1, double Y1, double Z1, + double X2, double Y2, double Z2, + double R[3][3], double T[3]); + + bool jacobi_4x4(double * A, double * D, double * U); + + double fx, fy, cx, cy; + double inv_fx, inv_fy, cx_fx, cy_fy; +}; + +#endif // P3P_H + diff --git a/modules/calib3d/src/polynom_solver.cpp b/modules/calib3d/src/polynom_solver.cpp new file mode 100644 index 0000000000..72a60444d6 --- /dev/null +++ b/modules/calib3d/src/polynom_solver.cpp @@ -0,0 +1,170 @@ +#include +#include +using namespace std; + +#include "polynom_solver.h" + +int solve_deg2(double a, double b, double c, double & x1, double & x2) +{ + double delta = b * b - 4 * a * c; + + if (delta < 0) return 0; + + double inv_2a = 0.5 / a; + + if (delta == 0) { + x1 = -b * inv_2a; + x2 = x1; + return 1; + } + + double sqrt_delta = sqrt(delta); + x1 = (-b + sqrt_delta) * inv_2a; + x2 = (-b - sqrt_delta) * inv_2a; + return 2; +} + + +/// Reference : Eric W. Weisstein. "Cubic Equation." From MathWorld--A Wolfram Web Resource. +/// http://mathworld.wolfram.com/CubicEquation.html +/// \return Number of real roots found. +int solve_deg3(double a, double b, double c, double d, + double & x0, double & x1, double & x2) +{ + if (a == 0) { + // Solve second order sytem + if (b == 0) { + // Solve first order system + if (c == 0) + return 0; + + x0 = -d / c; + return 1; + } + + x2 = 0; + return solve_deg2(b, c, d, x0, x1); + } + + // Calculate the normalized form x^3 + a2 * x^2 + a1 * x + a0 = 0 + double inv_a = 1. / a; + double b_a = inv_a * b, b_a2 = b_a * b_a; + double c_a = inv_a * c; + double d_a = inv_a * d; + + // Solve the cubic equation + double Q = (3 * c_a - b_a2) / 9; + double R = (9 * b_a * c_a - 27 * d_a - 2 * b_a * b_a2) / 54; + double Q3 = Q * Q * Q; + double D = Q3 + R * R; + double b_a_3 = (1. / 3.) * b_a; + + if (Q == 0) { + if(R == 0) { + x0 = x1 = x2 = - b_a_3; + return 3; + } + else { + x0 = pow(2 * R, 1 / 3.0) - b_a_3; + return 1; + } + } + + if (D <= 0) { + // Three real roots + double theta = acos(R / sqrt(-Q3)); + double sqrt_Q = sqrt(-Q); + x0 = 2 * sqrt_Q * cos(theta / 3.0) - b_a_3; + x1 = 2 * sqrt_Q * cos((theta + 2 * CV_PI)/ 3.0) - b_a_3; + x2 = 2 * sqrt_Q * cos((theta + 4 * CV_PI)/ 3.0) - b_a_3; + + return 3; + } + + // D > 0, only one real root + double AD = pow(fabs(R) + sqrt(D), 1.0 / 3.0) * (R > 0 ? 1 : (R < 0 ? -1 : 0)); + double BD = (AD == 0) ? 0 : -Q / AD; + + // Calculate the only real root + x0 = AD + BD - b_a_3; + + return 1; +} + +/// Reference : Eric W. Weisstein. "Quartic Equation." From MathWorld--A Wolfram Web Resource. +/// http://mathworld.wolfram.com/QuarticEquation.html +/// \return Number of real roots found. +int solve_deg4(double a, double b, double c, double d, double e, + double & x0, double & x1, double & x2, double & x3) +{ + if (a == 0) { + x3 = 0; + return solve_deg3(b, c, d, e, x0, x1, x2); + } + + // Normalize coefficients + double inv_a = 1. / a; + b *= inv_a; c *= inv_a; d *= inv_a; e *= inv_a; + double b2 = b * b, bc = b * c, b3 = b2 * b; + + // Solve resultant cubic + double r0, r1, r2; + int n = solve_deg3(1, -c, d * b - 4 * e, 4 * c * e - d * d - b2 * e, r0, r1, r2); + if (n == 0) return 0; + + // Calculate R^2 + double R2 = 0.25 * b2 - c + r0, R; + if (R2 < 0) + return 0; + + R = sqrt(R2); + double inv_R = 1. / R; + + int nb_real_roots = 0; + + // Calculate D^2 and E^2 + double D2, E2; + if (R < 10E-12) { + double temp = r0 * r0 - 4 * e; + if (temp < 0) + D2 = E2 = -1; + else { + double sqrt_temp = sqrt(temp); + D2 = 0.75 * b2 - 2 * c + 2 * sqrt_temp; + E2 = D2 - 4 * sqrt_temp; + } + } + else { + double u = 0.75 * b2 - 2 * c - R2, + v = 0.25 * inv_R * (4 * bc - 8 * d - b3); + D2 = u + v; + E2 = u - v; + } + + double b_4 = 0.25 * b, R_2 = 0.5 * R; + if (D2 >= 0) { + double D = sqrt(D2); + nb_real_roots = 2; + double D_2 = 0.5 * D; + x0 = R_2 + D_2 - b_4; + x1 = x0 - D; + } + + // Calculate E^2 + if (E2 >= 0) { + double E = sqrt(E2); + double E_2 = 0.5 * E; + if (nb_real_roots == 0) { + x0 = - R_2 + E_2 - b_4; + x1 = x0 - E; + nb_real_roots = 2; + } + else { + x2 = - R_2 + E_2 - b_4; + x3 = x2 - E; + nb_real_roots = 4; + } + } + + return nb_real_roots; +} diff --git a/modules/calib3d/src/polynom_solver.h b/modules/calib3d/src/polynom_solver.h new file mode 100644 index 0000000000..ce4624bc9c --- /dev/null +++ b/modules/calib3d/src/polynom_solver.h @@ -0,0 +1,12 @@ +#ifndef POLYNOM_SOLVER_H +#define POLYNOM_SOLVER_H + +int solve_deg2(double a, double b, double c, double & x1, double & x2); + +int solve_deg3(double a, double b, double c, double d, + double & x0, double & x1, double & x2); + +int solve_deg4(double a, double b, double c, double d, double e, + double & x0, double & x1, double & x2, double & x3); + +#endif // POLYNOM_SOLVER_H diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 5ceee9208f..5b99b36104 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -41,25 +41,62 @@ //M*/ #include "precomp.hpp" +#include "epnp.h" +#include "p3p.h" +#include using namespace cv; -void cv::solvePnP( InputArray _opoints, InputArray _ipoints, +bool cv::solvePnP( InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs, - OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess ) + OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags ) { Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); - CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); - + CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); _rvec.create(3, 1, CV_64F); _tvec.create(3, 1, CV_64F); Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); - CvMat c_objectPoints = opoints, c_imagePoints = ipoints; - CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs; - CvMat c_rvec = _rvec.getMat(), c_tvec = _tvec.getMat(); - cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix, - c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0, - &c_rvec, &c_tvec, useExtrinsicGuess ); + + if (flags == CV_EPNP) + { + cv::Mat undistortedPoints; + cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); + epnp PnP(cameraMatrix, opoints, undistortedPoints); + + cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); + PnP.compute_pose(R, tvec); + cv::Rodrigues(R, rvec); + return true; + } + else if (flags == CV_P3P) + { + CV_Assert( npoints == 4); + cv::Mat undistortedPoints; + cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); + p3p P3Psolver(cameraMatrix); + + cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); + bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints); + if (result) + cv::Rodrigues(R, rvec); + return result; + } + else if (flags == CV_ITERATIVE) + { + CvMat c_objectPoints = opoints, c_imagePoints = ipoints; + CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs; + CvMat c_rvec = _rvec.getMat(), c_tvec = _tvec.getMat(); + cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix, + c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0, + &c_rvec, &c_tvec, useExtrinsicGuess ); + return true; + } + else + { + CV_Error(CV_StsBadArg, "The flags argument must be one of CV_ITERATIVE or CV_EPNP"); + return false; + } + return false; } namespace cv @@ -84,25 +121,25 @@ namespace cv class Mutex { public: - Mutex() {} + Mutex() { + } void lock() { #ifdef HAVE_TBB - slock.acquire(resultsMutex); + resultsMutex.lock(); #endif } void unlock() { #ifdef HAVE_TBB - slock.release(); + resultsMutex.unlock(); #endif } private: #ifdef HAVE_TBB tbb::mutex resultsMutex; - tbb::mutex::scoped_lock slock; #endif }; @@ -124,6 +161,7 @@ namespace cv float reprojectionError; int minInliersCount; bool useExtrinsicGuess; + int flags; CameraParameters camera; }; @@ -159,8 +197,10 @@ namespace cv Mat localRvec, localTvec; rvecInit.copyTo(localRvec); tvecInit.copyTo(localTvec); - - solvePnP(modelObjectPoints, modelImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, params.useExtrinsicGuess); + + solvePnP(modelObjectPoints, modelImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, + params.useExtrinsicGuess, params.flags); + vector projected_points; projected_points.resize(objectPoints.cols); @@ -206,7 +246,7 @@ namespace cv generateVar(pointsMask); pnpTask(pointsMask, objectPoints, imagePoints, parameters, inliers, rvec, tvec, initRvec, initTvec, syncMutex); - if ((int)inliers.size() > parameters.minInliersCount) + if ((int)inliers.size() >= parameters.minInliersCount) { #ifdef HAVE_TBB tbb::task::self().cancel_group_execution(); @@ -261,7 +301,7 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int iterationsCount, float reprojectionError, int minInliersCount, - OutputArray _inliers) + OutputArray _inliers, int flags) { Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); @@ -288,6 +328,7 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, params.reprojectionError = reprojectionError; params.useExtrinsicGuess = useExtrinsicGuess; params.camera.init(cameraMatrix, distCoeffs); + params.flags = flags; vector localInliers; Mat localRvec, localTvec; @@ -302,18 +343,21 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT) { - int i, pointsCount = (int)localInliers.size(); - Mat inlierObjectPoints(1, pointsCount, CV_32FC3), inlierImagePoints(1, pointsCount, CV_32FC2); - for (i = 0; i < pointsCount; i++) - { - int index = localInliers[i]; - Mat colInlierImagePoints = inlierImagePoints(Rect(i, 0, 1, 1)); - imagePoints.col(index).copyTo(colInlierImagePoints); - Mat colInlierObjectPoints = inlierObjectPoints(Rect(i, 0, 1, 1)); - objectPoints.col(index).copyTo(colInlierObjectPoints); - } - solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, true); - localRvec.copyTo(rvec); + if (flags != CV_P3P) + { + int i, pointsCount = (int)localInliers.size(); + Mat inlierObjectPoints(1, pointsCount, CV_32FC3), inlierImagePoints(1, pointsCount, CV_32FC2); + for (i = 0; i < pointsCount; i++) + { + int index = localInliers[i]; + Mat colInlierImagePoints = inlierImagePoints(Rect(i, 0, 1, 1)); + imagePoints.col(index).copyTo(colInlierImagePoints); + Mat colInlierObjectPoints = inlierObjectPoints(Rect(i, 0, 1, 1)); + objectPoints.col(index).copyTo(colInlierObjectPoints); + } + solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, true, flags); + } + localRvec.copyTo(rvec); localTvec.copyTo(tvec); if (_inliers.needed()) Mat(localInliers).copyTo(_inliers); diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp index 6dc9a98ced..e4e3989107 100644 --- a/modules/calib3d/test/test_solvepnp_ransac.cpp +++ b/modules/calib3d/test/test_solvepnp_ransac.cpp @@ -48,100 +48,194 @@ using namespace std; class CV_solvePnPRansac_Test : public cvtest::BaseTest { public: - CV_solvePnPRansac_Test() {} + CV_solvePnPRansac_Test() + { + eps[CV_ITERATIVE] = 1.0e-2; + eps[CV_EPNP] = 1.0e-2; + eps[CV_P3P] = 1.0e-2; + totalTestsCount = 10; + } ~CV_solvePnPRansac_Test() {} protected: void generate3DPointCloud(vector& points, Point3f pmin = Point3f(-1, - -1, 5), Point3f pmax = Point3f(1, 1, 10)) + -1, 5), Point3f pmax = Point3f(1, 1, 10)) { - const Point3f delta = pmax - pmin; - for (size_t i = 0; i < points.size(); i++) + const Point3f delta = pmax - pmin; + for (size_t i = 0; i < points.size(); i++) + { + Point3f p(float(rand()) / RAND_MAX, float(rand()) / RAND_MAX, + float(rand()) / RAND_MAX); + p.x *= delta.x; + p.y *= delta.y; + p.z *= delta.z; + p = p + pmin; + points[i] = p; + } + } + + void generateCameraMatrix(Mat& cameraMatrix, RNG& rng) + { + const double fcMinVal = 1e-3; + const double fcMaxVal = 100; + cameraMatrix.create(3, 3, CV_64FC1); + cameraMatrix.setTo(Scalar(0)); + cameraMatrix.at(0,0) = rng.uniform(fcMinVal, fcMaxVal); + cameraMatrix.at(1,1) = rng.uniform(fcMinVal, fcMaxVal); + cameraMatrix.at(0,2) = rng.uniform(fcMinVal, fcMaxVal); + cameraMatrix.at(1,2) = rng.uniform(fcMinVal, fcMaxVal); + cameraMatrix.at(2,2) = 1; + } + + void generateDistCoeffs(Mat& distCoeffs, RNG& rng) + { + distCoeffs = Mat::zeros(4, 1, CV_64FC1); + for (int i = 0; i < 3; i++) + distCoeffs.at(i,0) = rng.uniform(0.0, 1.0e-6); + } + + void generatePose(Mat& rvec, Mat& tvec, RNG& rng) + { + const double minVal = 1.0e-3; + const double maxVal = 1.0; + rvec.create(3, 1, CV_64FC1); + tvec.create(3, 1, CV_64FC1); + for (int i = 0; i < 3; i++) + { + rvec.at(i,0) = rng.uniform(minVal, maxVal); + tvec.at(i,0) = rng.uniform(minVal, maxVal/10); + } + } + + virtual bool runTest(RNG& rng, int mode, int method, const vector& points, const double* eps, double& maxError) + { + Mat rvec, tvec; + vector inliers; + 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); + + vector projectedPoints; + projectedPoints.resize(points.size()); + projectPoints(Mat(points), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints); + for (size_t i = 0; i < projectedPoints.size(); i++) + { + if (i % 20 == 0) { - Point3f p(float(rand()) / RAND_MAX, float(rand()) / RAND_MAX, - float(rand()) / RAND_MAX); - p.x *= delta.x; - p.y *= delta.y; - p.z *= delta.z; - p = p + pmin; - points[i] = p; + projectedPoints[i] = projectedPoints[rng.uniform(0,points.size()-1)]; } + } + + solvePnPRansac(points, projectedPoints, intrinsics, distCoeffs, rvec, tvec, + false, 500, 0.5, -1, inliers, method); + + bool isTestSuccess = inliers.size() >= points.size()*0.95; + + double rvecDiff = norm(rvec-trueRvec), tvecDiff = norm(tvec-trueTvec); + isTestSuccess = isTestSuccess && rvecDiff < eps[method] && tvecDiff < eps[method]; + double error = rvecDiff > tvecDiff ? rvecDiff : tvecDiff; + //cout << error << " " << inliers.size() << " " << eps[method] << endl; + if (error > maxError) + maxError = error; + + return isTestSuccess; } void run(int) { cvtest::TS& ts = *this->ts; ts.set_failed_test_info(cvtest::TS::OK); - Mat intrinsics = Mat::eye(3, 3, CV_32FC1); - intrinsics.at (0, 0) = 400.0; - intrinsics.at (1, 1) = 400.0; - intrinsics.at (0, 2) = 640 / 2; - intrinsics.at (1, 2) = 480 / 2; - Mat dist_coeffs = Mat::zeros(5, 1, CV_32FC1); - Mat rvec1 = Mat::zeros(3, 1, CV_64FC1); - Mat tvec1 = Mat::zeros(3, 1, CV_64FC1); - rvec1.at (0, 0) = 1.0f; - tvec1.at (0, 0) = 1.0f; - tvec1.at (1, 0) = 1.0f; + vector points; - points.resize(500); + const int pointsCount = 500; + points.resize(pointsCount); generate3DPointCloud(points); - vector points1; - points1.resize(points.size()); - projectPoints(Mat(points), rvec1, tvec1, intrinsics, dist_coeffs, points1); - for (size_t i = 0; i < points1.size(); i++) + + const int methodsCount = 3; + RNG rng = ts.get_rng(); + + + for (int mode = 0; mode < 2; mode++) { - if (i % 20 == 0) + for (int method = 0; method < methodsCount; method++) { - points1[i] = points1[rand() % points.size()]; - } - } - double eps = 1.0e-7; - - int successfulTestsCount = 0; - int totalTestsCount = 10; - - for (int testIndex = 0; testIndex < totalTestsCount; testIndex++) - { - try - { - Mat rvec, tvec; - vector inliers; - - solvePnPRansac(points, points1, intrinsics, dist_coeffs, rvec, tvec, - false, 1000, 2.0, -1, inliers); - - bool isTestSuccess = inliers.size() == 475; - - isTestSuccess = isTestSuccess - && (abs(rvec.at (0, 0) - 1) < eps); - isTestSuccess = isTestSuccess && (abs(rvec.at (1, 0)) < eps); - isTestSuccess = isTestSuccess && (abs(rvec.at (2, 0)) < eps); - isTestSuccess = isTestSuccess - && (abs(tvec.at (0, 0) - 1) < eps); - isTestSuccess = isTestSuccess - && (abs(tvec.at (1, 0) - 1) < eps); - isTestSuccess = isTestSuccess && (abs(tvec.at (2, 0)) < eps); - - if (isTestSuccess) - successfulTestsCount++; - - } - catch(...) - { - ts.printf(cvtest::TS::LOG, "Exception in solvePnPRansac\n"); + double maxError = 0; + int successfulTestsCount = 0; + for (int testIndex = 0; testIndex < totalTestsCount; testIndex++) + { + if (runTest(rng, mode, 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); + } } } + } + double eps[3]; + int totalTestsCount; +}; - if (successfulTestsCount < 0.8*totalTestsCount) +class CV_solvePnP_Test : public CV_solvePnPRansac_Test +{ +public: + CV_solvePnP_Test() + { + eps[CV_ITERATIVE] = 1.0e-6; + eps[CV_EPNP] = 1.0e-6; + eps[CV_P3P] = 1.0e-4; + totalTestsCount = 1000; + } + + ~CV_solvePnP_Test() {} +protected: + virtual bool runTest(RNG& rng, int mode, int method, const vector& points, const double* eps, double& maxError) + { + Mat rvec, tvec; + 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; + if (method == 2) { - ts.printf( cvtest::TS::LOG, "Invalid accuracy, failed %d tests from %d\n", - totalTestsCount - successfulTestsCount, totalTestsCount); - ts.set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); + opoints = std::vector(points.begin(), points.begin()+4); } + else + opoints = points; + + vector projectedPoints; + projectedPoints.resize(opoints.size()); + projectPoints(Mat(opoints), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints); + + solvePnP(opoints, projectedPoints, intrinsics, distCoeffs, rvec, tvec, + false, method); + + double rvecDiff = norm(rvec-trueRvec), tvecDiff = norm(tvec-trueTvec); + bool isTestSuccess = rvecDiff < eps[method] && tvecDiff < eps[method]; + + double error = rvecDiff > tvecDiff ? rvecDiff : tvecDiff; + if (error > maxError) + maxError = error; + + return isTestSuccess; } }; TEST(Calib3d_SolvePnPRansac, accuracy) { CV_solvePnPRansac_Test test; test.safe_run(); } +TEST(Calib3d_SolvePnP, accuracy) { CV_solvePnP_Test test; test.safe_run(); }