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



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo