Main MRPT website > C++ reference
MRPT logo
CActionRobotMovement2D.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CActionRobotMovement2D_H
29 #define CActionRobotMovement2D_H
30 
31 #include <mrpt/slam/CAction.h>
32 #include <mrpt/poses/CPose2D.h>
33 //#include <mrpt/poses/CPosePDFGaussian.h>
34 //#include <mrpt/poses/CPosePDFParticles.h>
35 #include <mrpt/poses/CPosePDF.h>
36 
37 namespace mrpt
38 {
39  namespace slam
40  {
41  using namespace mrpt::math;
42  using namespace mrpt::poses;
43 
44  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CActionRobotMovement2D, CAction, OBS_IMPEXP )
45 
46  /** Represents a probabilistic 2D movement of the robot mobile base
47  *
48  * See the tutorial on <a href="http://www.mrpt.org/Probabilistic_Motion_Models" >probabilistic motion models</a>.
49  *
50  * \sa CAction
51  * \ingroup mrpt_obs_grp
52  */
53  class OBS_IMPEXP CActionRobotMovement2D : public CAction
54  {
55  // This must be added to any CSerializable derived class:
56  DEFINE_SERIALIZABLE( CActionRobotMovement2D )
57 
58  public:
59  /** A list of posible ways for estimating the content of a CActionRobotMovement2D object.
60  */
61  enum TEstimationMethod
62  {
63  emOdometry = 0,
64  emScan2DMatching
65  };
66 
67  /** Constructor
68  */
69  CActionRobotMovement2D();
70 
71  /** Copy constructor
72  */
73  CActionRobotMovement2D(const CActionRobotMovement2D &o);
74 
75  /** Copy operator
76  */
77  CActionRobotMovement2D & operator =(const CActionRobotMovement2D &o);
78 
79  /** Destructor
80  */
81  ~CActionRobotMovement2D();
82 
83  /** The 2D pose change probabilistic estimation.
84  */
85  CPosePDFPtr poseChange;
86 
87  /** This is the raw odometry reading, and only is used when "estimationMethod" is "TEstimationMethod::emOdometry"
88  */
89  CPose2D rawOdometryIncrementReading;
90 
91  /** This fields indicates the way this estimation was obtained.
92  */
93  TEstimationMethod estimationMethod;
94 
95  /** If "true" means that "encoderLeftTicks" and "encoderRightTicks" contain valid values.
96  */
97  bool hasEncodersInfo;
98 
99  /** For odometry only: the ticks count for each wheel FROM the last reading (positive means FORWARD, for both wheels);
100  * \sa hasEncodersInfo
101  */
102  int32_t encoderLeftTicks,encoderRightTicks;
103 
104  /** If "true" means that "velocityLin" and "velocityAng" contain valid values.
105  */
106  bool hasVelocities;
107 
108  /** The velocity of the robot, linear in meters/sec and angular in rad/sec.
109  */
110  float velocityLin, velocityAng;
111 
112  enum TDrawSampleMotionModel
113  {
114  mmGaussian = 0,
115  mmThrun
116  };
117  /** The parameter to be passed to "computeFromOdometry".
118  */
119  struct OBS_IMPEXP TMotionModelOptions
120  {
121  /** Default values loader.
122  */
123  TMotionModelOptions();
124 
125  /** The model to be used.
126  */
127  TDrawSampleMotionModel modelSelection;
128 
129  /** Options for the gaussian model, which generates a CPosePDFGaussian object in poseChange
130  */
131  struct OBS_IMPEXP TOptions_GaussianModel
132  {
133  float a1,a2,a3,a4,minStdXY,minStdPHI;
134  } gausianModel;
135 
136  /** Options for the Thrun's model, which generates a CPosePDFParticles object in poseChange
137  */
138  struct OBS_IMPEXP TOptions_ThrunModel
139  {
140  /** The default number of particles to generate in a internal representation (anyway you can draw as many samples as you want through CActionRobotMovement2D::drawSingleSample)
141  */
142  uint32_t nParticlesCount;
143 
144  float alfa1_rot_rot;
145  float alfa2_rot_trans;
146  float alfa3_trans_trans;
147  float alfa4_trans_rot;
148 
149  /** An additional noise added to the thrun model (std. dev. in meters and radians).
150  */
151  float additional_std_XY, additional_std_phi;
152  } thrunModel;
153 
154  } motionModelConfiguration;
155 
156  /** 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).
157  * According to the parameters in the passed struct, it will be called one the private sampling functions (see "see also" next).
158  * \sa computeFromOdometry_modelGaussian, computeFromOdometry_modelThrun
159  */
160  void computeFromOdometry(
161  const CPose2D &odometryIncrement,
162  const TMotionModelOptions &options);
163 
164  /** 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).
165  *
166  * \param K_left The meters / tick ratio for the left encoder.
167  * \param K_right The meters / tick ratio for the right encoder.
168  * \param D The distance between both wheels, in meters.
169  */
170  void computeFromEncoders(
171  double K_left,
172  double K_right,
173  double D );
174 
175  /** Using this method instead of "poseChange->drawSingleSample()" may be more efficient in most situations.
176  * \sa CPosePDF::drawSingleSample
177  */
178  void drawSingleSample( CPose2D &outSample ) const;
179 
180  /** Call this before calling a high number of times "fastDrawSingleSample", which is much faster than "drawSingleSample"
181  */
182  void prepareFastDrawSingleSamples() const;
183 
184  /** Faster version than "drawSingleSample", but requires a previous call to "prepareFastDrawSingleSamples"
185  */
186  void fastDrawSingleSample( CPose2D &outSample ) const;
187 
188  protected:
189  /** Computes the PDF of the pose increment from an odometry reading, using a Gaussian approximation as the motion model.
190  * \sa computeFromOdometry
191  */
192  void computeFromOdometry_modelGaussian(
193  const CPose2D &odometryIncrement,
194  const TMotionModelOptions &o
195  );
196 
197  /** Computes the PDF of the pose increment from an odometry reading, using the motion model from Thrun's book.
198  * This model is discussed in "Probabilistic Robotics", Thrun, Burgard, and Fox, 2006, pp.136.
199  * \sa computeFromOdometry
200  */
201  void computeFromOdometry_modelThrun(
202  const CPose2D &odometryIncrement,
203  const TMotionModelOptions &o
204  );
205 
206  /** The sample generator for the model "computeFromOdometry_modelGaussian", internally called when the user invokes "drawSingleSample".
207  */
208  void drawSingleSample_modelGaussian( CPose2D &outSample ) const;
209 
210  /** The sample generator for the model "computeFromOdometry_modelThrun", internally called when the user invokes "drawSingleSample".
211  */
212  void drawSingleSample_modelThrun( CPose2D &outSample ) const;
213 
214  /** Internal use
215  */
216  void prepareFastDrawSingleSample_modelGaussian() const;
217 
218  /** Internal use
219  */
220  void prepareFastDrawSingleSample_modelThrun() const;
221 
222  /** Internal use
223  */
224  void fastDrawSingleSample_modelGaussian( CPose2D &outSample ) const;
225 
226  /** Internal use
227  */
228  void fastDrawSingleSample_modelThrun( CPose2D &outSample ) const;
229 
230  /** Auxiliary matrix
231  */
232  mutable CMatrixDouble33 m_fastDrawGauss_Z;
233  mutable CPose2D m_fastDrawGauss_M;
234 
235 
236  }; // End of class def.
237 
238 
239  } // End of namespace
240 } // End of namespace
241 
242 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013