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


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