MRPT  2.0.4
upnp.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 // M*//////////////////////////////////////////////////////////////////////////////////////
10 //
11 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
12 //
13 // By downloading, copying, installing or using the software you agree to this
14 // license.
15 // If you do not agree to this license, do not download, install,
16 // copy or use the software.
17 //
18 //
19 // License Agreement
20 // For Open Source Computer Vision Library
21 //
22 // Copyright (C) 2000, Intel Corporation, all rights reserved.
23 // Copyright (C) 2013, OpenCV Foundation, all rights reserved.
24 // Third party copyrights are property of their respective owners.
25 //
26 // Redistribution and use in source and binary forms, with or without
27 // modification,
28 // are permitted provided that the following conditions are met:
29 //
30 // * Redistribution's of source code must retain the above copyright notice,
31 // this list of conditions and the following disclaimer.
32 //
33 // * Redistribution's in binary form must reproduce the above copyright notice,
34 // this list of conditions and the following disclaimer in the documentation
35 // and/or other materials provided with the distribution.
36 //
37 // * The name of the copyright holders may not be used to endorse or promote
38 // products
39 // derived from this software without specific prior written permission.
40 //
41 // This software is provided by the copyright holders and contributors "as is"
42 // and
43 // any express or implied warranties, including, but not limited to, the implied
44 // warranties of merchantability and fitness for a particular purpose are
45 // disclaimed.
46 // In no event shall the Intel Corporation or contributors be liable for any
47 // direct,
48 // indirect, incidental, special, exemplary, or consequential damages
49 // (including, but not limited to, procurement of substitute goods or services;
50 // loss of use, data, or profits; or business interruption) however caused
51 // and on any theory of liability, whether in contract, strict liability,
52 // or tort (including negligence or otherwise) arising in any way out of
53 // the use of this software, even if advised of the possibility of such damage.
54 //
55 // M*/
56 
57 /****************************************************************************************\
58 * Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation.
59 * Contributed by Edgar Riba
60 \****************************************************************************************/
61 
62 #pragma once
63 
64 #include <mrpt/3rdparty/do_opencv_includes.h>
65 #include <mrpt/config.h>
66 
67 #if MRPT_HAS_OPENCV
68 //#include <opencv2/core/core_c.h>
69 #include <iostream>
70 
71 namespace mrpt::vision::pnp
72 {
73 /** \addtogroup pnp Perspective-n-Point pose estimation
74  * \ingroup mrpt_vision_grp
75  * @{
76  */
77 
78 /**
79  * @class upnp
80  * @author Chandra Mangipudi
81  * @date 12/08/16
82  * @file upnp.h
83  * @brief Unified PnP - Eigen Wrapper for OpenCV function
84  */
85 class upnp
86 {
87  public:
88  //! Constructor for UPnP class
89  upnp(
90  const cv::Mat& cameraMatrix, const cv::Mat& opoints,
91  const cv::Mat& ipoints);
92 
93  //! Destructor for UPnP class
94  ~upnp();
95 
96  /**
97  * @brief Function to compute pose
98  * @param[out] R Rotation Matrix
99  * @param[out] t Translation Vector
100  * @return
101  */
102  double compute_pose(cv::Mat& R, cv::Mat& t);
103 
104  private:
105  /**
106  * @brief Initialize camera variables using camera intrinsic matrix
107  * @param[in] cameraMatrix Camera Intrinsic Matrix
108  */
109  template <typename T>
110  void init_camera_parameters(const cv::Mat& cameraMatrix)
111  {
112  uc = cameraMatrix.at<T>(0, 2);
113  vc = cameraMatrix.at<T>(1, 2);
114  fu = 1;
115  fv = 1;
116  }
117 
118  /**
119  * @brief Iniialize Object points and image points from OpenCV Matrix
120  * @param[in] opoints Object Points
121  * @param[in] ipoints Image Points
122  */
123  template <typename OpointType, typename IpointType>
124  void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
125  {
126  for (int i = 0; i < number_of_correspondences; i++)
127  {
128  pws[3 * i] = opoints.at<OpointType>(i).x;
129  pws[3 * i + 1] = opoints.at<OpointType>(i).y;
130  pws[3 * i + 2] = opoints.at<OpointType>(i).z;
131 
132  us[2 * i] = ipoints.at<IpointType>(i).x;
133  us[2 * i + 1] = ipoints.at<IpointType>(i).y;
134  }
135  }
136 
137  /**
138  * @brief Compute the reprojection error using the estimated Rotation matrix
139  * and Translation Vector
140  * @param[in] R Rotation matrix
141  * @param[in] t Trnaslation Vector
142  * @return Linear least squares error in pixels
143  */
144  double reprojection_error(const double R[3][3], const double t[3]);
145 
146  /**
147  * @brief Function to select 4 control points
148  */
149  void choose_control_points();
150 
151  /**
152  * @brief Function to comput @alphas
153  */
154  void compute_alphas();
155 
156  /**
157  * @brief Function to compute Maucaulay matrix M
158  * @param[out] M Maucaulay matrix
159  * @param[in] row Internal member
160  * @param[in] alphas Internal member
161  * @param[in] u Image pixel x co-ordinate
162  * @param[in] v Image pixel y co-ordinate
163  */
164  void fill_M(
165  cv::Mat* M, const int row, const double* alphas, const double u,
166  const double v);
167 
168  /**
169  * @brief Compute the control points
170  * @param[in] betas Internal member variable
171  * @param[in] ut Internal member variable
172  */
173  void compute_ccs(const double* betas, const double* ut);
174 
175  /**
176  * @brief Compute object points based on control points
177  */
178  void compute_pcs();
179 
180  /**
181  * @brief Internal member function
182  */
183  void solve_for_sign();
184 
185  /**
186  * @brief Function to approximately calculate betas and focal length
187  * @param[in] Ut
188  * @param[in] Rho
189  * @param[out] betas
190  * @param[out] efs
191  */
193  cv::Mat* Ut, cv::Mat* Rho, double* betas, double* efs);
194 
195  /**
196  * @brief Function to calculate betas and focal length (more accurate)
197  * @param[in] Ut
198  * @param[in] Rho
199  * @param[out] betas
200  * @param[out] efs
201  */
203  cv::Mat* Ut, cv::Mat* Rho, double* betas, double* efs);
204 
205  /**
206  * @brief Function to do a QR decomposition
207  * @param[in] A Matrix to be decomposed
208  * @param[out] b
209  * @param[out] X
210  */
211  void qr_solve(cv::Mat* A, cv::Mat* b, cv::Mat* X);
212 
213  /**
214  * @brief Internal function
215  * @param[in] M1
216  * @return Distance
217  */
219  const cv::Mat& M1);
220 
221  /**
222  * @brief Internal function
223  * @param[in] M1
224  * @param[in] M2
225  * @return Distance
226  */
228  const cv::Mat& M1, const cv::Mat& M2);
229 
230  /**
231  * @brief Get all possible solutions
232  * @param[in] betas
233  * @param[out] solutions
234  */
236  const double betas[5], double solutions[18][3]);
237 
238  /**
239  * @brief Return the sign of the scalar
240  * @param[in] v
241  * @return True if positive else Flase
242  */
243  double sign(const double v);
244 
245  /**
246  * @brief Compute the dot product between two vectors
247  * @param[in] v1
248  * @param[in] v2
249  * @return Dot product
250  */
251  double dot(const double* v1, const double* v2);
252 
253  /**
254  * @brief Compute dot product in 2D with only x and y components
255  * @param[in] v1
256  * @param[in] v2
257  * @return Dot product
258  */
259  double dotXY(const double* v1, const double* v2);
260 
261  /**
262  * @brief Compute the dot product using only z component
263  * @param[in] v1
264  * @param[in] v2
265  * @return Dot product
266  */
267  double dotZ(const double* v1, const double* v2);
268 
269  /**
270  * @brief Compute the euclidean distance squared between two points in 3D
271  * @param[in] p1
272  * @param[in] p2
273  * @return Distance squared
274  */
275  double dist2(const double* p1, const double* p2);
276 
277  /**
278  * @brief Internal fucntion
279  * @param[out] rho
280  */
281  void compute_rho(double* rho);
282 
283  /**
284  * @brief Internal function
285  * @param[in] ut
286  * @param[out] l_6x12
287  */
288  void compute_L_6x12(const double* ut, double* l_6x12);
289 
290  /**
291  * @brief Gauss Newton Iterative optimization
292  * @param[in] L_6x12
293  * @param[in] Rho
294  * @param[in,out] current_betas
295  * @param[out] efs
296  */
297  void gauss_newton(
298  const cv::Mat* L_6x12, const cv::Mat* Rho, double current_betas[4],
299  double* efs);
300 
301  /**
302  * @brief Compute matrix A and vector b
303  * @param[in] l_6x12
304  * @param[in] rho
305  * @param[in] cb
306  * @param[out] A
307  * @param[out] b
308  * @param[in] f
309  */
311  const double* l_6x12, const double* rho, const double cb[4], cv::Mat* A,
312  cv::Mat* b, double const f);
313 
314  /**
315  * @brief Function to compute the pose
316  * @param[in] ut
317  * @param[in] betas
318  * @param[out] R Rotation Matrix
319  * @param[out] t Translation VectorS
320  * @return
321  */
322  double compute_R_and_t(
323  const double* ut, const double* betas, double R[3][3], double t[3]);
324 
325  /**
326  * @brief Helper function to function compute_R_and_t()
327  * @param[out] R Rotaiton matrix
328  * @param[out] t Translation vector
329  */
330  void estimate_R_and_t(double R[3][3], double t[3]);
331 
332  /**
333  * @brief Function to copy the pose
334  * @param[in] R_dst
335  * @param[in] t_dst
336  * @param[out] R_src
337  * @param[out] t_src
338  */
339  void copy_R_and_t(
340  const double R_dst[3][3], const double t_dst[3], double R_src[3][3],
341  double t_src[3]);
342 
343  double uc; //! Image center in x-direction
344  double vc; //! Image center in y-direction
345  double fu; //! Focal length in x-direction
346  double fv; //! Focal length in y-direction
347 
348  std::vector<double> pws; //! Object points
349  std::vector<double> us; //! Image points
350  std::vector<double> alphas, pcs; //! Internal variable
351  int number_of_correspondences; //! Number of 2d/3d correspondences
352 
353  double cws[4][3], ccs[4][3]; //! Control point variables
354  int max_nr; //! Internal variable
355  double *A1, *A2; //! Internal variable
356 };
357 
358 /** @} */ // end of grouping
359 } // namespace mrpt::vision::pnp
360 #endif // Check for OPENCV_LIB
void generate_all_possible_solutions_for_f_unk(const double betas[5], double solutions[18][3])
Get all possible solutions.
Definition: upnp.cpp:603
void choose_control_points()
Function to select 4 control points.
Definition: upnp.cpp:295
double dot(const double *v1, const double *v2)
Compute the dot product between two vectors.
Definition: upnp.cpp:784
void compute_rho(double *rho)
Internal fucntion.
Definition: upnp.cpp:767
void gauss_newton(const cv::Mat *L_6x12, const cv::Mat *Rho, double current_betas[4], double *efs)
Gauss Newton Iterative optimization.
Definition: upnp.cpp:643
void compute_ccs(const double *betas, const double *ut)
Compute the control points.
Definition: upnp.cpp:337
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.
Definition: upnp.cpp:168
void init_points(const cv::Mat &opoints, const cv::Mat &ipoints)
Iniialize Object points and image points from OpenCV Matrix.
Definition: upnp.h:124
cv::Mat compute_constraint_distance_3param_6eq_6unk_f_unk(const cv::Mat &M1, const cv::Mat &M2)
Internal function.
Definition: upnp.cpp:479
double dotXY(const double *v1, const double *v2)
Compute dot product in 2D with only x and y components.
Definition: upnp.cpp:789
double ccs[4][3]
Definition: upnp.h:353
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)
Definition: upnp.cpp:386
double reprojection_error(const double R[3][3], const double t[3])
Compute the reprojection error using the estimated Rotation matrix and Translation Vector...
Definition: upnp.cpp:275
Perspective n Point (PnP) Algorithms toolkit for MRPT mrpt_vision_grp.
Definition: pnp_algos.h:23
std::vector< double > pws
Focal length in y-direction.
Definition: upnp.h:348
double cws[4][3]
Number of 2d/3d correspondences.
Definition: upnp.h:353
double dist2(const double *p1, const double *p2)
Compute the euclidean distance squared between two points in 3D.
Definition: upnp.cpp:777
std::vector< double > pcs
Definition: upnp.h:350
double dotZ(const double *v1, const double *v2)
Compute the dot product using only z component.
Definition: upnp.cpp:794
int number_of_correspondences
Internal variable.
Definition: upnp.h:351
std::vector< double > alphas
Image points.
Definition: upnp.h:350
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.
Definition: upnp.cpp:675
upnp(const cv::Mat &cameraMatrix, const cv::Mat &opoints, const cv::Mat &ipoints)
Constructor for UPnP class.
Definition: upnp.cpp:77
cv::Mat compute_constraint_distance_2param_6eq_2unk_f_unk(const cv::Mat &M1)
Internal function.
Definition: upnp.cpp:437
void compute_L_6x12(const double *ut, double *l_6x12)
Internal function.
Definition: upnp.cpp:719
void qr_solve(cv::Mat *A, cv::Mat *b, cv::Mat *X)
Function to do a QR decomposition.
Definition: upnp.cpp:800
void init_camera_parameters(const cv::Mat &cameraMatrix)
Initialize camera variables using camera intrinsic matrix.
Definition: upnp.h:110
void estimate_R_and_t(double R[3][3], double t[3])
Helper function to function compute_R_and_t()
Definition: upnp.cpp:179
void fill_M(cv::Mat *M, const int row, const double *alphas, const double u, const double v)
Function to compute Maucaulay matrix M.
Definition: upnp.cpp:319
void compute_pcs()
Compute object points based on control points.
Definition: upnp.cpp:352
const float R
std::vector< double > us
Object points.
Definition: upnp.h:349
void compute_alphas()
Function to comput .
Definition: upnp.cpp:301
void solve_for_sign()
Internal member function.
Definition: upnp.cpp:246
int max_nr
Control point variables.
Definition: upnp.h:354
double vc
Image center in x-direction.
Definition: upnp.h:344
~upnp()
Destructor for UPnP class.
Definition: upnp.cpp:110
double compute_R_and_t(const double *ut, const double *betas, double R[3][3], double t[3])
Function to compute the pose.
Definition: upnp.cpp:262
double * A1
Internal variable.
Definition: upnp.h:355
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.
Definition: upnp.cpp:365
double fu
Image center in y-direction.
Definition: upnp.h:345
double sign(const double v)
Return the sign of the scalar.
Definition: upnp.cpp:795
double compute_pose(cv::Mat &R, cv::Mat &t)
Function to compute pose.
Definition: upnp.cpp:116
double fv
Focal length in x-direction.
Definition: upnp.h:346



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020