00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 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( CActionRobotMovement2D, CAction ) 00045 00046 /** Represents a probabilistic 2D movement of the robot mobile base 00047 * 00048 * See the tutorial on <a href="http://babel.isa.uma.es/mrpt/index.php/Probabilistic_Motion_Models">probabilistic motion models</a>. 00049 * 00050 * \sa CAction 00051 */ 00052 class MRPTDLLIMPEXP 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 MRPTDLLIMPEXP 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 MRPTDLLIMPEXP 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 MRPTDLLIMPEXP 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.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:27:43 EDT 2009 |