Main MRPT website > C++ reference
MRPT logo
CRobot2DPoseEstimator.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CRobot2DPoseEstimator_H
29 #define CRobot2DPoseEstimator_H
30 
31 #include <mrpt/synch/CCriticalSection.h>
32 #include <mrpt/math/lightweight_geom_data.h>
33 
34 namespace mrpt
35 {
36  namespace poses
37  {
38  using namespace mrpt::math;
39  using namespace mrpt::system;
40 
41  /** A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry and localization data.
42  * The implemented model is a state vector:
43  * - (x,y,phi,v,w)
44  * for the robot pose (x,y,phi) and velocities (v,w).
45  *
46  * The filter can be asked for an extrapolation for some arbitrary time "t'", and it'll do a simple linear prediction.
47  * All methods are thread-safe.
48  * \ingroup poses_grp poses_pdf_grp
49  */
50  class BASE_IMPEXP CRobot2DPoseEstimator
51  {
52  public:
53  CRobot2DPoseEstimator( ); //!< Default constructor
54  virtual ~CRobot2DPoseEstimator(); //!< Destructor
55  void reset();
56 
57  /** Updates the filter so the pose is tracked to the current time */
58  void processUpdateNewPoseLocalization(
59  const TPose2D &newPose,
60  const CMatrixDouble33 &newPoseCov,
61  TTimeStamp cur_tim);
62 
63  /** Updates the filter so the pose is tracked to the current time */
64  void processUpdateNewOdometry(
65  const TPose2D &newGlobalOdometry,
66  TTimeStamp cur_tim,
67  bool hasVelocities = false,
68  float v = 0,
69  float w = 0);
70 
71  /** Get the current estimate, obtained as:
72  *
73  * last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw
74  *
75  * \return true is the estimate can be trusted. False if the real observed data is too old or there is no valid data yet.
76  * \sa getLatestRobotPose
77  */
78  bool getCurrentEstimate( TPose2D &pose, float &v, float &w, TTimeStamp tim_query = mrpt::system::now() ) const;
79 
80  /** Get the current estimate, obtained as:
81  *
82  * last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw
83  *
84  * \return true is the estimate can be trusted. False if the real observed data is too old or there is no valid data yet.
85  * \sa getLatestRobotPose
86  */
87  bool getCurrentEstimate( CPose2D &pose, float &v, float &w, TTimeStamp tim_query = mrpt::system::now() ) const
88  {
89  TPose2D p;
90  bool ret = getCurrentEstimate(p,v,w,tim_query);
91  if (ret)
92  pose = CPose2D(p);
93  return ret;
94  }
95 
96  /** Get the latest known robot pose, either from odometry or localization.
97  * This differs from getCurrentEstimate() in that this method does NOT extrapolate as getCurrentEstimate() does.
98  * \return false if there is not estimation yet.
99  * \sa getCurrentEstimate
100  */
101  bool getLatestRobotPose(TPose2D &pose) const;
102 
103  /** Get the latest known robot pose, either from odometry or localization.
104  * \return false if there is not estimation yet.
105  */
106  inline bool getLatestRobotPose(CPose2D &pose) const
107  {
108  TPose2D p;
109  bool v = getLatestRobotPose(p);
110  if (v)
111  {
112  pose.x(p.x);
113  pose.y(p.y);
114  pose.phi(p.phi);
115  }
116  return v;
117  }
118 
119 
120  struct TOptions
121  {
122  TOptions() :
123  max_odometry_age ( 1.0 ),
124  max_localiz_age ( 4.0 )
125  {}
126 
127  double max_odometry_age; //!< To consider data old, in seconds
128  double max_localiz_age; //!< To consider data old, in seconds
129  };
130 
131  TOptions params; //!< parameters of the filter.
132 
133  private:
134  mrpt::synch::CCriticalSection m_cs;
135 
136  TTimeStamp m_last_loc_time;
137  TPose2D m_last_loc; //!< Last pose as estimated by the localization/SLAM subsystem.
138  CMatrixDouble33 m_last_loc_cov;
139 
140  TPose2D m_loc_odo_ref; //!< The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subsequent odo readings)
141 
142  TTimeStamp m_last_odo_time;
143  TPose2D m_last_odo;
144  float m_robot_v;
145  float m_robot_w;
146 
147  /** An auxiliary method to extrapolate the pose of a robot located at "p" with velocities (v,w) after a time delay "delta_time".
148  */
149  static void extrapolateRobotPose(
150  const TPose2D &p,
151  const float v,
152  const float w,
153  const double delta_time,
154  TPose2D &new_p);
155 
156  }; // end of class
157 
158  } // End of namespace
159 } // End of namespace
160 
161 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013