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



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 67178ee79 Fri Apr 3 23:21:14 2020 +0200 at vie abr 3 23:30:09 CEST 2020