Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 
13 #include <mrpt/system/datetime.h>
14 #include <mutex>
15 
16 namespace mrpt
17 {
18 namespace poses
19 {
20 /** A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from
21  *asynchronous odometry and localization/SLAM data.
22  * The implemented model is a state vector:
23  * - TPose2D (x,y,phi) + TTwist2D (vx,vy,omega)
24  * The filter can be asked for an extrapolation for some arbitrary time `t'`,
25  *and it'll do a simple linear prediction.
26  * **All methods are thread-safe**.
27  * \ingroup poses_grp poses_pdf_grp
28  */
30 {
31  public:
32  /** Default constructor */
34  /** Destructor */
35  virtual ~CRobot2DPoseEstimator();
36  /** Resets all internal state. */
37  void reset();
38 
39  /** Updates the filter with new global-coordinates localization data from a
40  * localization or SLAM source.
41  * \param tim The timestamp of the sensor readings used to evaluate
42  * localization / SLAM.
43  */
45  const mrpt::math::TPose2D& newPose, mrpt::system::TTimeStamp tim);
46 
47  /** Updates the filter with new odometry readings. */
49  const mrpt::math::TPose2D& newGlobalOdometry,
50  mrpt::system::TTimeStamp cur_tim, bool hasVelocities = false,
51  const mrpt::math::TTwist2D& newRobotVelLocal = mrpt::math::TTwist2D());
52 
53  /** Get the estimate for a given timestamp (defaults to `now()`), obtained
54  * as:
55  *
56  * last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw
57  *
58  * \return true is the estimate can be trusted. False if the real observed
59  * data is too old or there is no valid data yet.
60  * \sa getLatestRobotPose
61  */
62  bool getCurrentEstimate(
64  mrpt::math::TTwist2D& velGlobal,
65  mrpt::system::TTimeStamp tim_query = mrpt::system::now()) const;
66 
67  /** Get the latest known robot pose, either from odometry or localization.
68  * This differs from getCurrentEstimate() in that this method does NOT
69  * extrapolate as getCurrentEstimate() does.
70  * \return false if there is not estimation yet.
71  * \sa getCurrentEstimate
72  */
73  bool getLatestRobotPose(mrpt::math::TPose2D& pose) const;
74 
75  /** \overload */
76  bool getLatestRobotPose(CPose2D& pose) const;
77 
78  struct TOptions
79  {
81  /** To consider data old, in seconds */
83  /** To consider data old, in seconds */
85  };
86 
87  /** parameters of the filter. */
89 
90  private:
91  std::mutex m_cs;
92 
94  /** Last pose as estimated by the localization/SLAM subsystem. */
96 
97  /** The interpolated odometry position for the last "m_robot_pose" (used as
98  * "coordinates base" for subsequent odo readings) */
100 
103  /** Robot odometry-based velocity in a local frame of reference. */
105 
106  /** An auxiliary method to extrapolate the pose of a robot located at "p"
107  * with velocities (v,w) after a time delay "delta_time". */
108  static void extrapolateRobotPose(
109  const mrpt::math::TPose2D& p,
110  const mrpt::math::TTwist2D& robot_vel_local, const double delta_time,
111  mrpt::math::TPose2D& new_p);
112 
113 }; // end of class
114 
115 } // End of namespace
116 } // End of namespace
117 
118 #endif
mrpt::poses::CRobot2DPoseEstimator::m_last_odo
mrpt::math::TPose2D m_last_odo
Definition: CRobot2DPoseEstimator.h:102
mrpt::poses::CRobot2DPoseEstimator::getCurrentEstimate
bool getCurrentEstimate(mrpt::math::TPose2D &pose, mrpt::math::TTwist2D &velLocal, mrpt::math::TTwist2D &velGlobal, mrpt::system::TTimeStamp tim_query=mrpt::system::now()) const
Get the estimate for a given timestamp (defaults to now()), obtained as:
Definition: CRobot2DPoseEstimator.cpp:109
mrpt::poses::CRobot2DPoseEstimator::processUpdateNewPoseLocalization
void processUpdateNewPoseLocalization(const mrpt::math::TPose2D &newPose, mrpt::system::TTimeStamp tim)
Updates the filter with new global-coordinates localization data from a localization or SLAM source.
Definition: CRobot2DPoseEstimator.cpp:52
mrpt::poses::CRobot2DPoseEstimator::params
TOptions params
parameters of the filter.
Definition: CRobot2DPoseEstimator.h:88
mrpt::poses::CRobot2DPoseEstimator::TOptions::max_odometry_age
double max_odometry_age
To consider data old, in seconds.
Definition: CRobot2DPoseEstimator.h:82
mrpt::system::now
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:75
mrpt::poses::CRobot2DPoseEstimator::processUpdateNewOdometry
void processUpdateNewOdometry(const mrpt::math::TPose2D &newGlobalOdometry, mrpt::system::TTimeStamp cur_tim, bool hasVelocities=false, const mrpt::math::TTwist2D &newRobotVelLocal=mrpt::math::TTwist2D())
Updates the filter with new odometry readings.
Definition: CRobot2DPoseEstimator.cpp:70
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::poses::CRobot2DPoseEstimator::extrapolateRobotPose
static void extrapolateRobotPose(const mrpt::math::TPose2D &p, const mrpt::math::TTwist2D &robot_vel_local, const double delta_time, mrpt::math::TPose2D &new_p)
An auxiliary method to extrapolate the pose of a robot located at "p" with velocities (v,...
Definition: CRobot2DPoseEstimator.cpp:174
p
GLfloat GLfloat p
Definition: glext.h:6305
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:31
lightweight_geom_data.h
mrpt::poses::CRobot2DPoseEstimator::m_last_odo_time
mrpt::system::TTimeStamp m_last_odo_time
Definition: CRobot2DPoseEstimator.h:101
mrpt::poses::CRobot2DPoseEstimator::m_cs
std::mutex m_cs
Definition: CRobot2DPoseEstimator.h:91
mrpt::poses::CRobot2DPoseEstimator::getLatestRobotPose
bool getLatestRobotPose(mrpt::math::TPose2D &pose) const
Get the latest known robot pose, either from odometry or localization.
Definition: CRobot2DPoseEstimator.cpp:144
mrpt::poses::CRobot2DPoseEstimator::CRobot2DPoseEstimator
CRobot2DPoseEstimator()
Default constructor.
Definition: CRobot2DPoseEstimator.cpp:29
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:40
mrpt::poses::CRobot2DPoseEstimator::m_last_loc_time
mrpt::system::TTimeStamp m_last_loc_time
Definition: CRobot2DPoseEstimator.h:93
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: lightweight_geom_data.h:186
mrpt::poses::CRobot2DPoseEstimator::~CRobot2DPoseEstimator
virtual ~CRobot2DPoseEstimator()
Destructor.
Definition: CRobot2DPoseEstimator.cpp:33
mrpt::poses::CRobot2DPoseEstimator::TOptions::TOptions
TOptions()
Definition: CRobot2DPoseEstimator.h:80
mrpt::poses::CRobot2DPoseEstimator::TOptions::max_localiz_age
double max_localiz_age
To consider data old, in seconds.
Definition: CRobot2DPoseEstimator.h:84
mrpt::poses::CRobot2DPoseEstimator
A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry an...
Definition: CRobot2DPoseEstimator.h:29
mrpt::poses::CRobot2DPoseEstimator::m_robot_vel_local
mrpt::math::TTwist2D m_robot_vel_local
Robot odometry-based velocity in a local frame of reference.
Definition: CRobot2DPoseEstimator.h:104
mrpt::poses::CRobot2DPoseEstimator::TOptions
Definition: CRobot2DPoseEstimator.h:78
mrpt::poses::CRobot2DPoseEstimator::reset
void reset()
Resets all internal state.
Definition: CRobot2DPoseEstimator.cpp:37
mrpt::poses::CRobot2DPoseEstimator::m_last_loc
mrpt::math::TPose2D m_last_loc
Last pose as estimated by the localization/SLAM subsystem.
Definition: CRobot2DPoseEstimator.h:95
datetime.h
mrpt::math::TTwist2D
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Definition: lightweight_geom_data.h:2145
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:99



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST