MRPT  2.0.1
epnp.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 
10 #pragma once
11 #include <mrpt/3rdparty/do_opencv_includes.h>
12 
13 #if MRPT_HAS_OPENCV
14 
15 namespace mrpt::vision::pnp
16 {
17 /** \addtogroup pnp Perspective-n-Point pose estimation
18  * \ingroup mrpt_vision_grp
19  * @{
20  */
21 
22 /**
23  * @class epnp
24  * @author Chandra Mangipudi
25  * @date 11/08/16
26  * @file epnp.h
27  * @brief Efficient PnP - Eigen Wrapper for OpenCV calib3d implementation
28  */
29 class epnp
30 {
31  public:
32  //! Constructor for EPnP class
33  epnp(
34  const cv::Mat& cameraMatrix, const cv::Mat& opoints,
35  const cv::Mat& ipoints);
36 
37  //! Destructor for EPnP class
38  ~epnp();
39 
40  /**
41  * @brief Add a 2d/3d correspondence
42  * @param[in] X X coordinate in Camera coordinate system
43  * @param[in] Y Y coordinate in Camera coordinate system
44  * @param[in] Z Z coordinate in Camera coordinate system
45  * @param[in] u Image pixel coordinate u in x axis
46  * @param[in] v Image pixel coordinate v in y axis
47  */
48  void add_correspondence(
49  const double X, const double Y, const double Z, const double u,
50  const double v);
51 
52  /**
53  * @brief OpenCV wrapper to compute pose
54  * @param[out] R Rotation Matrix
55  * @param[out] t Translation Vector
56  */
57  void compute_pose(cv::Mat& R, cv::Mat& t);
58 
59  private:
60  /**
61  * @brief Initialize Camera Matrix
62  * @param[in] cameraMatrix Camera Intrinsic matrix as a OpenCV Matrix
63  */
64  template <typename T>
65  void init_camera_parameters(const cv::Mat& cameraMatrix)
66  {
67  uc = cameraMatrix.at<T>(0, 2);
68  vc = cameraMatrix.at<T>(1, 2);
69  fu = cameraMatrix.at<T>(0, 0);
70  fv = cameraMatrix.at<T>(1, 1);
71  }
72 
73  /**
74  * @brief Convert object points and image points from OpenCV format to STL
75  * matrices
76  * @param opoints Object points in Camera coordinate system
77  * @param ipoints Imate points in pixel coordinates
78  */
79  template <typename OpointType, typename IpointType>
80  void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
81  {
82  for (int i = 0; i < number_of_correspondences; i++)
83  {
84  pws[3 * i] = opoints.at<OpointType>(i, 0);
85  pws[3 * i + 1] = opoints.at<OpointType>(i, 1);
86  pws[3 * i + 2] = opoints.at<OpointType>(i, 2);
87 
88  us[2 * i] = ipoints.at<IpointType>(i, 0) * fu + uc;
89  us[2 * i + 1] = ipoints.at<IpointType>(i, 1) * fv + vc;
90  }
91  }
92 
93  /**
94  * @brief Function to compute reprojection error
95  * @param R Rotation Matrix
96  * @param t Translation Vector
97  * @return
98  */
99  double reprojection_error(const double R[3][3], const double t[3]);
100 
101  /**
102  * @brief Function to select 4 control points from n points
103  */
104  void choose_control_points();
105 
106  /**
107  * @brief Convert from object space to relative object space (Barycentric
108  * coordinates)
109  */
111 
112  /**
113  * @brief Generate the Matrix M
114  * @param[out] M
115  * @param[in] row
116  * @param[in] alphas
117  * @param[in] u
118  * @param[in] v
119  */
120  void fill_M(
121  CvMat* M, const int row, const double* alphas, const double u,
122  const double v);
123 
124  /**
125  * @brief Internal function
126  * @param[in] betas
127  * @param[in] ut
128  */
129  void compute_ccs(const double* betas, const double* ut);
130 
131  /**
132  * @brief Internal function
133  */
134  void compute_pcs();
135 
136  /**
137  * @brief Internal function
138  */
139  void solve_for_sign();
140 
141  /**
142  * @brief Internal function
143  * @param[out] L_6x10
144  * @param[in] Rho
145  * @param[in] betas
146  */
147  void find_betas_approx_1(
148  const CvMat* L_6x10, const CvMat* Rho, double* betas);
149 
150  /**
151  * @brief Internal function
152  * @param[out] L_6x10
153  * @param[in] Rho
154  * @param[in] betas
155  */
156  void find_betas_approx_2(
157  const CvMat* L_6x10, const CvMat* Rho, double* betas);
158 
159  /**
160  * @brief Internal function
161  * @param[out] L_6x10
162  * @param[in] Rho
163  * @param[in] betas
164  */
165  void find_betas_approx_3(
166  const CvMat* L_6x10, const CvMat* Rho, double* betas);
167 
168  /**
169  * @brief QR optimization algorithm
170  * @param[in] A
171  * @param[out] b
172  * @param[out] X
173  */
174  void qr_solve(CvMat* A, CvMat* b, CvMat* X);
175 
176  /**
177  * @brief Dot product of two OpenCV vectors
178  * @param[in] v1
179  * @param[in] v2
180  * @return
181  */
182  double dot(const double* v1, const double* v2);
183 
184  /**
185  * @brief Squared distance between two vectors
186  * @param[in] p1
187  * @param[in] p2
188  * @return
189  */
190  double dist2(const double* p1, const double* p2);
191 
192  /**
193  * @brief Get distances between all object points taken 2 at a time(nC2)
194  * @param rho
195  */
196  void compute_rho(double* rho);
197 
198  /**
199  * @brief Internal function
200  * @param[in] ut
201  * @param[out] l_6x10
202  */
203  void compute_L_6x10(const double* ut, double* l_6x10);
204 
205  /**
206  * @brief Gauss Newton iterative algorithm
207  * @param[in] L_6x10
208  * @param[in] Rho
209  * @param[in,out] current_betas
210  */
211  void gauss_newton(
212  const CvMat* L_6x10, const CvMat* Rho, double current_betas[4]);
213 
214  /**
215  * @brief Internal function
216  * @param[in] l_6x10
217  * @param[in] rho
218  * @param[in] cb
219  * @param[out] A
220  * @param[out] b
221  */
223  const double* l_6x10, const double* rho, const double cb[4], CvMat* A,
224  CvMat* b);
225 
226  /**
227  * @brief Function to compute pose
228  * @param[in] ut
229  * @param[in] betas
230  * @param[out] R
231  * @param[out] t
232  * @return
233  */
234  double compute_R_and_t(
235  const double* ut, const double* betas, double R[3][3], double t[3]);
236 
237  /**
238  * @brief Helper function to @func compute_R_and_t()
239  * @param R
240  * @param t
241  */
242  void estimate_R_and_t(double R[3][3], double t[3]);
243 
244  /**
245  * @brief Copy function of output result
246  * @param[out] R_dst
247  * @param[out] t_dst
248  * @param[in] R_src
249  * @param[in] t_src
250  */
251  void copy_R_and_t(
252  const double R_dst[3][3], const double t_dst[3], double R_src[3][3],
253  double t_src[3]);
254 
255  double uc; //! Image center in x-direction
256  double vc; //! Image center in y-direction
257  double fu; //! Focal length in x-direction
258  double fv; //! Focal length in y-direction
259 
260  std::vector<double> pws, us, alphas, pcs; //! Internal member variables
261  int number_of_correspondences; //! Number of 2d/3d correspondences
262 
263  double cws[4][3], ccs[4][3]; //! Internal member variables
264  double cws_determinant; //! Internal member variable
265  int max_nr; //! Internal member variable
266  double *A1, *A2; //! Internal member variables
267 };
268 
269 /** @} */ // end of grouping
270 } // namespace mrpt::vision::pnp
271 #endif
void compute_A_and_b_gauss_newton(const double *l_6x10, const double *rho, const double cb[4], CvMat *A, CvMat *b)
Internal function.
Definition: epnp.cpp:513
double cws_determinant
Internal member variables.
Definition: epnp.h:264
std::vector< double > pws
Focal length in y-direction.
Definition: epnp.h:260
std::vector< double > us
Definition: epnp.h:260
void find_betas_approx_3(const CvMat *L_6x10, const CvMat *Rho, double *betas)
Internal function.
Definition: epnp.cpp:424
Perspective n Point (PnP) Algorithms toolkit for MRPT mrpt_vision_grp.
Definition: pnp_algos.h:23
void compute_pose(cv::Mat &R, cv::Mat &t)
OpenCV wrapper to compute pose.
Definition: epnp.cpp:161
void compute_rho(double *rho)
Get distances between all object points taken 2 at a time(nC2)
Definition: epnp.cpp:503
~epnp()
Destructor for EPnP class.
Definition: epnp.cpp:54
double reprojection_error(const double R[3][3], const double t[3])
Function to compute reprojection error.
Definition: epnp.cpp:328
double dist2(const double *p1, const double *p2)
Squared distance between two vectors.
Definition: epnp.cpp:221
int number_of_correspondences
Internal member variables.
Definition: epnp.h:261
void gauss_newton(const CvMat *L_6x10, const CvMat *Rho, double current_betas[4])
Gauss Newton iterative algorithm.
Definition: epnp.cpp:543
void compute_barycentric_coordinates()
Convert from object space to relative object space (Barycentric coordinates)
Definition: epnp.cpp:94
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])
Copy function of output result.
Definition: epnp.cpp:210
std::vector< double > pcs
Definition: epnp.h:260
void fill_M(CvMat *M, const int row, const double *alphas, const double u, const double v)
Generate the Matrix M.
Definition: epnp.cpp:118
void init_camera_parameters(const cv::Mat &cameraMatrix)
Initialize Camera Matrix.
Definition: epnp.h:65
void compute_pcs()
Internal function.
Definition: epnp.cpp:148
double * A1
Internal member variable.
Definition: epnp.h:266
double dot(const double *v1, const double *v2)
Dot product of two OpenCV vectors.
Definition: epnp.cpp:228
double cws[4][3]
Number of 2d/3d correspondences.
Definition: epnp.h:263
void init_points(const cv::Mat &opoints, const cv::Mat &ipoints)
Convert object points and image points from OpenCV format to STL matrices.
Definition: epnp.h:80
double ccs[4][3]
Definition: epnp.h:263
void qr_solve(CvMat *A, CvMat *b, CvMat *X)
QR optimization algorithm.
Definition: epnp.cpp:562
double fu
Image center in y-direction.
Definition: epnp.h:257
std::vector< double > alphas
Definition: epnp.h:260
void choose_control_points()
Function to select 4 control points from n points.
Definition: epnp.cpp:60
void estimate_R_and_t(double R[3][3], double t[3])
Helper function to compute_R_and_t()
Definition: epnp.cpp:233
void solve_for_sign()
Internal function.
Definition: epnp.cpp:299
const float R
void add_correspondence(const double X, const double Y, const double Z, const double u, const double v)
Add a 2d/3d correspondence.
void find_betas_approx_2(const CvMat *L_6x10, const CvMat *Rho, double *betas)
Internal function.
Definition: epnp.cpp:388
double compute_R_and_t(const double *ut, const double *betas, double R[3][3], double t[3])
Function to compute pose.
Definition: epnp.cpp:315
int max_nr
Internal member variable.
Definition: epnp.h:265
void compute_ccs(const double *betas, const double *ut)
Internal function.
Definition: epnp.cpp:136
double fv
Focal length in x-direction.
Definition: epnp.h:258
epnp(const cv::Mat &cameraMatrix, const cv::Mat &opoints, const cv::Mat &ipoints)
Constructor for EPnP class.
Definition: epnp.cpp:20
double vc
Image center in x-direction.
Definition: epnp.h:256
void compute_L_6x10(const double *ut, double *l_6x10)
Internal function.
Definition: epnp.cpp:457
void find_betas_approx_1(const CvMat *L_6x10, const CvMat *Rho, double *betas)
Internal function.
Definition: epnp.cpp:352



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020