Main MRPT website > C++ reference for MRPT 1.9.9
lhm.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 LHM_
11 
12 #define LHM_
13 
14 #include <iostream>
15 
16 #include <mrpt/utils/types_math.h> // Eigen must be included first via MRPT to enable the plugin system
17 #include <Eigen/Dense>
18 #include <Eigen/SVD>
19 #include <Eigen/StdVector>
20 
21 #define TOL_LHM 0.00001
22 #define EPSILON_LHM 0.00000001
23 
24 namespace mrpt
25 {
26 namespace vision
27 {
28 namespace pnp
29 {
30 /** \addtogroup pnp Perspective-n-Point pose estimation
31  * \ingroup mrpt_vision_grp
32  * @{
33  */
34 
35 /**
36  * @class lhm
37  * @author Chandra Mangipudi
38  * @date 10/08/16
39  * @file lhm.h
40  * @brief Lu Hage Mjolsness - Iterative PnP Algorithm (Eigen Implementation
41  */
42 class lhm
43 {
44  Eigen::MatrixXd obj_pts; //! Object points in Camera Co-ordinate system
45  Eigen::MatrixXd img_pts; //! Image points in pixel co-ordinates
46  Eigen::MatrixXd cam_intrinsic; //! Camera intrinsic matrix
47  Eigen::MatrixXd P; //! Trnaspose of Object points @obj_pts
48  Eigen::MatrixXd Q; //! Transpose of Image points @img_pts
49 
50  Eigen::Matrix3d R; //! Matrix for internal computations
51  Eigen::Matrix3d G; //! Rotation Matrix
52  Eigen::Vector3d t; //! Translation Vector
53 
54  std::vector<Eigen::Matrix3d> F; //! Storage matrix for each point
55  double err; //! Error variable for convergence selection
56  double err2; //! Error variable for convergence selection
57 
58  int n; //! Number of 2d/3d correspondences
59 
60  public:
61  //! Constructor for the LHM class
62  lhm(Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_,
63  Eigen::MatrixXd cam_, int n0);
64  //~lhm();
65 
66  /**
67  * @brief Function to compute pose using LHM PnP algorithm
68  * @param R_ Rotation matrix
69  * @param t_ Trnaslation Vector
70  * @return Success flag
71  */
72  bool compute_pose(
73  Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_);
74 
75  /**
76  * @brief Function to compute pose during an iteration
77  */
78  void absKernel();
79 
80  /**
81  * @brief Function to estimate translation given an estimated rotation
82  * matrix
83  */
84  void estimate_t();
85 
86  /**
87  * @brief Transform object points in Body frame (Landmark Frame) to
88  * estimated Camera Frame
89  */
90  void xform();
91 
92  /**
93  * @brief Iternal Function of quaternion
94  * @param q Quaternion
95  * @return
96  */
97  Eigen::Matrix4d qMatQ(Eigen::VectorXd q);
98 
99  /**
100  * @brief Iternal Function of quaternion
101  * @param q Quaternion
102  * @return
103  */
104  Eigen::Matrix4d qMatW(Eigen::VectorXd q);
105 };
106 
107 /** @} */ // end of grouping
108 }
109 }
110 }
111 #endif
Eigen::MatrixXd img_pts
Object points in Camera Co-ordinate system.
Definition: lhm.h:45
void xform()
Transform object points in Body frame (Landmark Frame) to estimated Camera Frame. ...
Definition: lhm.cpp:56
Eigen::MatrixXd P
Camera intrinsic matrix.
Definition: lhm.h:47
double err2
Error variable for convergence selection.
Definition: lhm.h:56
Eigen::MatrixXd obj_pts
Definition: lhm.h:44
void estimate_t()
Function to estimate translation given an estimated rotation matrix.
Definition: lhm.cpp:48
Eigen::Matrix4d qMatW(Eigen::VectorXd q)
Iternal Function of quaternion.
Definition: lhm.cpp:71
Eigen::Matrix3d G
Matrix for internal computations.
Definition: lhm.h:51
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
int n
Error variable for convergence selection.
Definition: lhm.h:58
void absKernel()
Function to compute pose during an iteration.
Definition: lhm.cpp:81
double err
Storage matrix for each point.
Definition: lhm.h:55
Eigen::MatrixXd Q
Trnaspose of Object points .
Definition: lhm.h:48
std::vector< Eigen::Matrix3d > F
Translation Vector.
Definition: lhm.h:54
lhm(Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_, Eigen::MatrixXd cam_, int n0)
Number of 2d/3d correspondences.
Definition: lhm.cpp:29
Eigen::Matrix3d R
Transpose of Image points .
Definition: lhm.h:50
Eigen::Vector3d t
Rotation Matrix.
Definition: lhm.h:52
Eigen::MatrixXd cam_intrinsic
Image points in pixel co-ordinates.
Definition: lhm.h:46
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool compute_pose(Eigen::Ref< Eigen::Matrix3d > R_, Eigen::Ref< Eigen::Vector3d > t_)
Function to compute pose using LHM PnP algorithm.
Definition: lhm.cpp:194
Eigen::Matrix4d qMatQ(Eigen::VectorXd q)
Iternal Function of quaternion.
Definition: lhm.cpp:61



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019