MRPT  1.9.9
rpnp.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 #pragma once
10 
11 //#include <mrpt/math/types_math.h> // Eigen must be included first via MRPT to
12 // enable the plugin system
13 
14 namespace mrpt::vision::pnp
15 {
16 /** \addtogroup pnp Perspective-n-Point pose estimation
17  * \ingroup mrpt_vision_grp
18  * @{
19  */
20 /**
21  * @class rpnp
22  * @author Chandra Mangipudi
23  * @date 10/08/16
24  * @file rpnp.h
25  * @brief Robust - PnP class definition for computing pose
26  */
27 class rpnp
28 {
29  Eigen::MatrixXd
30  obj_pts; //! Object Points (n X 3) in Camera Co-ordinate system
31  Eigen::MatrixXd
32  img_pts; //! Image Points (n X 3) in Camera Co-ordinate system
33  Eigen::MatrixXd cam_intrinsic; //! Camera Intrinsic Matrix
34  Eigen::MatrixXd P; //! Transposed Object Points (3 X n) for computations
35  Eigen::MatrixXd Q; //! Transposed Image Points (3 X n) for computations
36 
37  Eigen::Matrix3d R; //! Rotation matrix
38  Eigen::Vector3d t; //! Translation vector
39  int n; //! Number of 2D/3D correspondences
40 
41  public:
42  //! Constructor for rpnp class
43  rpnp(
44  Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_,
45  Eigen::MatrixXd cam_, int n0);
46 
47  /**
48  * @brief Function to compute pose
49  * @param[out] R_ Rotaiton matrix
50  * @param[out] t_ Translation vector
51  * @return Success flag
52  */
53  bool compute_pose(
54  Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_);
55 
56  /**
57  * @brief Function to compute pose using P3P
58  * @param[in] l1 Internal parameter for P3P computation
59  * @param[in] l2 Internal parameter for P3P computation
60  * @param[in] A5 Internal parameter for P3P computation
61  * @param[in] C1 Internal parameter for P3P computation
62  * @param[in] C2 Internal parameter for P3P computation
63  * @param[in] D1 Internal parameter for P3P computation
64  * @param[in] D2 Internal parameter for P3P computation
65  * @param[in] D3 Internal parameter for P3P computation
66  * @return Output vector
67  */
68  Eigen::VectorXd getp3p(
69  double l1, double l2, double A5, double C1, double C2, double D1,
70  double D2, double D3);
71 
72  /**
73  * @brief Get Polynomial from input vector
74  * @param vin Input vector
75  * @return Output Polynomial co-efficients
76  */
77  Eigen::VectorXd getpoly7(const Eigen::VectorXd& vin);
78 
79  /**
80  * @brief Function to calculate final pose
81  * @param XXc Object points
82  * @param XXw Image Points
83  * @param R2 Final Rotation matrix
84  * @param t2 Final Translation vector
85  */
86  void calcampose(
87  Eigen::MatrixXd& XXc, Eigen::MatrixXd& XXw, Eigen::Matrix3d& R2,
88  Eigen::Vector3d& t2);
89 };
90 
91 /** @} */ // end of grouping
92 } // namespace mrpt::vision::pnp
Eigen::MatrixXd P
Camera Intrinsic Matrix.
Definition: rpnp.h:34
void calcampose(Eigen::MatrixXd &XXc, Eigen::MatrixXd &XXw, Eigen::Matrix3d &R2, Eigen::Vector3d &t2)
Function to calculate final pose.
Definition: rpnp.cpp:318
Eigen::MatrixXd cam_intrinsic
Image Points (n X 3) in Camera Co-ordinate system.
Definition: rpnp.h:33
Eigen::Vector3d t
Rotation matrix.
Definition: rpnp.h:38
Perspective n Point (PnP) Algorithms toolkit for MRPT mrpt_vision_grp.
Definition: pnp_algos.h:23
rpnp(Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_, Eigen::MatrixXd cam_, int n0)
Number of 2D/3D correspondences.
Definition: rpnp.cpp:24
Eigen::VectorXd getp3p(double l1, double l2, double A5, double C1, double C2, double D1, double D2, double D3)
Function to compute pose using P3P.
Definition: rpnp.cpp:367
Eigen::MatrixXd img_pts
Object Points (n X 3) in Camera Co-ordinate system.
Definition: rpnp.h:32
bool compute_pose(Eigen::Ref< Eigen::Matrix3d > R_, Eigen::Ref< Eigen::Vector3d > t_)
Function to compute pose.
Definition: rpnp.cpp:43
int n
Translation vector.
Definition: rpnp.h:39
Eigen::MatrixXd obj_pts
Definition: rpnp.h:30
Eigen::MatrixXd Q
Transposed Object Points (3 X n) for computations.
Definition: rpnp.h:35
Eigen::Matrix3d R
Transposed Image Points (3 X n) for computations.
Definition: rpnp.h:37
Eigen::VectorXd getpoly7(const Eigen::VectorXd &vin)
Get Polynomial from input vector.
Definition: rpnp.cpp:355



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