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