modules/calib3d/src/epnp.h

   1 #ifndef epnp_h
   2 #define epnp_h
   3
   4 #include "precomp.hpp"
   5 #include "opencv2/core/core_c.h"
   6
   7 class epnp {
   8  public:
   9   epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints);
  10   ~epnp();
  11
  12   void add_correspondence(const double X, const double Y, const double Z,
  13               const double u, const double v);
  14
  15   void compute_pose(cv::Mat& R, cv::Mat& t);
  16  private:
  17   template <typename T>
  18   void init_camera_parameters(const cv::Mat& cameraMatrix)
  19   {
  20     uc = cameraMatrix.at<T> (0, 2);
  21     vc = cameraMatrix.at<T> (1, 2);
  22     fu = cameraMatrix.at<T> (0, 0);
  23     fv = cameraMatrix.at<T> (1, 1);
  24   }
  25   template <typename OpointType, typename IpointType>
  26   void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
  27   {
  28       for(int i = 0; i < number_of_correspondences; i++)
  29       {
  30           pws[3 * i    ] = opoints.at<OpointType>(i).x;
  31           pws[3 * i + 1] = opoints.at<OpointType>(i).y;
  32           pws[3 * i + 2] = opoints.at<OpointType>(i).z;
  33
  34           us[2 * i    ] = ipoints.at<IpointType>(i).x*fu + uc;
  35           us[2 * i + 1] = ipoints.at<IpointType>(i).y*fv + vc;
  36       }
  37   }
  38   double reprojection_error(const double R[3][3], const double t[3]);
  39   void choose_control_points(void);
  40   void compute_barycentric_coordinates(void);
  41   void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v);
  42   void compute_ccs(const double * betas, const double * ut);
  43   void compute_pcs(void);
  44
  45   void solve_for_sign(void);
  46
  47   void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas);
  48   void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas);
  49   void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas);
  50   void qr_solve(CvMat * A, CvMat * b, CvMat * X);
  51
  52   double dot(const double * v1, const double * v2);
  53   double dist2(const double * p1, const double * p2);
  54
  55   void compute_rho(double * rho);
  56   void compute_L_6x10(const double * ut, double * l_6x10);
  57
  58   void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]);
  59   void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
  60                     const double cb[4], CvMat * A, CvMat * b);
  61
  62   double compute_R_and_t(const double * ut, const double * betas,
  63              double R[3][3], double t[3]);
  64
  65   void estimate_R_and_t(double R[3][3], double t[3]);
  66
  67   void copy_R_and_t(const double R_dst[3][3], const double t_dst[3],
  68             double R_src[3][3], double t_src[3]);
  69
  70
  71   double uc, vc, fu, fv;
  72
  73   std::vector<double> pws, us, alphas, pcs;
  74   int number_of_correspondences;
  75
  76   double cws[4][3], ccs[4][3];
  77   int max_nr;
  78   double * A1, * A2;
  79 };
  80
  81 #endif