added P3P method

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

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,

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 <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;
-  int maximum_number_of_correspondences;
+  std::vector<double> pws, us, alphas, pcs;
   int number_of_correspondences;
 
   double cws[4][3], ccs[4][3];

modules/calib3d/src/p3p.h

+#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

+#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

+#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)) );
-    
+    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<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;
@@ -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);

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<Point3f>& 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++)
-            {
-                    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;
-            }
+        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 run(int)
+    void generateCameraMatrix(Mat& cameraMatrix, RNG& rng)
     {
-        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);
-        generate3DPointCloud(points);
+        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);
+    }
 
-        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++)
+    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)
             {
-                    points1[i] = points1[rand() % points.size()];
+                projectedPoints[i] = projectedPoints[rng.uniform(0,points.size()-1)];
             }
         }
-        double eps = 1.0e-7;
 
-        int successfulTestsCount = 0;
-        int totalTestsCount = 10;
+        solvePnPRansac(points, projectedPoints, intrinsics, distCoeffs, rvec, tvec,
+            false, 500, 0.5, -1, inliers, method);
 
-        for (int testIndex = 0; testIndex < totalTestsCount; testIndex++)
-        {
-            try
-            {
-                Mat rvec, tvec;
-                vector<int> inliers;
+        bool isTestSuccess = inliers.size() >= points.size()*0.95;
 
-                solvePnPRansac(points, points1, intrinsics, dist_coeffs, rvec, tvec,
-                                false, 1000, 2.0, -1, inliers);
+        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;
 
-                bool isTestSuccess = inliers.size() == 475;
+        return isTestSuccess;
+    }
 
-                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);
+    void run(int)
+    {
+        cvtest::TS& ts = *this->ts;
+        ts.set_failed_test_info(cvtest::TS::OK);
 
-                if (isTestSuccess)
-                    successfulTestsCount++;
+        vector<Point3f> points;
+        const int pointsCount = 500;
+        points.resize(pointsCount);
+        generate3DPointCloud(points);
 
-            }
-            catch(...)
+
+        const int methodsCount = 3;
+        RNG rng = ts.get_rng();
+
+
+        for (int mode = 0; mode < 2; mode++)
+        {
+            for (int method = 0; method < methodsCount; method++)
             {
-                    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<Point3f>& 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<Point3f> 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<Point3f>(points.begin(), points.begin()+4);
         }
+        else
+            opoints = points;
+
+        vector<Point2f> 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(); }