Main MRPT website > C++ reference
MRPT logo

CActionRobotMovement2D.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 CActionRobotMovement2D_H
00029 #define CActionRobotMovement2D_H
00030 
00031 #include <mrpt/slam/CAction.h>
00032 #include <mrpt/poses/CPose2D.h>
00033 //#include <mrpt/poses/CPosePDFGaussian.h>
00034 //#include <mrpt/poses/CPosePDFParticles.h>
00035 #include <mrpt/poses/CPosePDF.h>
00036 
00037 namespace mrpt
00038 {
00039         namespace slam
00040         {
00041                 using namespace mrpt::math;
00042                 using namespace mrpt::poses;
00043 
00044                 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CActionRobotMovement2D, CAction, OBS_IMPEXP )
00045 
00046                 /** Represents a probabilistic 2D movement of the robot mobile base
00047                  *
00048                  *  See the tutorial on <a href="http://www.mrpt.org/Probabilistic_Motion_Models">probabilistic motion models</a>.
00049                  *
00050                  * \sa CAction
00051                  */
00052                 class OBS_IMPEXP  CActionRobotMovement2D : public CAction
00053                 {
00054                         // This must be added to any CSerializable derived class:
00055                         DEFINE_SERIALIZABLE( CActionRobotMovement2D )
00056 
00057                 public:
00058                         /** A list of posible ways for estimating the content of a CActionRobotMovement2D object.
00059                                 */
00060                         enum TEstimationMethod
00061                         {
00062                                 emOdometry = 0,
00063                                 emScan2DMatching
00064                         };
00065 
00066                         /** Constructor
00067                           */
00068                         CActionRobotMovement2D();
00069 
00070                         /** Copy constructor
00071                           */
00072                         CActionRobotMovement2D(const CActionRobotMovement2D &o);
00073 
00074                         /** Copy operator
00075                           */
00076                         CActionRobotMovement2D & operator =(const CActionRobotMovement2D &o);
00077 
00078                         /** Destructor
00079                           */
00080                         ~CActionRobotMovement2D();
00081 
00082                         /** The 2D pose change probabilistic estimation.
00083                           */
00084                         CPosePDFPtr                             poseChange;
00085 
00086                         /** This is the raw odometry reading, and only is used when "estimationMethod" is "TEstimationMethod::emOdometry"
00087                           */
00088                         CPose2D                                 rawOdometryIncrementReading;
00089 
00090                         /** This fields indicates the way this estimation was obtained.
00091                           */
00092                         TEstimationMethod               estimationMethod;
00093 
00094                         /** If "true" means that "encoderLeftTicks" and "encoderRightTicks" contain valid values.
00095                           */
00096                         bool                                    hasEncodersInfo;
00097 
00098                         /** For odometry only: the ticks count for each wheel FROM the last reading (positive means FORWARD, for both wheels);
00099                           * \sa hasEncodersInfo
00100                           */
00101                         int32_t                                 encoderLeftTicks,encoderRightTicks;
00102 
00103                         /** If "true" means that "velocityLin" and "velocityAng" contain valid values.
00104                           */
00105                         bool                                    hasVelocities;
00106 
00107                         /** The velocity of the robot, linear in meters/sec and angular in rad/sec.
00108                           */
00109                         float                                   velocityLin, velocityAng;
00110 
00111                         enum TDrawSampleMotionModel
00112                         {
00113                                 mmGaussian = 0,
00114                                 mmThrun
00115                         };
00116                         /** The parameter to be passed to "computeFromOdometry".
00117                           */
00118                         struct OBS_IMPEXP TMotionModelOptions
00119                         {
00120                                 /** Default values loader.
00121                                   */
00122                                 TMotionModelOptions();
00123 
00124                                 /** The model to be used.
00125                                   */
00126                                 TDrawSampleMotionModel  modelSelection;
00127 
00128                                 /** Options for the gaussian model, which generates a CPosePDFGaussian object in poseChange
00129                                   */
00130                                 struct OBS_IMPEXP TOptions_GaussianModel
00131                                 {
00132                                         float           a1,a2,a3,a4,minStdXY,minStdPHI;
00133                                 } gausianModel;
00134 
00135                                 /** Options for the Thrun's model, which generates a CPosePDFParticles object in poseChange
00136                                   */
00137                                 struct OBS_IMPEXP  TOptions_ThrunModel
00138                                 {
00139                                         /** The default number of particles to generate in a internal representation (anyway you can draw as many samples as you want through CActionRobotMovement2D::drawSingleSample)
00140                                           */
00141                                         uint32_t                nParticlesCount;
00142 
00143                                         float                   alfa1_rot_rot;
00144                                         float                   alfa2_rot_trans;
00145                                         float                   alfa3_trans_trans;
00146                                         float                   alfa4_trans_rot;
00147 
00148                                         /** An additional noise added to the thrun model (std. dev. in meters and radians).
00149                                           */
00150                                         float                   additional_std_XY, additional_std_phi;
00151                                 } thrunModel;
00152 
00153                         } motionModelConfiguration;
00154 
00155                         /** Computes the PDF of the pose increment from an odometry reading and according to the given motion model (speed and encoder ticks information is not modified).
00156                           * According to the parameters in the passed struct, it will be called one the private sampling functions (see "see also" next).
00157                           * \sa computeFromOdometry_modelGaussian, computeFromOdometry_modelThrun
00158                           */
00159                         void  computeFromOdometry(
00160                                 const CPose2D                           &odometryIncrement,
00161                                 const TMotionModelOptions       &options);
00162 
00163                         /** If "hasEncodersInfo"=true, this method updates the pose estimation according to the ticks from both encoders and the passed parameters, which is passed internally to the method "computeFromOdometry" with the last used PDF options (or the defualt ones if not explicitly called by the user).
00164                           *
00165                           * \param K_left The meters / tick ratio for the left encoder.
00166                           * \param K_right The meters / tick ratio for the right encoder.
00167                           * \param D The distance between both wheels, in meters.
00168                           */
00169                         void  computeFromEncoders(
00170                                 double  K_left,
00171                                 double  K_right,
00172                                 double  D );
00173 
00174                         /** Using this method instead of "poseChange->drawSingleSample()" may be more efficient in most situations.
00175                           * \sa CPosePDF::drawSingleSample
00176                           */
00177                         void  drawSingleSample( CPose2D &outSample ) const;
00178 
00179                         /** Call this before calling a high number of times "fastDrawSingleSample", which is much faster than "drawSingleSample"
00180                           */
00181                         void  prepareFastDrawSingleSamples() const;
00182 
00183                         /** Faster version than "drawSingleSample", but requires a previous call to "prepareFastDrawSingleSamples"
00184                           */
00185                         void  fastDrawSingleSample( CPose2D &outSample ) const;
00186 
00187                 protected:
00188                         /** Computes the PDF of the pose increment from an odometry reading, using a Gaussian approximation as the motion model.
00189                          * \sa computeFromOdometry
00190                          */
00191                         void  computeFromOdometry_modelGaussian(
00192                                 const CPose2D                           &odometryIncrement,
00193                                 const TMotionModelOptions       &o
00194                                 );
00195 
00196                         /** Computes the PDF of the pose increment from an odometry reading, using the motion model from Thrun's book.
00197                          * This model is discussed in "Probabilistic Robotics", Thrun, Burgard, and Fox, 2006, pp.136.
00198                          * \sa computeFromOdometry
00199                          */
00200                         void  computeFromOdometry_modelThrun(
00201                                 const CPose2D                           &odometryIncrement,
00202                                 const TMotionModelOptions       &o
00203                                 );
00204 
00205                         /** The sample generator for the model "computeFromOdometry_modelGaussian", internally called when the user invokes "drawSingleSample".
00206                           */
00207                         void  drawSingleSample_modelGaussian( CPose2D &outSample ) const;
00208 
00209                         /** The sample generator for the model "computeFromOdometry_modelThrun", internally called when the user invokes "drawSingleSample".
00210                           */
00211                         void  drawSingleSample_modelThrun( CPose2D &outSample ) const;
00212 
00213                         /** Internal use
00214                           */
00215                         void  prepareFastDrawSingleSample_modelGaussian() const;
00216 
00217                         /** Internal use
00218                           */
00219                         void  prepareFastDrawSingleSample_modelThrun() const;
00220 
00221                         /** Internal use
00222                           */
00223                         void  fastDrawSingleSample_modelGaussian( CPose2D &outSample ) const;
00224 
00225                         /** Internal use
00226                           */
00227                         void  fastDrawSingleSample_modelThrun( CPose2D &outSample ) const;
00228 
00229                         /** Auxiliary matrix
00230                           */
00231                         mutable CMatrixDouble33 m_fastDrawGauss_Z;
00232                         mutable CPose2D                 m_fastDrawGauss_M;
00233 
00234 
00235                 }; // End of class def.
00236 
00237 
00238         } // End of namespace
00239 } // End of namespace
00240 
00241 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011