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
16
17namespace 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 */
39 const mrpt::math::TPose2D &newPose,
40 const mrpt::math::CMatrixDouble33 &newPoseCov,
42
43 /** Updates the filter so the pose is tracked to the current time */
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 */
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 */
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
A numeric matrix of compile-time fixed size.
A class used to store a 2D pose.
Definition: CPose2D.h:37
A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry an...
bool getCurrentEstimate(mrpt::math::TPose2D &pose, float &v, float &w, mrpt::system::TTimeStamp tim_query=mrpt::system::now()) const
Get the current estimate, obtained as:
mrpt::system::TTimeStamp m_last_odo_time
mrpt::synch::CCriticalSection m_cs
bool getLatestRobotPose(mrpt::math::TPose2D &pose) const
Get the latest known robot pose, either from odometry or localization.
virtual ~CRobot2DPoseEstimator()
Destructor.
bool getLatestRobotPose(CPose2D &pose) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
bool getCurrentEstimate(mrpt::poses::CPose2D &pose, float &v, float &w, mrpt::system::TTimeStamp tim_query=mrpt::system::now()) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
mrpt::system::TTimeStamp m_last_loc_time
mrpt::math::TPose2D m_last_loc
Last pose as estimated by the localization/SLAM subsystem.
void processUpdateNewOdometry(const mrpt::math::TPose2D &newGlobalOdometry, mrpt::system::TTimeStamp cur_tim, bool hasVelocities=false, float v=0, float w=0)
Updates the filter so the pose is tracked to the current time.
CRobot2DPoseEstimator()
Default constructor.
mrpt::math::CMatrixDouble33 m_last_loc_cov
void processUpdateNewPoseLocalization(const mrpt::math::TPose2D &newPose, const mrpt::math::CMatrixDouble33 &newPoseCov, mrpt::system::TTimeStamp cur_tim)
Updates the filter so the pose is tracked to the current time.
static void extrapolateRobotPose(const mrpt::math::TPose2D &p, const float v, const float w, 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,...
mrpt::math::TPose2D m_loc_odo_ref
The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subseq...
TOptions params
parameters of the filter.
This class provides simple critical sections functionality.
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
Definition: datetime.h:30
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 2D pose.
double max_localiz_age
To consider data old, in seconds.
double max_odometry_age
To consider data old, in seconds.



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Fri Jan 20 00:13:14 UTC 2023