Main MRPT website > C++ reference for MRPT 1.5.5
p3p.h
Go to the documentation of this file.
1 #ifndef P3P_H
2 #define P3P_H
3 
4 #include <vector>
5 #include <mrpt/utils/types_math.h> // Eigen must be included first via MRPT to enable the plugin system
6 #include <Eigen/Dense>
7 #include <Eigen/SVD>
8 #include <mrpt/otherlibs/do_opencv_includes.h>
9 
10 namespace mrpt
11 {
12  namespace vision
13  {
14  namespace pnp
15  {
16  /** \addtogroup pnp Perspective-n-Point pose estimation
17  * \ingroup mrpt_vision_grp
18  * @{
19  */
20 
21  /**
22  * @class p3p
23  * @author Chandra Mangipudi
24  * @date 11/08/16
25  * @file p3p.h
26  * @brief P3P Pose estimation Algorithm - Eigen Implementation
27  */
28  class p3p
29  {
30  public:
31  //! Constructor for p3p class using C
32  p3p(double fx, double fy, double cx, double cy);
33 
34  //! Constructor using Eigen matrix
35  p3p(Eigen::MatrixXd cam_intrinsic);
36 
37  #if MRPT_HAS_OPENCV
38  //! Constructor using OpenCV matrix
39  p3p(cv::Mat cameraMatrix);
40 
41  /**
42  * @brief Function to compute pose by P3P using OpenCV
43  * @param[out] R Rotation Matrix
44  * @param[out] tvec Translation Vector
45  * @param[in] opoints Object points
46  * @param[in] ipoints Image points
47  * @return Success flag
48  */
49  bool solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat& ipoints);
50  #endif
51  bool solve(Eigen::Ref<Eigen::Matrix3d> R, Eigen::Ref<Eigen::Vector3d> t, Eigen::MatrixXd obj_pts, Eigen::MatrixXd img_pts);
52 
53  /**
54  * @brief Function to compute pose from 3 points using C function
55  * @param[out] R Rotation Matrix
56  * @param[out] t Translation Vector
57  * @param[in] mu0 x- coordinate of image point 1
58  * @param[in] mv0 y- coordinate of image point 1
59  * @param[in] X0 X- coordinate of object point 1
60  * @param[in] Y0 Y- coordinate of object point 1
61  * @param[in] Z0 Z- coordinate of object point 1
62  * @param[in] mu1 x- coordinate of image point 2
63  * @param[in] mv1 y- coordinate of image point 2
64  * @param[in] X1 X- coordinate of object point 2
65  * @param[in] Y1 Y- coordinate of object point 2
66  * @param[in] Z1 Z- coordinate of object point 2
67  * @param[in] mu2 x- coordinate of image point 3
68  * @param[in] mv2 y- coordinate of image point 3
69  * @param[in] X2 X- coordinate of object point 3
70  * @param[in] Y2 Y- coordinate of object point 3
71  * @param[in] Z2 Z- coordinate of object point 3
72  * @return Success flag
73  */
74  int solve(double R[4][3][3], double t[4][3],
75  double mu0, double mv0, double X0, double Y0, double Z0,
76  double mu1, double mv1, double X1, double Y1, double Z1,
77  double mu2, double mv2, double X2, double Y2, double Z2);
78 
79  /**
80  * @brief Function to compute pose from 4 points using C function
81  * @param[out] R Rotation Matrix
82  * @param[out] t Translation Vector
83  * @param[in] mu0 x- coordinate of image point 1
84  * @param[in] mv0 y- coordinate of image point 1
85  * @param[in] X0 X- coordinate of object point 1
86  * @param[in] Y0 Y- coordinate of object point 1
87  * @param[in] Z0 Z- coordinate of object point 1
88  * @param[in] mu1 x- coordinate of image point 2
89  * @param[in] mv1 y- coordinate of image point 2
90  * @param[in] X1 X- coordinate of object point 2
91  * @param[in] Y1 Y- coordinate of object point 2
92  * @param[in] Z1 Z- coordinate of object point 2
93  * @param[in] mu2 x- coordinate of image point 3
94  * @param[in] mv2 y- coordinate of image point 3
95  * @param[in] X2 X- coordinate of object point 3
96  * @param[in] Y2 Y- coordinate of object point 3
97  * @param[in] Z2 Z- coordinate of object point 3
98  * @param[in] mu3 x- coordinate of image point 4
99  * @param[in] mv3 y- coordinate of image point 4
100  * @param[in] X3 X- coordinate of object point 4
101  * @param[in] Y3 Y- coordinate of object point 4
102  * @param[in] Z3 Z- coordinate of object point 4
103  * @return
104  */
105  bool solve(double R[3][3], double t[3],
106  double mu0, double mv0, double X0, double Y0, double Z0,
107  double mu1, double mv1, double X1, double Y1, double Z1,
108  double mu2, double mv2, double X2, double Y2, double Z2,
109  double mu3, double mv3, double X3, double Y3, double Z3);
110 
111  private:
112  #if MRPT_HAS_OPENCV
113  /**
114  * @brief OpenCV Initialization function to access camera intrinsic matrix
115  * @param[in] cameraMatrix Camera Intrinsic Matrix in OpenCV Mat format
116  */
117  template <typename T>
118  void init_camera_parameters(const cv::Mat& cameraMatrix)
119  {
120  cx = cameraMatrix.at<T> (0, 2);
121  cy = cameraMatrix.at<T> (1, 2);
122  fx = cameraMatrix.at<T> (0, 0);
123  fy = cameraMatrix.at<T> (1, 1);
124  }
125 
126  /**
127  * @brief OpoenCV wrapper for extracting object and image points
128  * @param[in] opoints Object points (OpenCV Mat)
129  * @param[in] ipoints Image points (OpenCV Mat)
130  * @param[out] points Combination of object and image points (C structure)
131  */
132  template <typename OpointType, typename IpointType>
133  void extract_points(const cv::Mat& opoints, const cv::Mat& ipoints, std::vector<double>& points)
134  {
135  points.clear();
136  points.resize(20);
137  for(int i = 0; i < 4; i++)
138  {
139  points[i*5] = ipoints.at<IpointType>(i).x*fx + cx;
140  points[i*5+1] = ipoints.at<IpointType>(i).y*fy + cy;
141  points[i*5+2] = opoints.at<OpointType>(i).x;
142  points[i*5+3] = opoints.at<OpointType>(i).y;
143  points[i*5+4] = opoints.at<OpointType>(i).z;
144  }
145  }
146  #endif
147 
148  /**
149  * @brief Eigen wrapper for extracting object and image points
150  * @param[in] opoints Object points (Eigen Mat)
151  * @param[in] ipoints Image points (Eigen Mat)
152  * @param[out] points Combination of object and image points (C structure)
153  */
154  void extract_points(Eigen::MatrixXd obj_pts, Eigen::MatrixXd img_pts, std::vector<double>& points)
155  {
156  points.clear();
157  points.resize(20);
158  for(int i=0; i<4; i++)
159  {
160  points[i*5] = img_pts(i,0)*fx + cx;
161  points[i*5+1]=img_pts(i,1)*fy + cy;
162  points[i*5+2]=obj_pts(i,0);
163  points[i*5+3]=obj_pts(i,1);
164  points[i*5+4]=obj_pts(i,2);
165  }
166  }
167  /**
168  * @brief Function to compute inverse parameters of camera intrinsic matrix
169  */
170  void init_inverse_parameters();
171 
172  /**
173  * @brief Helper function to @func solve()
174  * @param[out] lengths Internal lengths used for P3P
175  * @param[in] distances Internal distances used for computation of lengths
176  * @param[in] cosines Internal cosines used for computation of lengths
177  * @return
178  */
179  int solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3]);
180  bool align(double M_start[3][3],
181  double X0, double Y0, double Z0,
182  double X1, double Y1, double Z1,
183  double X2, double Y2, double Z2,
184  double R[3][3], double T[3]);
185  /**
186  * @brief Function used to compute the SVD
187  * @param[in] A Input Matrix for which SVD is to be computed
188  * @param[out] D Diagonal Matrix
189  * @param[out] U Matrix of left eigen vectors
190  * @return
191  */
192  bool jacobi_4x4(double * A, double * D, double * U);
193 
194  double fx;//! Focal length x
195  double fy;//! Focal length y
196  double cx;//! Image center x
197  double cy;//! Image center y
198  double inv_fx;//! Inverse of focal length x
199  double inv_fy;//! Inverse of focal length y
200  double cx_fx;//! Inverse of image center point x
201  double cy_fy;//! Inverse of image center point y
202  };
203 
204  /** @} */ // end of grouping
205 
206  }
207  }
208 }
209 
210 #endif // P3P_H
t
GLdouble GLdouble t
Definition: glext.h:3610
z
GLdouble GLdouble z
Definition: glext.h:3734
mrpt::vision::pnp::p3p::extract_points
void extract_points(Eigen::MatrixXd obj_pts, Eigen::MatrixXd img_pts, std::vector< double > &points)
Eigen wrapper for extracting object and image points.
Definition: p3p.h:154
types_math.h
mrpt::vision::pnp::p3p::jacobi_4x4
bool jacobi_4x4(double *A, double *D, double *U)
Function used to compute the SVD.
Definition: p3p.cpp:375
points
GLsizei const GLfloat * points
Definition: glext.h:4797
mrpt::vision::pnp::p3p::inv_fy
double inv_fy
Inverse of focal length x.
Definition: p3p.h:199
mrpt::vision::pnp::p3p::cy
double cy
Image center x.
Definition: p3p.h:197
mrpt::vision::pnp::p3p::align
bool align(double M_start[3][3], double X0, double Y0, double Z0, double X1, double Y1, double Z1, double X2, double Y2, double Z2, double R[3][3], double T[3])
Definition: p3p.cpp:303
mrpt::vision::pnp::p3p::solve_for_lengths
int solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3])
Helper function to solve()
Definition: p3p.cpp:211
mrpt::vision::pnp::p3p::cy_fy
double cy_fy
Inverse of image center point x.
Definition: p3p.h:201
mrpt::vision::pnp::p3p::init_inverse_parameters
void init_inverse_parameters()
Function to compute inverse parameters of camera intrinsic matrix.
Definition: p3p.cpp:25
mrpt::vision::pnp::p3p::solve
bool solve(Eigen::Ref< Eigen::Matrix3d > R, Eigen::Ref< Eigen::Vector3d > t, Eigen::MatrixXd obj_pts, Eigen::MatrixXd img_pts)
Definition: p3p.cpp:87
mrpt::vision::pnp::p3p::cx
double cx
Focal length y.
Definition: p3p.h:196
mrpt::vision::pnp::p3p::p3p
p3p(double fx, double fy, double cx, double cy)
Constructor for p3p class using C.
Definition: p3p.cpp:42
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
R
const float R
Definition: CKinematicChain.cpp:141
mrpt::vision::pnp::p3p::fy
double fy
Focal length x.
Definition: p3p.h:195
mrpt::vision::pnp::p3p::cx_fx
double cx_fx
Inverse of focal length y.
Definition: p3p.h:200
y
GLenum GLint GLint y
Definition: glext.h:3516
x
GLenum GLint x
Definition: glext.h:3516
mrpt::vision::pnp::p3p::inv_fx
double inv_fx
Image center y.
Definition: p3p.h:198


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