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