1 //M*////////////////////////////////////////////////////////////////////////////////////// 2 // 3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 // 5 // By downloading, copying, installing or using the software you agree to this license. 6 // If you do not agree to this license, do not download, install, 7 // copy or use the software. 8 // 9 // 10 // License Agreement 11 // For Open Source Computer Vision Library 12 // 13 // Copyright (C) 2000, Intel Corporation, all rights reserved. 14 // Copyright (C) 2013, OpenCV Foundation, all rights reserved. 15 // Third party copyrights are property of their respective owners. 16 // 17 // Redistribution and use in source and binary forms, with or without modification, 18 // are permitted provided that the following conditions are met: 19 // 20 // * Redistribution's of source code must retain the above copyright notice, 21 // this list of conditions and the following disclaimer. 22 // 23 // * Redistribution's in binary form must reproduce the above copyright notice, 24 // this list of conditions and the following disclaimer in the documentation 25 // and/or other materials provided with the distribution. 26 // 27 // * The name of the copyright holders may not be used to endorse or promote products 28 // derived from this software without specific prior written permission. 29 // 30 // This software is provided by the copyright holders and contributors "as is" and 31 // any express or implied warranties, including, but not limited to, the implied 32 // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 // In no event shall the Intel Corporation or contributors be liable for any direct, 34 // indirect, incidental, special, exemplary, or consequential damages 35 // (including, but not limited to, procurement of substitute goods or services; 36 // loss of use, data, or profits; or business interruption) however caused 37 // and on any theory of liability, whether in contract, strict liability, 38 // or tort (including negligence or otherwise) arising in any way out of 39 // the use of this software, even if advised of the possibility of such damage. 40 // 41 //M*/ 42 43 /****************************************************************************************\ 44 * Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation. 45 * Contributed by Edgar Riba 46 \****************************************************************************************/ 47 48 #ifndef OPENCV_CALIB3D_UPNP_H_ 49 #define OPENCV_CALIB3D_UPNP_H_ 50 51 #include "precomp.hpp" 52 #include "opencv2/core/core_c.h" 53 #include <iostream> 54 55 class upnp 56 { 57 public: 58 upnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints); 59 ~upnp(); 60 61 double compute_pose(cv::Mat& R, cv::Mat& t); 62 private: 63 template <typename T> 64 void init_camera_parameters(const cv::Mat& cameraMatrix) 65 { 66 uc = cameraMatrix.at<T> (0, 2); 67 vc = cameraMatrix.at<T> (1, 2); 68 fu = 1; 69 fv = 1; 70 } 71 template <typename OpointType, typename IpointType> 72 void init_points(const cv::Mat& opoints, const cv::Mat& ipoints) 73 { 74 for(int i = 0; i < number_of_correspondences; i++) 75 { 76 pws[3 * i ] = opoints.at<OpointType>(i).x; 77 pws[3 * i + 1] = opoints.at<OpointType>(i).y; 78 pws[3 * i + 2] = opoints.at<OpointType>(i).z; 79 80 us[2 * i ] = ipoints.at<IpointType>(i).x; 81 us[2 * i + 1] = ipoints.at<IpointType>(i).y; 82 } 83 } 84 85 double reprojection_error(const double R[3][3], const double t[3]); 86 void choose_control_points(); 87 void compute_alphas(); 88 void fill_M(cv::Mat * M, const int row, const double * alphas, const double u, const double v); 89 void compute_ccs(const double * betas, const double * ut); 90 void compute_pcs(void); 91 92 void solve_for_sign(void); 93 94 void find_betas_and_focal_approx_1(cv::Mat * Ut, cv::Mat * Rho, double * betas, double * efs); 95 void find_betas_and_focal_approx_2(cv::Mat * Ut, cv::Mat * Rho, double * betas, double * efs); 96 void qr_solve(cv::Mat * A, cv::Mat * b, cv::Mat * X); 97 98 cv::Mat compute_constraint_distance_2param_6eq_2unk_f_unk(const cv::Mat& M1); 99 cv::Mat compute_constraint_distance_3param_6eq_6unk_f_unk(const cv::Mat& M1, const cv::Mat& M2); 100 void generate_all_possible_solutions_for_f_unk(const double betas[5], double solutions[18][3]); 101 102 double sign(const double v); 103 double dot(const double * v1, const double * v2); 104 double dotXY(const double * v1, const double * v2); 105 double dotZ(const double * v1, const double * v2); 106 double dist2(const double * p1, const double * p2); 107 108 void compute_rho(double * rho); 109 void compute_L_6x12(const double * ut, double * l_6x12); 110 111 void gauss_newton(const cv::Mat * L_6x12, const cv::Mat * Rho, double current_betas[4], double * efs); 112 void compute_A_and_b_gauss_newton(const double * l_6x12, const double * rho, 113 const double cb[4], cv::Mat * A, cv::Mat * b, double const f); 114 115 double compute_R_and_t(const double * ut, const double * betas, 116 double R[3][3], double t[3]); 117 118 void estimate_R_and_t(double R[3][3], double t[3]); 119 120 void copy_R_and_t(const double R_dst[3][3], const double t_dst[3], 121 double R_src[3][3], double t_src[3]); 122 123 124 double uc, vc, fu, fv; 125 126 std::vector<double> pws, us, alphas, pcs; 127 int number_of_correspondences; 128 129 double cws[4][3], ccs[4][3]; 130 int max_nr; 131 double * A1, * A2; 132 }; 133 134 #endif // OPENCV_CALIB3D_UPNP_H_ 135
