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 #if 0 // fix buffer overflow first (FIXIT mark in .cpp file) 56 57 class upnp 58 { 59 public: 60 upnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints); 61 ~upnp(); 62 63 double compute_pose(cv::Mat& R, cv::Mat& t); 64 private: 65 upnp(const upnp &); // copy disabled 66 upnp& operator=(const upnp &); // assign disabled 67 template <typename T> 68 void init_camera_parameters(const cv::Mat& cameraMatrix) 69 { 70 uc = cameraMatrix.at<T> (0, 2); 71 vc = cameraMatrix.at<T> (1, 2); 72 fu = 1; 73 fv = 1; 74 } 75 template <typename OpointType, typename IpointType> 76 void init_points(const cv::Mat& opoints, const cv::Mat& ipoints) 77 { 78 for(int i = 0; i < number_of_correspondences; i++) 79 { 80 pws[3 * i ] = opoints.at<OpointType>(i).x; 81 pws[3 * i + 1] = opoints.at<OpointType>(i).y; 82 pws[3 * i + 2] = opoints.at<OpointType>(i).z; 83 84 us[2 * i ] = ipoints.at<IpointType>(i).x; 85 us[2 * i + 1] = ipoints.at<IpointType>(i).y; 86 } 87 } 88 89 double reprojection_error(const double R[3][3], const double t[3]); 90 void choose_control_points(); 91 void compute_alphas(); 92 void fill_M(cv::Mat * M, const int row, const double * alphas, const double u, const double v); 93 void compute_ccs(const double * betas, const double * ut); 94 void compute_pcs(void); 95 96 void solve_for_sign(void); 97 98 void find_betas_and_focal_approx_1(cv::Mat * Ut, cv::Mat * Rho, double * betas, double * efs); 99 void find_betas_and_focal_approx_2(cv::Mat * Ut, cv::Mat * Rho, double * betas, double * efs); 100 void qr_solve(cv::Mat * A, cv::Mat * b, cv::Mat * X); 101 102 cv::Mat compute_constraint_distance_2param_6eq_2unk_f_unk(const cv::Mat& M1); 103 cv::Mat compute_constraint_distance_3param_6eq_6unk_f_unk(const cv::Mat& M1, const cv::Mat& M2); 104 void generate_all_possible_solutions_for_f_unk(const double betas[5], double solutions[18][3]); 105 106 double sign(const double v); 107 double dot(const double * v1, const double * v2); 108 double dotXY(const double * v1, const double * v2); 109 double dotZ(const double * v1, const double * v2); 110 double dist2(const double * p1, const double * p2); 111 112 void compute_rho(double * rho); 113 void compute_L_6x12(const double * ut, double * l_6x12); 114 115 void gauss_newton(const cv::Mat * L_6x12, const cv::Mat * Rho, double current_betas[4], double * efs); 116 void compute_A_and_b_gauss_newton(const double * l_6x12, const double * rho, 117 const double cb[4], cv::Mat * A, cv::Mat * b, double const f); 118 119 double compute_R_and_t(const double * ut, const double * betas, 120 double R[3][3], double t[3]); 121 122 void estimate_R_and_t(double R[3][3], double t[3]); 123 124 void copy_R_and_t(const double R_dst[3][3], const double t_dst[3], 125 double R_src[3][3], double t_src[3]); 126 127 128 double uc, vc, fu, fv; 129 130 std::vector<double> pws, us, alphas, pcs; 131 int number_of_correspondences; 132 133 double cws[4][3], ccs[4][3]; 134 int max_nr; 135 double * A1, * A2; 136 }; 137 138 #endif 139 140 #endif // OPENCV_CALIB3D_UPNP_H_ 141