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 epnp(const epnp &); // copy disabled 21 epnp& operator=(const epnp &); // assign disabled 22 template <typename T> init_camera_parameters(const cv::Mat & cameraMatrix)23 void init_camera_parameters(const cv::Mat& cameraMatrix) 24 { 25 uc = cameraMatrix.at<T> (0, 2); 26 vc = cameraMatrix.at<T> (1, 2); 27 fu = cameraMatrix.at<T> (0, 0); 28 fv = cameraMatrix.at<T> (1, 1); 29 } 30 template <typename OpointType, typename IpointType> init_points(const cv::Mat & opoints,const cv::Mat & ipoints)31 void init_points(const cv::Mat& opoints, const cv::Mat& ipoints) 32 { 33 for(int i = 0; i < number_of_correspondences; i++) 34 { 35 pws[3 * i ] = opoints.at<OpointType>(i).x; 36 pws[3 * i + 1] = opoints.at<OpointType>(i).y; 37 pws[3 * i + 2] = opoints.at<OpointType>(i).z; 38 39 us[2 * i ] = ipoints.at<IpointType>(i).x*fu + uc; 40 us[2 * i + 1] = ipoints.at<IpointType>(i).y*fv + vc; 41 } 42 } 43 double reprojection_error(const double R[3][3], const double t[3]); 44 void choose_control_points(void); 45 void compute_barycentric_coordinates(void); 46 void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v); 47 void compute_ccs(const double * betas, const double * ut); 48 void compute_pcs(void); 49 50 void solve_for_sign(void); 51 52 void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas); 53 void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas); 54 void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas); 55 void qr_solve(CvMat * A, CvMat * b, CvMat * X); 56 57 double dot(const double * v1, const double * v2); 58 double dist2(const double * p1, const double * p2); 59 60 void compute_rho(double * rho); 61 void compute_L_6x10(const double * ut, double * l_6x10); 62 63 void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]); 64 void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho, 65 const double cb[4], CvMat * A, CvMat * b); 66 67 double compute_R_and_t(const double * ut, const double * betas, 68 double R[3][3], double t[3]); 69 70 void estimate_R_and_t(double R[3][3], double t[3]); 71 72 void copy_R_and_t(const double R_dst[3][3], const double t_dst[3], 73 double R_src[3][3], double t_src[3]); 74 75 76 double uc, vc, fu, fv; 77 78 std::vector<double> pws, us, alphas, pcs; 79 int number_of_correspondences; 80 81 double cws[4][3], ccs[4][3]; 82 int max_nr; 83 double * A1, * A2; 84 }; 85 86 } 87 88 #endif 89