00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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
00052 class MRPTDLLIMPEXP CActionRobotMovement2D : public CAction
00053 {
00054
00055 DEFINE_SERIALIZABLE( CActionRobotMovement2D )
00056
00057 public:
00060 enum TEstimationMethod
00061 {
00062 emOdometry = 0,
00063 emScan2DMatching
00064 };
00065
00068 CActionRobotMovement2D();
00069
00072 CActionRobotMovement2D(const CActionRobotMovement2D &o);
00073
00076 CActionRobotMovement2D & operator =(const CActionRobotMovement2D &o);
00077
00080 ~CActionRobotMovement2D();
00081
00084 CPosePDFPtr poseChange;
00085
00088 CPose2D rawOdometryIncrementReading;
00089
00092 TEstimationMethod estimationMethod;
00093
00096 bool hasEncodersInfo;
00097
00101 int32_t encoderLeftTicks,encoderRightTicks;
00102
00105 bool hasVelocities;
00106
00109 float velocityLin, velocityAng;
00110
00111 enum TDrawSampleMotionModel
00112 {
00113 mmGaussian = 0,
00114 mmThrun
00115 };
00118 struct MRPTDLLIMPEXP TMotionModelOptions
00119 {
00122 TMotionModelOptions();
00123
00126 TDrawSampleMotionModel modelSelection;
00127
00130 struct MRPTDLLIMPEXP TOptions_GaussianModel
00131 {
00132 float a1,a2,a3,a4,minStdXY,minStdPHI;
00133 } gausianModel;
00134
00137 struct MRPTDLLIMPEXP TOptions_ThrunModel
00138 {
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
00150 float additional_std_XY, additional_std_phi;
00151 } thrunModel;
00152
00153 } motionModelConfiguration;
00154
00159 void computeFromOdometry(
00160 const CPose2D &odometryIncrement,
00161 const TMotionModelOptions &options);
00162
00169 void computeFromEncoders(
00170 double K_left,
00171 double K_right,
00172 double D );
00173
00177 void drawSingleSample( CPose2D &outSample ) const;
00178
00181 void prepareFastDrawSingleSamples() const;
00182
00185 void fastDrawSingleSample( CPose2D &outSample ) const;
00186
00187 protected:
00191 void computeFromOdometry_modelGaussian(
00192 const CPose2D &odometryIncrement,
00193 const TMotionModelOptions &o
00194 );
00195
00200 void computeFromOdometry_modelThrun(
00201 const CPose2D &odometryIncrement,
00202 const TMotionModelOptions &o
00203 );
00204
00207 void drawSingleSample_modelGaussian( CPose2D &outSample ) const;
00208
00211 void drawSingleSample_modelThrun( CPose2D &outSample ) const;
00212
00215 void prepareFastDrawSingleSample_modelGaussian() const;
00216
00219 void prepareFastDrawSingleSample_modelThrun() const;
00220
00223 void fastDrawSingleSample_modelGaussian( CPose2D &outSample ) const;
00224
00227 void fastDrawSingleSample_modelThrun( CPose2D &outSample ) const;
00228
00231 mutable CMatrixD m_fastDrawGauss_Z;
00232 mutable CPose2D m_fastDrawGauss_M;
00233
00234
00235 };
00236
00237
00238 }
00239 }
00240
00241 #endif