Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 
14 #include <mrpt/system/datetime.h>
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 asynchronous odometry and localization/SLAM data.
21  * The implemented model is a state vector:
22  * - TPose2D (x,y,phi) + TTwist2D (vx,vy,omega)
23  * The filter can be asked for an extrapolation for some arbitrary time `t'`, and it'll do a simple linear prediction.
24  * **All methods are thread-safe**.
25  * \ingroup poses_grp poses_pdf_grp
26  */
28  {
29  public:
30  CRobot2DPoseEstimator( ); //!< Default constructor
31  virtual ~CRobot2DPoseEstimator(); //!< Destructor
32  void reset(); //!< Resets all internal state.
33 
34  /** Updates the filter with new global-coordinates localization data from a localization or SLAM source.
35  * \param tim The timestamp of the sensor readings used to evaluate localization / SLAM.
36  */
37  void processUpdateNewPoseLocalization(
38  const mrpt::math::TPose2D &newPose,
40 
41  /** Updates the filter with new odometry readings. */
42  void processUpdateNewOdometry(
43  const mrpt::math::TPose2D &newGlobalOdometry,
45  bool hasVelocities = false,
46  const mrpt::math::TTwist2D &newRobotVelLocal = mrpt::math::TTwist2D() );
47 
48  /** Get the estimate for a given timestamp (defaults to `now()`), obtained as:
49  *
50  * last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw
51  *
52  * \return true is the estimate can be trusted. False if the real observed data is too old or there is no valid data yet.
53  * \sa getLatestRobotPose
54  */
55  bool getCurrentEstimate(mrpt::math::TPose2D &pose, mrpt::math::TTwist2D &velLocal, mrpt::math::TTwist2D &velGlobal, mrpt::system::TTimeStamp tim_query = mrpt::system::now()) const;
56 
57  MRPT_DEPRECATED("Use the other signature with TTwist2D output for velocities")
58  bool getCurrentEstimate( mrpt::math::TPose2D &pose, float &v, float &w, mrpt::system::TTimeStamp tim_query = mrpt::system::now() ) const;
59  MRPT_DEPRECATED("Use the other signature with TTwist2D output for velocities")
60  bool getCurrentEstimate( mrpt::poses::CPose2D &pose, float &v, float &w, mrpt::system::TTimeStamp tim_query = mrpt::system::now() ) const;
61 
62  /** Get the latest known robot pose, either from odometry or localization.
63  * This differs from getCurrentEstimate() in that this method does NOT extrapolate as getCurrentEstimate() does.
64  * \return false if there is not estimation yet.
65  * \sa getCurrentEstimate
66  */
67  bool getLatestRobotPose(mrpt::math::TPose2D &pose) const;
68 
69  /** \overload */
70  bool getLatestRobotPose(CPose2D &pose) const;
71 
72  struct TOptions
73  {
75  max_odometry_age ( 1.0 ),
76  max_localiz_age ( 4.0 )
77  {}
78 
79  double max_odometry_age; //!< To consider data old, in seconds
80  double max_localiz_age; //!< To consider data old, in seconds
81  };
82 
83  TOptions params; //!< parameters of the filter.
84 
85  private:
87 
89  mrpt::math::TPose2D m_last_loc; //!< Last pose as estimated by the localization/SLAM subsystem.
90 
91  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)
92 
95  mrpt::math::TTwist2D m_robot_vel_local; //!< Robot odometry-based velocity in a local frame of reference.
96 
97  /** An auxiliary method to extrapolate the pose of a robot located at "p" with velocities (v,w) after a time delay "delta_time". */
98  static void extrapolateRobotPose(
99  const mrpt::math::TPose2D &p,
100  const mrpt::math::TTwist2D &robot_vel_local,
101  const double delta_time,
102  mrpt::math::TPose2D &new_p);
103 
104  }; // end of class
105 
106  } // End of namespace
107 } // End of namespace
108 
109 #endif
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
This class provides simple critical sections functionality.
mrpt::math::TPose2D m_loc_odo_ref
The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subseq...
double max_odometry_age
To consider data old, in seconds.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry an...
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
mrpt::math::TPose2D m_last_loc
Last pose as estimated by the localization/SLAM subsystem.
mrpt::synch::CCriticalSection m_cs
mrpt::system::TTimeStamp m_last_loc_time
mrpt::system::TTimeStamp m_last_odo_time
double max_localiz_age
To consider data old, in seconds.
const GLdouble * v
Definition: glext.h:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
#define MRPT_DEPRECATED(msg)
Usage: MRPT_DEPRECATED("Use XX instead") void myFunc(double);.
GLboolean reset
Definition: glext.h:3535
Lightweight 2D pose.
TOptions params
parameters of the filter.
mrpt::math::TTwist2D m_robot_vel_local
Robot odometry-based velocity in a local frame of reference.
GLfloat GLfloat p
Definition: glext.h:5587



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019