MRPT  1.9.9
CActionRobotMovement2D.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
12 #include <mrpt/math/TTwist2D.h>
13 #include <mrpt/obs/CAction.h>
14 #include <mrpt/poses/CPose2D.h>
15 #include <mrpt/poses/CPosePDF.h>
16 
17 namespace mrpt::obs
18 {
19 /** Represents a probabilistic 2D movement of the robot mobile base
20  *
21  * See docs:
22  * https://www.mrpt.org/tutorials/programming/odometry-and-motion-models/probabilistic_motion_models/
23  *
24  * \note [New in MRPT 1.5.0] Velocity is now encoded as mrpt::math::TTwist2D as
25  * a more general version of the old (linVel, angVel).
26  * \sa CAction
27  * \ingroup mrpt_obs_grp
28  */
30 {
32 
33  public:
34  /** A list of posible ways for estimating the content of a
35  * CActionRobotMovement2D object.
36  */
38  {
41  };
42 
44 
45  /** The 2D pose change probabilistic estimation. */
47  /** This is the raw odometry reading, and only is used when
48  * "estimationMethod" is "TEstimationMethod::emOdometry" */
50  /** This fields indicates the way in which this estimation was obtained. */
52 
53  /** If "true" means that "encoderLeftTicks" and "encoderRightTicks" contain
54  * valid values. */
55  bool hasEncodersInfo{false};
56  /** For odometry only: the ticks count for each wheel FROM the last reading
57  * (positive means FORWARD, for both wheels);
58  * \sa hasEncodersInfo
59  */
61 
62  /** If "true" means that "velocityLin" and "velocityAng" contain valid
63  * values. */
64  bool hasVelocities{false};
65  /** If "hasVelocities"=true, the robot velocity in local (robot frame, +X
66  * forward) coordinates. */
68 
69  double velocityLin() const { return velocityLocal.vx; }
70  double velocityAng() const { return velocityLocal.omega; }
72  {
75  };
76  /** The parameter to be passed to "computeFromOdometry". */
78  {
79  /** Default values loader. */
80  TMotionModelOptions() = default;
81 
82  /** The model to be used. */
84 
85  /** Options for the gaussian model, which generates a CPosePDFGaussian
86  * object in poseChange using a closed-form linear Gaussian model.
87  * See docs in:
88  * https://www.mrpt.org/tutorials/programming/odometry-and-motion-models/probabilistic_motion_models/
89  */
91  {
92  TOptions_GaussianModel() = default;
94  double a1_, double a2_, double a3_, double a4_,
95  double minStdXY_, double minStdPHI_)
96  : a1(a1_),
97  a2(a2_),
98  a3(a3_),
99  a4(a4_),
100  minStdXY(minStdXY_),
101  minStdPHI(minStdPHI_)
102  {
103  }
104 
105  /** Ratio of uncertainty: [meter/meter] */
106  double a1{0.01};
107  /** Ratio of uncertainty: [meter/degree] */
108  double a2{mrpt::RAD2DEG(0.001)};
109  /** Ratio of uncertainty: [degree/meter] */
110  double a3{mrpt::DEG2RAD(1.0)};
111  /** Ratio of uncertainty: [degree/degree] */
112  double a4{0.05};
113  /** Additional uncertainty: [meters] */
114  double minStdXY{0.01};
115  /** Additional uncertainty: [degrees] */
116  double minStdPHI{mrpt::DEG2RAD(0.2)};
117  };
118 
120 
121  /** Options for the Thrun's model, which generates a CPosePDFParticles
122  * object in poseChange using a MonteCarlo simulation.
123  * See docs in:
124  * https://www.mrpt.org/tutorials/programming/odometry-and-motion-models/probabilistic_motion_models/
125  */
127  {
128  /** The default number of particles to generate in a internal
129  * representation (anyway you can draw as many samples as you want
130  * through CActionRobotMovement2D::drawSingleSample) */
132  float alfa1_rot_rot{0.05f};
134  float alfa3_trans_trans{0.01f};
136 
137  /** An additional noise added to the thrun model (std. dev. in
138  * meters and radians). */
139  float additional_std_XY{0.001f};
141  };
142 
144  };
145 
147 
148  /** Computes the PDF of the pose increment from an odometry reading and
149  * according to the given motion model (speed and encoder ticks information
150  * is not modified).
151  * According to the parameters in the passed struct, it will be called one
152  * the private sampling functions (see "see also" next).
153  * \sa computeFromOdometry_modelGaussian, computeFromOdometry_modelThrun
154  */
155  void computeFromOdometry(
156  const mrpt::poses::CPose2D& odometryIncrement,
157  const TMotionModelOptions& options);
158 
159  /** If "hasEncodersInfo"=true, this method updates the pose estimation
160  * according to the ticks from both encoders and the passed parameters,
161  * which is passed internally to the method "computeFromOdometry" with the
162  * last used PDF options (or the defualt ones if not explicitly called by
163  * the user).
164  *
165  * \param K_left The meters / tick ratio for the left encoder.
166  * \param K_right The meters / tick ratio for the right encoder.
167  * \param D The distance between both wheels, in meters.
168  */
169  void computeFromEncoders(double K_left, double K_right, double D);
170 
171  /** Using this method instead of "poseChange->drawSingleSample()" may be
172  * more efficient in most situations.
173  * \sa CPosePDF::drawSingleSample
174  */
175  void drawSingleSample(mrpt::poses::CPose2D& outSample) const;
176 
177  /** Call this before calling a high number of times "fastDrawSingleSample",
178  * which is much faster than "drawSingleSample"
179  */
180  void prepareFastDrawSingleSamples() const;
181 
182  /** Faster version than "drawSingleSample", but requires a previous call to
183  * "prepareFastDrawSingleSamples"
184  */
185  void fastDrawSingleSample(mrpt::poses::CPose2D& outSample) const;
186 
187  protected:
188  /** Computes the PDF of the pose increment from an odometry reading, using a
189  * Gaussian approximation as the motion model.
190  * \sa computeFromOdometry
191  */
193  const mrpt::poses::CPose2D& odometryIncrement,
194  const TMotionModelOptions& o);
195 
196  /** Computes the PDF of the pose increment from an odometry reading, using
197  * the motion model from Thrun's book.
198  * This model is discussed in "Probabilistic Robotics", Thrun, Burgard, and
199  * Fox, 2006, pp.136.
200  * \sa computeFromOdometry
201  */
203  const mrpt::poses::CPose2D& odometryIncrement,
204  const TMotionModelOptions& o);
205 
206  /** The sample generator for the model "computeFromOdometry_modelGaussian",
207  * internally called when the user invokes "drawSingleSample".
208  */
210 
211  /** The sample generator for the model "computeFromOdometry_modelThrun",
212  * internally called when the user invokes "drawSingleSample".
213  */
214  void drawSingleSample_modelThrun(mrpt::poses::CPose2D& outSample) const;
215 
216  /** Internal use
217  */
219 
220  /** Internal use
221  */
223 
224  /** Internal use
225  */
227  mrpt::poses::CPose2D& outSample) const;
228 
229  /** Internal use
230  */
232 
233  /** Auxiliary matrix
234  */
237 
238 }; // End of class def.
239 
240 } // namespace mrpt::obs
Options for the gaussian model, which generates a CPosePDFGaussian object in poseChange using a close...
double RAD2DEG(const double x)
Radians to degrees.
TEstimationMethod estimationMethod
This fields indicates the way in which this estimation was obtained.
Wrapper to a std::shared_ptr<>, adding deep-copy semantics to copy ctor and copy operator, suitable for polymorphic classes with a clone() method.
void drawSingleSample(mrpt::poses::CPose2D &outSample) const
Using this method instead of "poseChange->drawSingleSample()" may be more efficient in most situation...
void drawSingleSample_modelThrun(mrpt::poses::CPose2D &outSample) const
The sample generator for the model "computeFromOdometry_modelThrun", internally called when the user ...
double DEG2RAD(const double x)
Degrees to radians.
void computeFromOdometry(const mrpt::poses::CPose2D &odometryIncrement, const TMotionModelOptions &options)
Computes the PDF of the pose increment from an odometry reading and according to the given motion mod...
The parameter to be passed to "computeFromOdometry".
void computeFromOdometry_modelGaussian(const mrpt::poses::CPose2D &odometryIncrement, const TMotionModelOptions &o)
Computes the PDF of the pose increment from an odometry reading, using a Gaussian approximation as th...
TOptions_GaussianModel(double a1_, double a2_, double a3_, double a4_, double minStdXY_, double minStdPHI_)
void drawSingleSample_modelGaussian(mrpt::poses::CPose2D &outSample) const
The sample generator for the model "computeFromOdometry_modelGaussian", internally called when the us...
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Definition: TTwist2D.h:19
Represents a probabilistic 2D movement of the robot mobile base.
mrpt::math::CMatrixDouble33 m_fastDrawGauss_Z
Auxiliary matrix.
void fastDrawSingleSample(mrpt::poses::CPose2D &outSample) const
Faster version than "drawSingleSample", but requires a previous call to "prepareFastDrawSingleSamples...
double vx
Velocity components: X,Y (m/s)
Definition: TTwist2D.h:26
float additional_std_XY
An additional noise added to the thrun model (std.
mrpt::math::TTwist2D velocityLocal
If "hasVelocities"=true, the robot velocity in local (robot frame, +X forward) coordinates.
void computeFromOdometry_modelThrun(const mrpt::poses::CPose2D &odometryIncrement, const TMotionModelOptions &o)
Computes the PDF of the pose increment from an odometry reading, using the motion model from Thrun&#39;s ...
void fastDrawSingleSample_modelThrun(mrpt::poses::CPose2D &outSample) const
Internal use.
This namespace contains representation of robot actions and observations.
int32_t encoderLeftTicks
For odometry only: the ticks count for each wheel FROM the last reading (positive means FORWARD...
void computeFromEncoders(double K_left, double K_right, double D)
If "hasEncodersInfo"=true, this method updates the pose estimation according to the ticks from both e...
TEstimationMethod
A list of posible ways for estimating the content of a CActionRobotMovement2D object.
bool hasEncodersInfo
If "true" means that "encoderLeftTicks" and "encoderRightTicks" contain valid values.
TDrawSampleMotionModel modelSelection
The model to be used.
Options for the Thrun&#39;s model, which generates a CPosePDFParticles object in poseChange using a Monte...
void fastDrawSingleSample_modelGaussian(mrpt::poses::CPose2D &outSample) const
Internal use.
Declares a class for storing a robot action.
Definition: CAction.h:24
void prepareFastDrawSingleSamples() const
Call this before calling a high number of times "fastDrawSingleSample", which is much faster than "dr...
__int32 int32_t
Definition: rptypes.h:49
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
void prepareFastDrawSingleSample_modelGaussian() const
Internal use.
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDF::Ptr > poseChange
The 2D pose change probabilistic estimation.
void prepareFastDrawSingleSample_modelThrun() const
Internal use.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
uint32_t nParticlesCount
The default number of particles to generate in a internal representation (anyway you can draw as many...
unsigned __int32 uint32_t
Definition: rptypes.h:50
TMotionModelOptions()=default
Default values loader.
double omega
Angular velocity (rad/s)
Definition: TTwist2D.h:28
mrpt::poses::CPose2D rawOdometryIncrementReading
This is the raw odometry reading, and only is used when "estimationMethod" is "TEstimationMethod::emO...
bool hasVelocities
If "true" means that "velocityLin" and "velocityAng" contain valid values.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce1a28c9f Fri Aug 23 08:02:09 2019 +0200 at vie ago 23 08:10:11 CEST 2019