Main MRPT website > C++ reference
MRPT logo

CRobot2DPoseEstimator.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CRobot2DPoseEstimator_H
00029 #define CRobot2DPoseEstimator_H
00030 
00031 #include <mrpt/synch/CCriticalSection.h>
00032 #include <mrpt/math/lightweight_geom_data.h>
00033 
00034 namespace mrpt
00035 {
00036         namespace poses
00037         {
00038                 using namespace mrpt::math;
00039                 using namespace mrpt::system;
00040 
00041                 /** A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry and localization data.
00042                   *  The implemented model is a state vector:
00043                   *             - (x,y,phi,v,w)
00044                   *  for the robot pose (x,y,phi) and velocities (v,w).
00045                   *
00046                   *  The filter can be asked for an extrapolation for some arbitrary time "t'", and it'll do a simple linear prediction.
00047                   *  All methods are thread-safe.
00048                   */
00049                 class BASE_IMPEXP CRobot2DPoseEstimator
00050                 {
00051                 public:
00052                          CRobot2DPoseEstimator( ); //!< Default constructor
00053                          virtual ~CRobot2DPoseEstimator();      //!< Destructor
00054                          void reset();
00055 
00056                          /** Updates the filter so the pose is tracked to the current time */
00057                          void processUpdateNewPoseLocalization(
00058                                  const TPose2D &newPose,
00059                                  const CMatrixDouble33 &newPoseCov,
00060                                  TTimeStamp cur_tim);
00061 
00062                          /** Updates the filter so the pose is tracked to the current time */
00063                          void processUpdateNewOdometry(
00064                                  const TPose2D &newGlobalOdometry,
00065                                  TTimeStamp cur_tim,
00066                                  bool hasVelocities = false,
00067                                  float v = 0,
00068                                  float w = 0);
00069 
00070                          /** Get the current estimate, obtained as:
00071                            *
00072                            *   last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw
00073                            *
00074                            * \return true is the estimate can be trusted. False if the real observed data is too old or there is no valid data yet.
00075                            * \sa getLatestRobotPose
00076                            */
00077                          bool getCurrentEstimate( TPose2D &pose, float &v, float &w, TTimeStamp tim_query = mrpt::system::now() ) const;
00078 
00079                          /** Get the current estimate, obtained as:
00080                            *
00081                            *   last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw
00082                            *
00083                            * \return true is the estimate can be trusted. False if the real observed data is too old or there is no valid data yet.
00084                            * \sa getLatestRobotPose
00085                            */
00086                          bool getCurrentEstimate( CPose2D &pose, float &v, float &w, TTimeStamp tim_query = mrpt::system::now() ) const
00087                          {
00088                                 TPose2D  p;
00089                                 bool ret = getCurrentEstimate(p,v,w,tim_query);
00090                                 if (ret) 
00091                                         pose = CPose2D(p);
00092                                 return ret;                             
00093                          }
00094                          
00095                          /** Get the latest known robot pose, either from odometry or localization. 
00096                            *  This differs from getCurrentEstimate() in that this method does NOT extrapolate as getCurrentEstimate() does.
00097                            * \return false if there is not estimation yet.
00098                            * \sa getCurrentEstimate
00099                            */
00100                          bool getLatestRobotPose(TPose2D &pose) const;
00101                          
00102                          /** Get the latest known robot pose, either from odometry or localization.
00103                            * \return false if there is not estimation yet.
00104                            */
00105                          inline bool getLatestRobotPose(CPose2D &pose) const
00106                          {
00107                                 TPose2D p;
00108                                 bool v = getLatestRobotPose(p);
00109                                 if (v) 
00110                                 {
00111                                         pose.x(p.x);
00112                                         pose.y(p.y);
00113                                         pose.phi(p.phi);
00114                                 }
00115                                 return v;
00116                          }
00117                          
00118 
00119                          struct TOptions
00120                          {
00121                                 TOptions() :
00122                                         max_odometry_age        ( 1.0 ),
00123                                         max_localiz_age         ( 4.0 )
00124                                 {}
00125 
00126                                 double  max_odometry_age; //!< To consider data old, in seconds
00127                                 double  max_localiz_age; //!< To consider data old, in seconds
00128                          };
00129 
00130                          TOptions params; //!< parameters of the filter.
00131 
00132                 private:
00133                         mrpt::synch::CCriticalSection  m_cs;
00134 
00135                         TTimeStamp              m_last_loc_time;
00136                         TPose2D                 m_last_loc;   //!< Last pose as estimated by the localization/SLAM subsystem.
00137                         CMatrixDouble33 m_last_loc_cov;
00138 
00139                         TPose2D                 m_loc_odo_ref;  //!< The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subsequent odo readings)
00140 
00141                         TTimeStamp              m_last_odo_time;
00142                         TPose2D                 m_last_odo;
00143                         float                   m_robot_v;
00144                         float                   m_robot_w;
00145 
00146                         /** An auxiliary method to extrapolate the pose of a robot located at "p" with velocities (v,w) after a time delay "delta_time".
00147                           */
00148                         static void extrapolateRobotPose(
00149                                 const TPose2D &p,
00150                                 const float v,
00151                                 const float w,
00152                                 const double delta_time,
00153                                 TPose2D &new_p);
00154 
00155                 }; // end of class
00156 
00157         } // End of namespace
00158 } // End of namespace
00159 
00160 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011