Main MRPT website > C++ reference for MRPT 1.5.6
posit.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 #include <mrpt/utils/types_math.h> // Eigen must be included first via MRPT to enable the plugin system
11 #include <Eigen/Dense>
12 #include <Eigen/SVD>
13 
14 #include <iostream>
15 
16 #define LOOP_MAX_COUNT 30
17 
18 namespace mrpt
19 {
20  namespace vision
21  {
22  namespace pnp
23  {
24  /** \addtogroup pnp Perspective-n-Point pose estimation
25  * \ingroup mrpt_vision_grp
26  * @{
27  */
28 
29  /**
30  * @class POSIT
31  * @author Chandra Mangipudi
32  * @date 10/08/16
33  * @file posit.h
34  * @brief Pose from Orthogonality and Scaling (POSIT) - Eigen Implementation
35  */
36  class posit
37  {
38 
39  Eigen::MatrixXd obj_pts; //! Object Points in Camera Co-ordinate system
40  Eigen::MatrixXd img_pts; //! Image Points in pixels
41  Eigen::MatrixXd cam_intrinsic; //! Camera Intrinsic matrix
42  Eigen::MatrixXd obj_matrix; //! Pseudo-Inverse of Object Points matrix
43 
44  double f; //! Focal Length from camera intrinsic matrix
45  Eigen::VectorXd epsilons; //! Co-efficients used for scaling
46  int n; //! Number of 2d/3d correspondences
47 
48  Eigen::MatrixXd R; //! Rotation Matrix
49  Eigen::VectorXd t; //! Translation Vector
50 
51  Eigen::MatrixXd obj_vecs; //! Object Points relative to 1st object point
52  Eigen::MatrixXd img_vecs; //! Image Points relative to 1st image point
53  Eigen::MatrixXd img_vecs_old; //! Used to store img_vecs from previous iteration
54 
55  public:
56 
57  //! Constructor for P-PnP class
58  posit(Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_, Eigen::MatrixXd camera_intrinsic_, int n);
59 
60  /**
61  * @brief Function used to compute pose from orthogonality
62  */
63  void POS();
64 
65  /**
66  * @brief Computes pose using iterative computation of @func POS()
67  * @param[out] R_
68  * @param[out] t_
69  * @return true on success
70  */
71  bool compute_pose(Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_);
72 
73  /**
74  * @brief Function to check for convergence
75  * @return Representative value of error
76  */
77  long get_img_diff();
78 
79  };
80 
81  /** @} */ // end of grouping
82 
83  }
84  }
85 }
86 
Eigen::MatrixXd obj_pts
Definition: posit.h:39
bool compute_pose(Eigen::Ref< Eigen::Matrix3d > R_, Eigen::Ref< Eigen::Vector3d > t_)
Computes pose using iterative computation of POS()
Definition: posit.cpp:82
Eigen::VectorXd epsilons
Focal Length from camera intrinsic matrix.
Definition: posit.h:45
GLsizei n
Definition: glew.h:5051
Eigen::MatrixXd obj_vecs
Translation Vector.
Definition: posit.h:51
Eigen::MatrixXd R
Number of 2d/3d correspondences.
Definition: posit.h:48
int n
Co-efficients used for scaling.
Definition: posit.h:46
Eigen::VectorXd t
Rotation Matrix.
Definition: posit.h:49
posit(Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_, Eigen::MatrixXd camera_intrinsic_, int n)
Used to store img_vecs from previous iteration.
Definition: posit.cpp:19
Eigen::MatrixXd img_pts
Object Points in Camera Co-ordinate system.
Definition: posit.h:40
long get_img_diff()
Function to check for convergence.
Definition: posit.cpp:139
Eigen::MatrixXd img_vecs
Object Points relative to 1st object point.
Definition: posit.h:52
double f
Pseudo-Inverse of Object Points matrix.
Definition: posit.h:44
Eigen::MatrixXd obj_matrix
Camera Intrinsic matrix.
Definition: posit.h:42
Eigen::MatrixXd cam_intrinsic
Image Points in pixels.
Definition: posit.h:41
Eigen::MatrixXd img_vecs_old
Image Points relative to 1st image point.
Definition: posit.h:53
void POS()
Function used to compute pose from orthogonality.
Definition: posit.cpp:43



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