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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 9690a7a25 Mon Feb 24 17:42:02 2020 +0100 at lun feb 24 18:00:10 CET 2020