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



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018