Main MRPT website > C++ reference for MRPT 1.4.0
CRobot2DPoseEstimator.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-2016, 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 #ifndef CRobot2DPoseEstimator_H
10 #define CRobot2DPoseEstimator_H
11 
15 #include <mrpt/system/datetime.h>
16 
17 namespace mrpt
18 {
19  namespace poses
20  {
21  /** A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry and localization data.
22  * The implemented model is a state vector:
23  * - (x,y,phi,v,w)
24  * for the robot pose (x,y,phi) and velocities (v,w).
25  *
26  * The filter can be asked for an extrapolation for some arbitrary time "t'", and it'll do a simple linear prediction.
27  * All methods are thread-safe.
28  * \ingroup poses_grp poses_pdf_grp
29  */
31  {
32  public:
33  CRobot2DPoseEstimator( ); //!< Default constructor
34  virtual ~CRobot2DPoseEstimator(); //!< Destructor
35  void reset();
36 
37  /** Updates the filter so the pose is tracked to the current time */
38  void processUpdateNewPoseLocalization(
39  const mrpt::math::TPose2D &newPose,
40  const mrpt::math::CMatrixDouble33 &newPoseCov,
41  mrpt::system::TTimeStamp cur_tim);
42 
43  /** Updates the filter so the pose is tracked to the current time */
44  void processUpdateNewOdometry(
45  const mrpt::math::TPose2D &newGlobalOdometry,
47  bool hasVelocities = false,
48  float v = 0,
49  float w = 0);
50 
51  /** Get the current estimate, obtained as:
52  *
53  * last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw
54  *
55  * \return true is the estimate can be trusted. False if the real observed data is too old or there is no valid data yet.
56  * \sa getLatestRobotPose
57  */
58  bool getCurrentEstimate( mrpt::math::TPose2D &pose, float &v, float &w, mrpt::system::TTimeStamp tim_query = mrpt::system::now() ) const;
59 
60  /** \overload */
61  bool getCurrentEstimate( mrpt::poses::CPose2D &pose, float &v, float &w, mrpt::system::TTimeStamp tim_query = mrpt::system::now() ) const;
62 
63  /** Get the latest known robot pose, either from odometry or localization.
64  * This differs from getCurrentEstimate() in that this method does NOT extrapolate as getCurrentEstimate() does.
65  * \return false if there is not estimation yet.
66  * \sa getCurrentEstimate
67  */
68  bool getLatestRobotPose(mrpt::math::TPose2D &pose) const;
69 
70  /** \overload */
71  bool getLatestRobotPose(CPose2D &pose) const;
72 
73  struct TOptions
74  {
76  max_odometry_age ( 1.0 ),
77  max_localiz_age ( 4.0 )
78  {}
79 
80  double max_odometry_age; //!< To consider data old, in seconds
81  double max_localiz_age; //!< To consider data old, in seconds
82  };
83 
84  TOptions params; //!< parameters of the filter.
85 
86  private:
88 
90  mrpt::math::TPose2D m_last_loc; //!< Last pose as estimated by the localization/SLAM subsystem.
92 
93  mrpt::math::TPose2D m_loc_odo_ref; //!< The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subsequent odo readings)
94 
97  float m_robot_v;
98  float m_robot_w;
99 
100  /** An auxiliary method to extrapolate the pose of a robot located at "p" with velocities (v,w) after a time delay "delta_time".
101  */
102  static void extrapolateRobotPose(
103  const mrpt::math::TPose2D &p,
104  const float v,
105  const float w,
106  const double delta_time,
107  mrpt::math::TPose2D &new_p);
108 
109  }; // end of class
110 
111  } // End of namespace
112 } // End of namespace
113 
114 #endif
mrpt::poses::CRobot2DPoseEstimator::m_last_loc_cov
mrpt::math::CMatrixDouble33 m_last_loc_cov
Definition: CRobot2DPoseEstimator.h:91
mrpt::poses::CRobot2DPoseEstimator::m_last_odo
mrpt::math::TPose2D m_last_odo
Definition: CRobot2DPoseEstimator.h:96
CMatrixFixedNumeric.h
mrpt::poses::CRobot2DPoseEstimator::params
TOptions params
parameters of the filter.
Definition: CRobot2DPoseEstimator.h:84
mrpt::poses::CRobot2DPoseEstimator::m_robot_w
float m_robot_w
Definition: CRobot2DPoseEstimator.h:98
mrpt::poses::CRobot2DPoseEstimator::TOptions::max_odometry_age
double max_odometry_age
To consider data old, in seconds.
Definition: CRobot2DPoseEstimator.h:80
mrpt::system::now
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::system::TTimeStamp
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
Definition: datetime.h:30
lightweight_geom_data.h
mrpt::poses::CRobot2DPoseEstimator::m_last_odo_time
mrpt::system::TTimeStamp m_last_odo_time
Definition: CRobot2DPoseEstimator.h:95
mrpt::poses::CRobot2DPoseEstimator::m_cs
mrpt::synch::CCriticalSection m_cs
Definition: CRobot2DPoseEstimator.h:87
mrpt::synch::CCriticalSection
This class provides simple critical sections functionality.
Definition: CCriticalSection.h:31
mrpt::poses::CPose2D
A class used to store a 2D pose.
Definition: CPose2D.h:36
mrpt::poses::CRobot2DPoseEstimator::m_last_loc_time
mrpt::system::TTimeStamp m_last_loc_time
Definition: CRobot2DPoseEstimator.h:89
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: lightweight_geom_data.h:148
mrpt::poses::CRobot2DPoseEstimator::TOptions::TOptions
TOptions()
Definition: CRobot2DPoseEstimator.h:75
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:34
mrpt::poses::CRobot2DPoseEstimator::TOptions::max_localiz_age
double max_localiz_age
To consider data old, in seconds.
Definition: CRobot2DPoseEstimator.h:81
mrpt::poses::CRobot2DPoseEstimator
A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry an...
Definition: CRobot2DPoseEstimator.h:30
mrpt::poses::CRobot2DPoseEstimator::m_robot_v
float m_robot_v
Definition: CRobot2DPoseEstimator.h:97
mrpt::poses::CRobot2DPoseEstimator::TOptions
Definition: CRobot2DPoseEstimator.h:73
CCriticalSection.h
mrpt::poses::CRobot2DPoseEstimator::m_last_loc
mrpt::math::TPose2D m_last_loc
Last pose as estimated by the localization/SLAM subsystem.
Definition: CRobot2DPoseEstimator.h:90
datetime.h
mrpt::poses::CRobot2DPoseEstimator::m_loc_odo_ref
mrpt::math::TPose2D m_loc_odo_ref
The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subseq...
Definition: CRobot2DPoseEstimator.h:93



Page generated by Doxygen 1.8.17 for MRPT 1.4.0 SVN: at Tue Mar 3 09:15:16 UTC 2020