Main MRPT website > C++ reference
MRPT logo
CRobot2DPoseEstimator.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (c) 2005-2013, Individual contributors, see AUTHORS file |
7  | Copyright (c) 2005-2013, MAPIR group, University of Malaga |
8  | Copyright (c) 2012-2013, University of Almeria |
9  | All rights reserved. |
10  | |
11  | Redistribution and use in source and binary forms, with or without |
12  | modification, are permitted provided that the following conditions are |
13  | met: |
14  | * Redistributions of source code must retain the above copyright |
15  | notice, this list of conditions and the following disclaimer. |
16  | * Redistributions in binary form must reproduce the above copyright |
17  | notice, this list of conditions and the following disclaimer in the |
18  | documentation and/or other materials provided with the distribution. |
19  | * Neither the name of the copyright holders nor the |
20  | names of its contributors may be used to endorse or promote products |
21  | derived from this software without specific prior written permission.|
22  | |
23  | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
24  | 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED |
25  | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR|
26  | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE |
27  | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL|
28  | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR|
29  | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) |
30  | HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, |
31  | STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
32  | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
33  | POSSIBILITY OF SUCH DAMAGE. |
34  +---------------------------------------------------------------------------+ */
35 #ifndef CRobot2DPoseEstimator_H
36 #define CRobot2DPoseEstimator_H
37 
40 
41 namespace mrpt
42 {
43  namespace poses
44  {
45  using namespace mrpt::math;
46  using namespace mrpt::system;
47 
48  /** A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry and localization data.
49  * The implemented model is a state vector:
50  * - (x,y,phi,v,w)
51  * for the robot pose (x,y,phi) and velocities (v,w).
52  *
53  * The filter can be asked for an extrapolation for some arbitrary time "t'", and it'll do a simple linear prediction.
54  * All methods are thread-safe.
55  * \ingroup poses_grp poses_pdf_grp
56  */
58  {
59  public:
60  CRobot2DPoseEstimator( ); //!< Default constructor
61  virtual ~CRobot2DPoseEstimator(); //!< Destructor
62  void reset();
63 
64  /** Updates the filter so the pose is tracked to the current time */
65  void processUpdateNewPoseLocalization(
66  const TPose2D &newPose,
67  const CMatrixDouble33 &newPoseCov,
68  TTimeStamp cur_tim);
69 
70  /** Updates the filter so the pose is tracked to the current time */
71  void processUpdateNewOdometry(
72  const TPose2D &newGlobalOdometry,
73  TTimeStamp cur_tim,
74  bool hasVelocities = false,
75  float v = 0,
76  float w = 0);
77 
78  /** Get the current estimate, obtained as:
79  *
80  * last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw
81  *
82  * \return true is the estimate can be trusted. False if the real observed data is too old or there is no valid data yet.
83  * \sa getLatestRobotPose
84  */
85  bool getCurrentEstimate( TPose2D &pose, float &v, float &w, TTimeStamp tim_query = mrpt::system::now() ) const;
86 
87  /** Get the current estimate, obtained as:
88  *
89  * last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw
90  *
91  * \return true is the estimate can be trusted. False if the real observed data is too old or there is no valid data yet.
92  * \sa getLatestRobotPose
93  */
94  bool getCurrentEstimate( CPose2D &pose, float &v, float &w, TTimeStamp tim_query = mrpt::system::now() ) const
95  {
96  TPose2D p;
97  bool ret = getCurrentEstimate(p,v,w,tim_query);
98  if (ret)
99  pose = CPose2D(p);
100  return ret;
101  }
102 
103  /** Get the latest known robot pose, either from odometry or localization.
104  * This differs from getCurrentEstimate() in that this method does NOT extrapolate as getCurrentEstimate() does.
105  * \return false if there is not estimation yet.
106  * \sa getCurrentEstimate
107  */
108  bool getLatestRobotPose(TPose2D &pose) const;
109 
110  /** Get the latest known robot pose, either from odometry or localization.
111  * \return false if there is not estimation yet.
112  */
113  inline bool getLatestRobotPose(CPose2D &pose) const
114  {
115  TPose2D p;
116  bool v = getLatestRobotPose(p);
117  if (v)
118  {
119  pose.x(p.x);
120  pose.y(p.y);
121  pose.phi(p.phi);
122  }
123  return v;
124  }
125 
126 
127  struct TOptions
128  {
130  max_odometry_age ( 1.0 ),
131  max_localiz_age ( 4.0 )
132  {}
133 
134  double max_odometry_age; //!< To consider data old, in seconds
135  double max_localiz_age; //!< To consider data old, in seconds
136  };
137 
138  TOptions params; //!< parameters of the filter.
139 
140  private:
142 
144  TPose2D m_last_loc; //!< Last pose as estimated by the localization/SLAM subsystem.
146 
147  TPose2D m_loc_odo_ref; //!< The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subsequent odo readings)
148 
151  float m_robot_v;
152  float m_robot_w;
153 
154  /** An auxiliary method to extrapolate the pose of a robot located at "p" with velocities (v,w) after a time delay "delta_time".
155  */
156  static void extrapolateRobotPose(
157  const TPose2D &p,
158  const float v,
159  const float w,
160  const double delta_time,
161  TPose2D &new_p);
162 
163  }; // end of class
164 
165  } // End of namespace
166 } // End of namespace
167 
168 #endif
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:54
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:142
This class provides simple critical sections functionality.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:58
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:94
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:45
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:61
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:109
Lightweight 2D pose.
TOptions params
parameters of the filter.
double phi
Phi coordinate.



Page generated by Doxygen 1.8.14 for MRPT 1.0.2 SVN: at lun oct 28 00:52:41 CET 2019 Hosted on:
SourceForge.net Logo