Main MRPT website > C++ reference for MRPT 1.9.9
pnp_algos.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-2018, 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 #ifndef __pnp_algos_h
10 #define __pnp_algos_h
11 
12 #include <mrpt/config.h>
13 
14 #include <Eigen/Core>
15 #include <Eigen/Dense>
16 
17 namespace mrpt
18 {
19 namespace vision
20 {
21 /** Perspective n Point (PnP) Algorithms toolkit for MRPT \ingroup
22  * mrpt_vision_grp */
23 namespace pnp
24 {
25 /** \addtogroup pnp Perspective-n-Point pose estimation
26  * \ingroup mrpt_vision_grp
27  * @{
28  */
29 
30 /**
31  * @class CPnP
32  * @author Chandra Mangipudi
33  * @date 17/08/16
34  * @file pnp_algos.h
35  * @brief PnP Algorithms toolkit for MRPT
36  */
37 
38 /** This class is used for Pose estimation from a known landmark using a
39  * monocular camera.
40  * The toolkit comprises of state of the art PnP (Perspective n Point)
41  * algorithms
42  *
43  * The Python Bindings pnp_perf_comp.py can be used to generate performance
44  * comparison using
45  * standard tests between the different algorithms.
46  *
47  * <h2> <a href="pnp_perf_comp.html">Performance comparison Results </a> </h2>
48  *
49  */
50 class CPnP
51 {
52  public:
53  /**
54  * @brief \cite hesch Direct Least Squares (DLS) - PnP : Algorithm
55  * formulates position as a function of rotation.
56  * Use Cayley's rotation theorem to represent the rotation using
57  * parameters ($s_1, s_2, s_3$).
58  * Solve the rotation using multi-variate polynomial expressions
59  *
60  * @param[in] obj_pts Object points in Camera Co-ordinate system {C} nX3
61  * (only 4 points used) array [p_x, p_y, p_z]
62  * @param[in] img_pts Image points in pixels nX3 (only 4 points used) array
63  * containing pixel data from camera [u, v, 1]
64  * @param[in] n number of 2D-3D correspondences
65  * @param[in] cam_intrinsic Camera Intrinsic matrix
66  * @param[out] pose_mat Output pose vector 6X1, pose_mat[0:2]-> Translation,
67  * pose_mat[3:5] -> Quaternion vector component
68  * @return success flag
69  */
70  bool dls(
71  const Eigen::Ref<Eigen::MatrixXd> obj_pts,
72  const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
73  const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
74  Eigen::Ref<Eigen::MatrixXd> pose_mat);
75 
76  /**
77  * @brief \cite lepetit Efficient-PnP (EPnP) - Algorithm takes 4 control
78  * points based on n points and uses the control points to compute the pose
79  *
80  * @param[in] obj_pts Object points in Camera Co-ordinate system {C} nX3
81  * (only 4 points used) array [p_x, p_y, p_z]
82  * @param[in] img_pts Image points in pixels nX3 (only 4 points used) array
83  * containing pixel data from camera [u, v, 1]
84  * @param[in] n number of 2D-3D correspondences
85  * @param[in] cam_intrinsic Camera Intrinsic matrix
86  * @param[out] pose_mat Output pose vector 6X1, pose_mat[0:2]-> Translation,
87  * pose_mat[3:5] -> Quaternion vector component
88  * @return success flag
89  */
90  bool epnp(
91  const Eigen::Ref<Eigen::MatrixXd> obj_pts,
92  const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
93  const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
94  Eigen::Ref<Eigen::MatrixXd> pose_mat);
95 
96  /**
97  * @brief \cite Kneip2014 Unified-PnP (UPnP) : Algorithm to compute pose
98  * from unknown camera intrinsic matrix
99  *
100  * @param[in] obj_pts Object points in Camera Co-ordinate system {C} nX3
101  * (only 4 points used) array [p_x, p_y, p_z]
102  * @param[in] img_pts Image points in pixels nX3 (only 4 points used) array
103  * containing pixel data from camera [u, v, 1]
104  * @param[in] n number of 2D-3D correspondences
105  * @param[in] cam_intrinsic Camera Intrinsic matrix
106  * @param[out] pose_mat Output pose vector 6X1, pose_mat[0:2]-> Translation,
107  * pose_mat[3:5] -> Quaternion vector component
108  * @return success flag
109  */
110  bool upnp(
111  const Eigen::Ref<Eigen::MatrixXd> obj_pts,
112  const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
113  const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
114  Eigen::Ref<Eigen::MatrixXd> pose_mat);
115 
116  /**
117  * @brief \cite kneip P3P - A closed form solution for n=3, 2D-3D
118  * correspondences
119  *
120  * @param[in] obj_pts Object points in Camera Co-ordinate system {C} nX3
121  * (only 4 points used) array [p_x, p_y, p_z]
122  * @param[in] img_pts Image points in pixels nX3 (only 4 points used) array
123  * containing pixel data from camera [u, v, 1]
124  * @param[in] n number of 2D-3D correspondences
125  * @param[in] cam_intrinsic Camera Intrinsic matrix
126  * @param[out] pose_mat Output pose vector 6X1, pose_mat[0:2]-> Translation,
127  * pose_mat[3:5] -> Quaternion vector component
128  * @return success flag
129  */
130  bool p3p(
131  const Eigen::Ref<Eigen::MatrixXd> obj_pts,
132  const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
133  const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
134  Eigen::Ref<Eigen::MatrixXd> pose_mat);
135 
136  /**
137  * @brief \cite xie Robust (R-PnP)- A closed form solution with intermediate
138  * P3P computations
139  *
140  * @param[in] obj_pts Object points in Camera Co-ordinate system {C} nX3
141  * (only 4 points used) array [p_x, p_y, p_z]
142  * @param[in] img_pts Image points in pixels nX3 (only 4 points used) array
143  * containing pixel data from camera [u, v, 1]
144  * @param[in] n number of 2D-3D correspondences
145  * @param[in] cam_intrinsic Camera Intrinsic matrix
146  * @param[out] pose_mat Output pose vector 6X1, pose_mat[0:2]-> Translation,
147  * pose_mat[3:5] -> Quaternion vector component
148  * @return success flag
149  */
150  bool rpnp(
151  const Eigen::Ref<Eigen::MatrixXd> obj_pts,
152  const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
153  const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
154  Eigen::Ref<Eigen::MatrixXd> pose_mat);
155 
156  /**
157  * @brief \cite garro Procrustes-PnP(PPnP) Algorithm : Iterative SVD based
158  * algorithm
159  * @param[in] obj_pts Object points in Camera Co-ordinate system {C} nX3
160  * array [p_x, p_y, p_z]
161  * @param[in] img_pts Image points in pixels nX3 array containing pixel data
162  * from camera [u, v, 1]
163  * @param[in] n number of 2D-3D correspondences
164  * @param[in] cam_intrinsic Camera Intrinsic matrix
165  * @param[out] pose_mat Output pose vector 6X1, pose_mat[0:2]-> Translation,
166  * pose_mat[3:5] -> Quaternion vector component
167  * @return success flag
168  */
169  bool ppnp(
170  const Eigen::Ref<Eigen::MatrixXd> obj_pts,
171  const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
172  const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
173  Eigen::Ref<Eigen::MatrixXd> pose_mat);
174 
175  /**
176  * @brief \cite dementhon Pose from Orthogoanlity and Scaling :Iterative
177  * (POSIT) - A Geometric algorithm to compute scale and orthogonality
178  * independently
179  * @param[in] obj_pts Object points in Camera Co-ordinate system {C} nX3
180  * array [p_x, p_y, p_z]
181  * @param[in] img_pts Image points in pixels nX3 array containing pixel data
182  * from camera [u, v, 1]
183  * @param[in] n number of 2D-3D correspondences
184  * @param[in] cam_intrinsic Camera Intrinsic matrix
185  * @param[out] pose_mat Output pose vector 6X1, pose_mat[0:2]-> Translation,
186  * pose_mat[3:5] -> Quaternion vector component
187  * @return success flag
188  */
189  bool posit(
190  const Eigen::Ref<Eigen::MatrixXd> obj_pts,
191  const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
192  const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
193  Eigen::Ref<Eigen::MatrixXd> pose_mat);
194 
195  /**@brief \cite lu Lu-Hager-Mjolsness(LHM)-PnP algorithm : Iterative
196  * algorithm to reduce object space error
197  * @param[in] obj_pts Object points in Camera Co-ordinate system {C} nX3
198  * array [p_x p_y p_z]
199  * @param[in] img_pts Image points in pixels nX3 array containing pixel data
200  * from camera [u, v, 1]
201  * @param[in] n number of 2D-3D correspondences
202  * @param[in] cam_intrinsic Camera Intrinsic matrix
203  * @param[out] pose_mat Output pose vector 6X1, pose_mat[0:2]-> Translation,
204  * pose_mat[3:5] -> Quaternion vector component
205  * @return success flag
206  */
207  bool lhm(
208  const Eigen::Ref<Eigen::MatrixXd> obj_pts,
209  const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
210  const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
211  Eigen::Ref<Eigen::MatrixXd> pose_mat);
212 };
213 
214 /** @} */ // end of grouping
215 }
216 }
217 }
218 
219 #endif
n
GLenum GLsizei n
Definition: glext.h:5074
mrpt::vision::pnp::CPnP::p3p
bool p3p(const Eigen::Ref< Eigen::MatrixXd > obj_pts, const Eigen::Ref< Eigen::MatrixXd > img_pts, int n, const Eigen::Ref< Eigen::MatrixXd > cam_intrinsic, Eigen::Ref< Eigen::MatrixXd > pose_mat)
P3P - A closed form solution for n=3, 2D-3D correspondences
Definition: pnp_algos.cpp:293
mrpt::vision::pnp::CPnP::ppnp
bool ppnp(const Eigen::Ref< Eigen::MatrixXd > obj_pts, const Eigen::Ref< Eigen::MatrixXd > img_pts, int n, const Eigen::Ref< Eigen::MatrixXd > cam_intrinsic, Eigen::Ref< Eigen::MatrixXd > pose_mat)
Procrustes-PnP(PPnP) Algorithm : Iterative SVD based algorithm
Definition: pnp_algos.cpp:421
mrpt::vision::pnp::CPnP
This class is used for Pose estimation from a known landmark using a monocular camera.
Definition: pnp_algos.h:50
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::vision::pnp::CPnP::lhm
bool lhm(const Eigen::Ref< Eigen::MatrixXd > obj_pts, const Eigen::Ref< Eigen::MatrixXd > img_pts, int n, const Eigen::Ref< Eigen::MatrixXd > cam_intrinsic, Eigen::Ref< Eigen::MatrixXd > pose_mat)
Lu-Hager-Mjolsness(LHM)-PnP algorithm : Iterative algorithm to reduce object space error
Definition: pnp_algos.cpp:551
mrpt::vision::pnp::CPnP::posit
bool posit(const Eigen::Ref< Eigen::MatrixXd > obj_pts, const Eigen::Ref< Eigen::MatrixXd > img_pts, int n, const Eigen::Ref< Eigen::MatrixXd > cam_intrinsic, Eigen::Ref< Eigen::MatrixXd > pose_mat)
Pose from Orthogoanlity and Scaling :Iterative (POSIT) - A Geometric algorithm to compute scale and o...
Definition: pnp_algos.cpp:486
mrpt::vision::pnp::CPnP::upnp
bool upnp(const Eigen::Ref< Eigen::MatrixXd > obj_pts, const Eigen::Ref< Eigen::MatrixXd > img_pts, int n, const Eigen::Ref< Eigen::MatrixXd > cam_intrinsic, Eigen::Ref< Eigen::MatrixXd > pose_mat)
Unified-PnP (UPnP) : Algorithm to compute pose from unknown camera intrinsic matrix
Definition: pnp_algos.cpp:211
mrpt::vision::pnp::CPnP::rpnp
bool rpnp(const Eigen::Ref< Eigen::MatrixXd > obj_pts, const Eigen::Ref< Eigen::MatrixXd > img_pts, int n, const Eigen::Ref< Eigen::MatrixXd > cam_intrinsic, Eigen::Ref< Eigen::MatrixXd > pose_mat)
Robust (R-PnP)- A closed form solution with intermediate P3P computations
Definition: pnp_algos.cpp:357
mrpt::vision::pnp::CPnP::epnp
bool epnp(const Eigen::Ref< Eigen::MatrixXd > obj_pts, const Eigen::Ref< Eigen::MatrixXd > img_pts, int n, const Eigen::Ref< Eigen::MatrixXd > cam_intrinsic, Eigen::Ref< Eigen::MatrixXd > pose_mat)
Efficient-PnP (EPnP) - Algorithm takes 4 control points based on n points and uses the control points...
Definition: pnp_algos.cpp:128
mrpt::vision::pnp::CPnP::dls
bool dls(const Eigen::Ref< Eigen::MatrixXd > obj_pts, const Eigen::Ref< Eigen::MatrixXd > img_pts, int n, const Eigen::Ref< Eigen::MatrixXd > cam_intrinsic, Eigen::Ref< Eigen::MatrixXd > pose_mat)
Direct Least Squares (DLS) - PnP : Algorithm formulates position as a function of rotation.
Definition: pnp_algos.cpp:45


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST