added P3P method

added test for solvePnP
changed test for solvePnPRansac
fixed bug with mutex solvePnPRansac

modules/calib3d/include/opencv2/calib3d/calib3d.hpp

@@ -87,6 +87,13 @@ CVAPI(void) cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst );
 #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,
                                  int method CV_DEFAULT(CV_FM_RANSAC),
@@ -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,
@@ -507,7 +516,8 @@ CV_EXPORTS_W void solvePnPRansac( InputArray objectPoints,
                                   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,

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 compute_pose(cv::Mat& R, cv::Mat& t);
-
-  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]);
-
  private:
+  template <typename T>
+  void init_camera_parameters(const cv::Mat& cameraMatrix)
+  {	
+	uc = cameraMatrix.at<T> (0, 2);
+	vc = cameraMatrix.at<T> (1, 2);
+	fu = cameraMatrix.at<T> (0, 0);
+	fv = cameraMatrix.at<T> (1, 1);
+  }
+  template <typename OpointType, typename IpointType>
+  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<OpointType>(0,i).x;
+		  pws[3 * i + 1] = opoints.at<OpointType>(0,i).y;
+		  pws[3 * i + 2] = opoints.at<OpointType>(0,i).z;
+
+		  us[2 * i    ] = ipoints.at<IpointType>(0,i).x*fu + uc;
+		  us[2 * i + 1] = ipoints.at<IpointType>(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;
+  std::vector<double> pws, us, alphas, pcs;
-  int maximum_number_of_correspondences;
   int number_of_correspondences;
 
   double cws[4][3], ccs[4][3];

modules/calib3d/src/p3p.cpp

@@ -0,0 +1,416 @@
+#include <cstring>
+#include <cmath>
+#include <iostream>
+
+#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<float>(cameraMatrix);
+	else
+		init_camera_parameters<double>(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<double> points;
+	if (opoints.depth() == ipoints.depth())
+	{
+		if (opoints.depth() == CV_32F)
+			extract_points<cv::Point3f,cv::Point2f>(opoints, ipoints, points);
+		else
+			extract_points<cv::Point3d,cv::Point2d>(opoints, ipoints, points);
+	}
+	else if (opoints.depth() == CV_32F)
+		extract_points<cv::Point3f,cv::Point2d>(opoints, ipoints, points);
+	else
+		extract_points<cv::Point3d,cv::Point2f>(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;
+}
+

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 <typename T>
+  void init_camera_parameters(const cv::Mat& cameraMatrix)
+  {	
+	cx = cameraMatrix.at<T> (0, 2);
+	cy = cameraMatrix.at<T> (1, 2);
+	fx = cameraMatrix.at<T> (0, 0);
+	fy = cameraMatrix.at<T> (1, 1);
+  }
+  template <typename OpointType, typename IpointType>
+  void extract_points(const cv::Mat& opoints, const cv::Mat& ipoints, std::vector<double>& points)
+  {
+	  points.clear();
+	  points.resize(20);
+	  for(int i = 0; i < 4; i++)
+	  {
+		  points[i*5] = ipoints.at<IpointType>(0,i).x*fx + cx;
+		  points[i*5+1] = ipoints.at<IpointType>(0,i).y*fy + cy;
+		  points[i*5+2] = opoints.at<OpointType>(0,i).x;
+		  points[i*5+3] = opoints.at<OpointType>(0,i).y;
+		  points[i*5+4] = opoints.at<OpointType>(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
+

modules/calib3d/src/polynom_solver.cpp

@@ -0,0 +1,170 @@
+#include <math.h>
+#include <iostream>
+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;
+}

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

modules/calib3d/src/solvepnp.cpp

@@ -41,25 +41,62 @@
  //M*/
 
 #include "precomp.hpp"
+#include "epnp.h"
+#include "p3p.h"
+#include <iostream>
 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)) ); 
-    
     _rvec.create(3, 1, CV_64F);
     _tvec.create(3, 1, CV_64F);
     Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
+
+    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;
         };
         
@@ -160,7 +198,9 @@ namespace cv
             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<Point2f> 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<int> localInliers;
     Mat localRvec, localTvec;
@@ -301,6 +342,8 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
     }
     
     if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT)
+    {
+		if (flags != CV_P3P)
 		{
 			int i, pointsCount = (int)localInliers.size();
 			Mat inlierObjectPoints(1, pointsCount, CV_32FC3), inlierImagePoints(1, pointsCount, CV_32FC2);
@@ -312,7 +355,8 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
 				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);
+			solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, true, flags);
+		}
 		localRvec.copyTo(rvec);
         localTvec.copyTo(tvec);
         if (_inliers.needed())

modules/calib3d/test/test_solvepnp_ransac.cpp

@@ -48,7 +48,13 @@ 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<Point3f>& points, Point3f pmin = Point3f(-1,
@@ -67,81 +73,169 @@ protected:
         }
     }
 
+    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<double>(0,0) = rng.uniform(fcMinVal, fcMaxVal);
+        cameraMatrix.at<double>(1,1) = rng.uniform(fcMinVal, fcMaxVal);
+        cameraMatrix.at<double>(0,2) = rng.uniform(fcMinVal, fcMaxVal);
+        cameraMatrix.at<double>(1,2) = rng.uniform(fcMinVal, fcMaxVal);
+        cameraMatrix.at<double>(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<double>(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<double>(i,0) = rng.uniform(minVal, maxVal);
+            tvec.at<double>(i,0) = rng.uniform(minVal, maxVal/10);
+        }
+    }
+
+    virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, const double* eps, double& maxError)
+    {
+        Mat rvec, tvec;
+        vector<int> 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<Point2f> 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)
+            {
+                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<float> (0, 0) = 400.0;
-        intrinsics.at<float> (1, 1) = 400.0;
-        intrinsics.at<float> (0, 2) = 640 / 2;
-        intrinsics.at<float> (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<double> (0, 0) = 1.0f;
-        tvec1.at<double> (0, 0) = 1.0f;
-        tvec1.at<double> (1, 0) = 1.0f;
         vector<Point3f> points;
-        points.resize(500);
+        const int pointsCount = 500;
+        points.resize(pointsCount);
         generate3DPointCloud(points);
 
-        vector<Point2f> points1;
-        points1.resize(points.size());
-        projectPoints(Mat(points), rvec1, tvec1, intrinsics, dist_coeffs, points1);
-        for (size_t i = 0; i < points1.size(); i++)
-        {
-            if (i % 20 == 0)
-            {
-                    points1[i] = points1[rand() % points.size()];
-            }
-        }
-        double eps = 1.0e-7;
 
+        const int methodsCount = 3;
+        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;
-        int totalTestsCount = 10;
-
                 for (int testIndex = 0; testIndex < totalTestsCount; testIndex++)
                 {
-            try
+                    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;
+};
+
+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<Point3f>& points, const double* eps, double& maxError)
     {
         Mat rvec, tvec;
-                vector<int> 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);
 
-                solvePnPRansac(points, points1, intrinsics, dist_coeffs, rvec, tvec,
+        std::vector<Point3f> opoints;
-                                false, 1000, 2.0, -1, inliers);
+        if (method == 2)
-
-                bool isTestSuccess = inliers.size() == 475;
-
-                isTestSuccess = isTestSuccess
-                                && (abs(rvec.at<double> (0, 0) - 1) < eps);
-                isTestSuccess = isTestSuccess && (abs(rvec.at<double> (1, 0)) < eps);
-                isTestSuccess = isTestSuccess && (abs(rvec.at<double> (2, 0)) < eps);
-                isTestSuccess = isTestSuccess
-                                && (abs(tvec.at<double> (0, 0) - 1) < eps);
-                isTestSuccess = isTestSuccess
-                                && (abs(tvec.at<double> (1, 0) - 1) < eps);
-                isTestSuccess = isTestSuccess && (abs(tvec.at<double> (2, 0)) < eps);
-
-                if (isTestSuccess)
-                    successfulTestsCount++;
-
-            }
-            catch(...)
         {
-                    ts.printf(cvtest::TS::LOG, "Exception in solvePnPRansac\n");
+            opoints = std::vector<Point3f>(points.begin(), points.begin()+4);
-                    ts.set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
-            }
         }
+        else
+            opoints = points;
 
-        if (successfulTestsCount < 0.8*totalTestsCount)
+        vector<Point2f> projectedPoints;
-        {
+        projectedPoints.resize(opoints.size());
-            ts.printf( cvtest::TS::LOG, "Invalid accuracy, failed %d tests from %d\n",
+        projectPoints(Mat(opoints), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);
-                totalTestsCount - successfulTestsCount, totalTestsCount);
+
-            ts.set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
+        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(); }