Main MRPT website > C++ reference for MRPT 1.5.9
upnp.h
Go to the documentation of this file.
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 <mrpt/config.h>
52 #include <mrpt/otherlibs/do_opencv_includes.h>
53 
54 #if MRPT_HAS_OPENCV
55  //#include <opencv2/core/core_c.h>
56  #include <iostream>
57 
58  namespace mrpt
59  {
60  namespace vision
61  {
62  namespace pnp
63  {
64  /** \addtogroup pnp Perspective-n-Point pose estimation
65  * \ingroup mrpt_vision_grp
66  * @{
67  */
68 
69  /**
70  * @class upnp
71  * @author Chandra Mangipudi
72  * @date 12/08/16
73  * @file upnp.h
74  * @brief Unified PnP - Eigen Wrapper for OpenCV function
75  */
76  class upnp
77  {
78  public:
79  //! Constructor for UPnP class
80  upnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints);
81 
82  //! Destructor for UPnP class
83  ~upnp();
84 
85  /**
86  * @brief Function to compute pose
87  * @param[out] R Rotation Matrix
88  * @param[out] t Translation Vector
89  * @return
90  */
91  double compute_pose(cv::Mat& R, cv::Mat& t);
92  private:
93 
94  /**
95  * @brief Initialize camera variables using camera intrinsic matrix
96  * @param[in] cameraMatrix Camera Intrinsic Matrix
97  */
98  template <typename T>
99  void init_camera_parameters(const cv::Mat& cameraMatrix)
100  {
101  uc = cameraMatrix.at<T> (0, 2);
102  vc = cameraMatrix.at<T> (1, 2);
103  fu = 1;
104  fv = 1;
105  }
106 
107  /**
108  * @brief Iniialize Object points and image points from OpenCV Matrix
109  * @param[in] opoints Object Points
110  * @param[in] ipoints Image Points
111  */
112  template <typename OpointType, typename IpointType>
113  void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
114  {
115  for(int i = 0; i < number_of_correspondences; i++)
116  {
117  pws[3 * i ] = opoints.at<OpointType>(i).x;
118  pws[3 * i + 1] = opoints.at<OpointType>(i).y;
119  pws[3 * i + 2] = opoints.at<OpointType>(i).z;
120 
121  us[2 * i ] = ipoints.at<IpointType>(i).x;
122  us[2 * i + 1] = ipoints.at<IpointType>(i).y;
123  }
124  }
125 
126  /**
127  * @brief Compute the reprojection error using the estimated Rotation matrix and Translation Vector
128  * @param[in] R Rotation matrix
129  * @param[in] t Trnaslation Vector
130  * @return Linear least squares error in pixels
131  */
132  double reprojection_error(const double R[3][3], const double t[3]);
133 
134  /**
135  * @brief Function to select 4 control points
136  */
137  void choose_control_points();
138 
139  /**
140  * @brief Function to comput @alphas
141  */
142  void compute_alphas();
143 
144  /**
145  * @brief Function to compute Maucaulay matrix M
146  * @param[out] M Maucaulay matrix
147  * @param[in] row Internal member
148  * @param[in] alphas Internal member
149  * @param[in] u Image pixel x co-ordinate
150  * @param[in] v Image pixel y co-ordinate
151  */
152  void fill_M(cv::Mat * M, const int row, const double * alphas, const double u, const double v);
153 
154  /**
155  * @brief Compute the control points
156  * @param[in] betas Internal member variable
157  * @param[in] ut Internal member variable
158  */
159  void compute_ccs(const double * betas, const double * ut);
160 
161  /**
162  * @brief Compute object points based on control points
163  */
164  void compute_pcs(void);
165 
166  /**
167  * @brief Internal member function
168  */
169  void solve_for_sign(void);
170 
171  /**
172  * @brief Function to approximately calculate betas and focal length
173  * @param[in] Ut
174  * @param[in] Rho
175  * @param[out] betas
176  * @param[out] efs
177  */
178  void find_betas_and_focal_approx_1(cv::Mat * Ut, cv::Mat * Rho, double * betas, double * efs);
179 
180  /**
181  * @brief Function to calculate betas and focal length (more accurate)
182  * @param[in] Ut
183  * @param[in] Rho
184  * @param[out] betas
185  * @param[out] efs
186  */
187  void find_betas_and_focal_approx_2(cv::Mat * Ut, cv::Mat * Rho, double * betas, double * efs);
188 
189  /**
190  * @brief Function to do a QR decomposition
191  * @param[in] A Matrix to be decomposed
192  * @param[out] b
193  * @param[out] X
194  */
195  void qr_solve(cv::Mat * A, cv::Mat * b, cv::Mat * X);
196 
197  /**
198  * @brief Internal function
199  * @param[in] M1
200  * @return Distance
201  */
202  cv::Mat compute_constraint_distance_2param_6eq_2unk_f_unk(const cv::Mat& M1);
203 
204  /**
205  * @brief Internal function
206  * @param[in] M1
207  * @param[in] M2
208  * @return Distance
209  */
210  cv::Mat compute_constraint_distance_3param_6eq_6unk_f_unk(const cv::Mat& M1, const cv::Mat& M2);
211 
212  /**
213  * @brief Get all possible solutions
214  * @param[in] betas
215  * @param[out] solutions
216  */
217  void generate_all_possible_solutions_for_f_unk(const double betas[5], double solutions[18][3]);
218 
219  /**
220  * @brief Return the sign of the scalar
221  * @param[in] v
222  * @return True if positive else Flase
223  */
224  double sign(const double v);
225 
226  /**
227  * @brief Compute the dot product between two vectors
228  * @param[in] v1
229  * @param[in] v2
230  * @return Dot product
231  */
232  double dot(const double * v1, const double * v2);
233 
234  /**
235  * @brief Compute dot product in 2D with only x and y components
236  * @param[in] v1
237  * @param[in] v2
238  * @return Dot product
239  */
240  double dotXY(const double * v1, const double * v2);
241 
242  /**
243  * @brief Compute the dot product using only z component
244  * @param[in] v1
245  * @param[in] v2
246  * @return Dot product
247  */
248  double dotZ(const double * v1, const double * v2);
249 
250  /**
251  * @brief Compute the euclidean distance squared between two points in 3D
252  * @param[in] p1
253  * @param[in] p2
254  * @return Distance squared
255  */
256  double dist2(const double * p1, const double * p2);
257 
258  /**
259  * @brief Internal fucntion
260  * @param[out] rho
261  */
262  void compute_rho(double * rho);
263 
264  /**
265  * @brief Internal function
266  * @param[in] ut
267  * @param[out] l_6x12
268  */
269  void compute_L_6x12(const double * ut, double * l_6x12);
270 
271 
272  /**
273  * @brief Gauss Newton Iterative optimization
274  * @param[in] L_6x12
275  * @param[in] Rho
276  * @param[in,out] current_betas
277  * @param[out] efs
278  */
279  void gauss_newton(const cv::Mat * L_6x12, const cv::Mat * Rho, double current_betas[4], double * efs);
280 
281  /**
282  * @brief Compute matrix A and vector b
283  * @param[in] l_6x12
284  * @param[in] rho
285  * @param[in] cb
286  * @param[out] A
287  * @param[out] b
288  * @param[in] f
289  */
290  void compute_A_and_b_gauss_newton(const double * l_6x12, const double * rho,
291  const double cb[4], cv::Mat * A, cv::Mat * b, double const f);
292 
293  /**
294  * @brief Function to compute the pose
295  * @param[in] ut
296  * @param[in] betas
297  * @param[out] R Rotation Matrix
298  * @param[out] t Translation VectorS
299  * @return
300  */
301  double compute_R_and_t(const double * ut, const double * betas,
302  double R[3][3], double t[3]);
303 
304  /**
305  * @brief Helper function to function compute_R_and_t()
306  * @param[out] R Rotaiton matrix
307  * @param[out] t Translation vector
308  */
309  void estimate_R_and_t(double R[3][3], double t[3]);
310 
311  /**
312  * @brief Function to copy the pose
313  * @param[in] R_dst
314  * @param[in] t_dst
315  * @param[out] R_src
316  * @param[out] t_src
317  */
318  void copy_R_and_t(const double R_dst[3][3], const double t_dst[3],
319  double R_src[3][3], double t_src[3]);
320 
321 
322  double uc; //! Image center in x-direction
323  double vc; //! Image center in y-direction
324  double fu; //! Focal length in x-direction
325  double fv; //! Focal length in y-direction
326 
327  std::vector<double> pws; //! Object points
328  std::vector<double> us; //! Image points
329  std::vector<double> alphas, pcs; //! Internal variable
330  int number_of_correspondences;//! Number of 2d/3d correspondences
331 
332  double cws[4][3], ccs[4][3]; //! Control point variables
333  int max_nr; //! Internal variable
334  double * A1, * A2; //! Internal variable
335  };
336 
337  /** @} */ // end of grouping
338 
339  }
340  }
341  }
342 #endif // Check for OPENCV_LIB
343 #endif // OPENCV_CALIB3D_UPNP_H_
mrpt::vision::pnp::upnp::compute_R_and_t
double compute_R_and_t(const double *ut, const double *betas, double R[3][3], double t[3])
Function to compute the pose.
mrpt::vision::pnp::upnp::fill_M
void fill_M(cv::Mat *M, const int row, const double *alphas, const double u, const double v)
Function to compute Maucaulay matrix M.
t
GLdouble GLdouble t
Definition: glext.h:3610
z
GLdouble GLdouble z
Definition: glext.h:3734
mrpt::vision::pnp::upnp::init_points
void init_points(const cv::Mat &opoints, const cv::Mat &ipoints)
Iniialize Object points and image points from OpenCV Matrix.
Definition: upnp.h:113
mrpt::vision::pnp::upnp::estimate_R_and_t
void estimate_R_and_t(double R[3][3], double t[3])
Helper function to function compute_R_and_t()
mrpt::vision::pnp::upnp::ccs
double ccs[4][3]
Definition: upnp.h:332
mrpt::vision::pnp::upnp::copy_R_and_t
void copy_R_and_t(const double R_dst[3][3], const double t_dst[3], double R_src[3][3], double t_src[3])
Function to copy the pose.
mrpt::vision::pnp::upnp::pws
std::vector< double > pws
Focal length in y-direction.
Definition: upnp.h:327
mrpt::vision::pnp::upnp::cws
double cws[4][3]
Number of 2d/3d correspondences.
Definition: upnp.h:332
mrpt::vision::pnp::upnp::compute_pcs
void compute_pcs(void)
Compute object points based on control points.
mrpt::vision::pnp::upnp::compute_ccs
void compute_ccs(const double *betas, const double *ut)
Compute the control points.
mrpt::vision::pnp::upnp::compute_alphas
void compute_alphas()
Function to comput .
mrpt::vision::pnp::upnp::pcs
std::vector< double > pcs
Definition: upnp.h:329
mrpt::vision::pnp::upnp::number_of_correspondences
int number_of_correspondences
Internal variable.
Definition: upnp.h:330
mrpt::vision::pnp::upnp::dotXY
double dotXY(const double *v1, const double *v2)
Compute dot product in 2D with only x and y components.
mrpt::vision::pnp::upnp::find_betas_and_focal_approx_1
void find_betas_and_focal_approx_1(cv::Mat *Ut, cv::Mat *Rho, double *betas, double *efs)
Function to approximately calculate betas and focal length.
mrpt::vision::pnp::upnp::compute_pose
double compute_pose(cv::Mat &R, cv::Mat &t)
Function to compute pose.
mrpt::vision::pnp::upnp::alphas
std::vector< double > alphas
Image points.
Definition: upnp.h:329
mrpt::vision::pnp::upnp::qr_solve
void qr_solve(cv::Mat *A, cv::Mat *b, cv::Mat *X)
Function to do a QR decomposition.
mrpt::vision::pnp::upnp::dot
double dot(const double *v1, const double *v2)
Compute the dot product between two vectors.
mrpt::vision::pnp::upnp::dotZ
double dotZ(const double *v1, const double *v2)
Compute the dot product using only z component.
mrpt::vision::pnp::upnp::upnp
upnp(const cv::Mat &cameraMatrix, const cv::Mat &opoints, const cv::Mat &ipoints)
Constructor for UPnP class.
mrpt::vision::pnp::upnp::generate_all_possible_solutions_for_f_unk
void generate_all_possible_solutions_for_f_unk(const double betas[5], double solutions[18][3])
Get all possible solutions.
mrpt::vision::pnp::upnp::dist2
double dist2(const double *p1, const double *p2)
Compute the euclidean distance squared between two points in 3D.
b
GLubyte GLubyte b
Definition: glext.h:5575
mrpt::vision::pnp::upnp::compute_constraint_distance_3param_6eq_6unk_f_unk
cv::Mat compute_constraint_distance_3param_6eq_6unk_f_unk(const cv::Mat &M1, const cv::Mat &M2)
Internal function.
mrpt::vision::pnp::upnp::sign
double sign(const double v)
Return the sign of the scalar.
mrpt::vision::pnp::upnp::compute_A_and_b_gauss_newton
void compute_A_and_b_gauss_newton(const double *l_6x12, const double *rho, const double cb[4], cv::Mat *A, cv::Mat *b, double const f)
Compute matrix A and vector b.
mrpt::vision::pnp::upnp::init_camera_parameters
void init_camera_parameters(const cv::Mat &cameraMatrix)
Initialize camera variables using camera intrinsic matrix.
Definition: upnp.h:99
v
const GLdouble * v
Definition: glext.h:3603
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
v1
GLfloat GLfloat v1
Definition: glext.h:3922
R
const float R
Definition: CKinematicChain.cpp:141
mrpt::vision::pnp::upnp::us
std::vector< double > us
Object points.
Definition: upnp.h:328
row
GLenum GLenum GLvoid * row
Definition: glext.h:3533
mrpt::vision::pnp::upnp::find_betas_and_focal_approx_2
void find_betas_and_focal_approx_2(cv::Mat *Ut, cv::Mat *Rho, double *betas, double *efs)
Function to calculate betas and focal length (more accurate)
mrpt::vision::pnp::upnp::max_nr
int max_nr
Control point variables.
Definition: upnp.h:333
mrpt::vision::pnp::upnp::vc
double vc
Image center in x-direction.
Definition: upnp.h:323
y
GLenum GLint GLint y
Definition: glext.h:3516
mrpt::vision::pnp::upnp::~upnp
~upnp()
Destructor for UPnP class.
v2
GLfloat GLfloat GLfloat v2
Definition: glext.h:3923
x
GLenum GLint x
Definition: glext.h:3516
mrpt::vision::pnp::upnp::compute_constraint_distance_2param_6eq_2unk_f_unk
cv::Mat compute_constraint_distance_2param_6eq_2unk_f_unk(const cv::Mat &M1)
Internal function.
mrpt::vision::pnp::upnp::compute_rho
void compute_rho(double *rho)
Internal fucntion.
mrpt::vision::pnp::upnp::compute_L_6x12
void compute_L_6x12(const double *ut, double *l_6x12)
Internal function.
mrpt::vision::pnp::upnp::reprojection_error
double reprojection_error(const double R[3][3], const double t[3])
Compute the reprojection error using the estimated Rotation matrix and Translation Vector...
mrpt::vision::pnp::upnp::choose_control_points
void choose_control_points()
Function to select 4 control points.
mrpt::vision::pnp::upnp::solve_for_sign
void solve_for_sign(void)
Internal member function.
mrpt::vision::pnp::upnp::A1
double * A1
Internal variable.
Definition: upnp.h:334
mrpt::vision::pnp::upnp::fu
double fu
Image center in y-direction.
Definition: upnp.h:324
mrpt::vision::pnp::upnp::gauss_newton
void gauss_newton(const cv::Mat *L_6x12, const cv::Mat *Rho, double current_betas[4], double *efs)
Gauss Newton Iterative optimization.
mrpt::vision::pnp::upnp::fv
double fv
Focal length in x-direction.
Definition: upnp.h:325


Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at mié abr 15 19:30:12 CEST 2020