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


Page generated by Doxygen 1.8.14 for MRPT 1.5.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019