MRPT  2.0.1
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-2020, 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>
17 
18 namespace mrpt::obs
19 {
20 /** Represents a probabilistic 2D movement of the robot mobile base
21  *
22  * See docs:
23  * https://www.mrpt.org/tutorials/programming/odometry-and-motion-models/probabilistic_motion_models/
24  *
25  * \note [New in MRPT 1.5.0] Velocity is now encoded as mrpt::math::TTwist2D as
26  * a more general version of the old (linVel, angVel).
27  * \sa CAction
28  * \ingroup mrpt_obs_grp
29  */
31 {
33 
34  public:
35  /** A list of posible ways for estimating the content of a
36  * CActionRobotMovement2D object.
37  */
39  {
42  };
43 
45 
46  /** The 2D pose change probabilistic estimation. */
48  /** This is the raw odometry reading, and only is used when
49  * "estimationMethod" is "TEstimationMethod::emOdometry" */
51  /** This fields indicates the way in which this estimation was obtained. */
53 
54  /** If "true" means that "encoderLeftTicks" and "encoderRightTicks" contain
55  * valid values. */
56  bool hasEncodersInfo{false};
57  /** For odometry only: the ticks count for each wheel FROM the last reading
58  * (positive means FORWARD, for both wheels);
59  * \sa hasEncodersInfo
60  */
62 
63  /** If "true" means that "velocityLin" and "velocityAng" contain valid
64  * values. */
65  bool hasVelocities{false};
66  /** If "hasVelocities"=true, the robot velocity in local (robot frame, +X
67  * forward) coordinates. */
69 
70  double velocityLin() const { return velocityLocal.vx; }
71  double velocityAng() const { return velocityLocal.omega; }
73  {
76  };
77  /** The parameter to be passed to "computeFromOdometry". */
79  {
80  /** Default values loader. */
81  TMotionModelOptions() = default;
82 
83  /** The model to be used. */
85 
86  /** Options for the gaussian model, which generates a CPosePDFGaussian
87  * object in poseChange using a closed-form linear Gaussian model.
88  * See docs in:
89  * https://www.mrpt.org/tutorials/programming/odometry-and-motion-models/probabilistic_motion_models/
90  */
92  {
93  TOptions_GaussianModel() = default;
95  double a1_, double a2_, double a3_, double a4_,
96  double minStdXY_, double minStdPHI_)
97  : a1(a1_),
98  a2(a2_),
99  a3(a3_),
100  a4(a4_),
101  minStdXY(minStdXY_),
102  minStdPHI(minStdPHI_)
103  {
104  }
105 
106  /** Ratio of uncertainty: [meter/meter] */
107  double a1{0.01};
108  /** Ratio of uncertainty: [meter/degree] */
109  double a2{mrpt::RAD2DEG(0.001)};
110  /** Ratio of uncertainty: [degree/meter] */
111  double a3{mrpt::DEG2RAD(1.0)};
112  /** Ratio of uncertainty: [degree/degree] */
113  double a4{0.05};
114  /** Additional uncertainty: [meters] */
115  double minStdXY{0.01};
116  /** Additional uncertainty: [degrees] */
117  double minStdPHI{mrpt::DEG2RAD(0.2)};
118  };
119 
121 
122  /** Options for the Thrun's model, which generates a CPosePDFParticles
123  * object in poseChange using a MonteCarlo simulation.
124  * See docs in:
125  * https://www.mrpt.org/tutorials/programming/odometry-and-motion-models/probabilistic_motion_models/
126  */
128  {
129  /** The default number of particles to generate in a internal
130  * representation (anyway you can draw as many samples as you want
131  * through CActionRobotMovement2D::drawSingleSample) */
132  uint32_t nParticlesCount{300};
133  float alfa1_rot_rot{0.05f};
135  float alfa3_trans_trans{0.01f};
137 
138  /** An additional noise added to the thrun model (std. dev. in
139  * meters and radians). */
140  float additional_std_XY{0.001f};
142  };
143 
145  };
146 
148 
149  /** Computes the PDF of the pose increment from an odometry reading and
150  * according to the given motion model (speed and encoder ticks information
151  * is not modified).
152  * According to the parameters in the passed struct, it will be called one
153  * the private sampling functions (see "see also" next).
154  * \sa computeFromOdometry_modelGaussian, computeFromOdometry_modelThrun
155  */
156  void computeFromOdometry(
157  const mrpt::poses::CPose2D& odometryIncrement,
158  const TMotionModelOptions& options);
159 
160  /** If "hasEncodersInfo"=true, this method updates the pose estimation
161  * according to the ticks from both encoders and the passed parameters,
162  * which is passed internally to the method "computeFromOdometry" with the
163  * last used PDF options (or the defualt ones if not explicitly called by
164  * 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(double K_left, double K_right, double D);
171 
172  /** Using this method instead of "poseChange->drawSingleSample()" may be
173  * more efficient in most situations.
174  * \sa CPosePDF::drawSingleSample
175  */
176  void drawSingleSample(mrpt::poses::CPose2D& outSample) const;
177 
178  /** Call this before calling a high number of times "fastDrawSingleSample",
179  * which is much faster than "drawSingleSample"
180  */
181  void prepareFastDrawSingleSamples() const;
182 
183  /** Faster version than "drawSingleSample", but requires a previous call to
184  * "prepareFastDrawSingleSamples"
185  */
186  void fastDrawSingleSample(mrpt::poses::CPose2D& outSample) const;
187 
188  virtual void getDescriptionAsText(std::ostream& o) const override;
189 
190  protected:
191  /** Computes the PDF of the pose increment from an odometry reading, using a
192  * Gaussian approximation as the motion model.
193  * \sa computeFromOdometry
194  */
196  const mrpt::poses::CPose2D& odometryIncrement,
197  const TMotionModelOptions& o);
198 
199  /** Computes the PDF of the pose increment from an odometry reading, using
200  * the motion model from Thrun's book.
201  * This model is discussed in "Probabilistic Robotics", Thrun, Burgard, and
202  * Fox, 2006, pp.136.
203  * \sa computeFromOdometry
204  */
206  const mrpt::poses::CPose2D& odometryIncrement,
207  const TMotionModelOptions& o);
208 
209  /** The sample generator for the model "computeFromOdometry_modelGaussian",
210  * internally called when the user invokes "drawSingleSample".
211  */
213 
214  /** The sample generator for the model "computeFromOdometry_modelThrun",
215  * internally called when the user invokes "drawSingleSample".
216  */
217  void drawSingleSample_modelThrun(mrpt::poses::CPose2D& outSample) const;
218 
219  /** Internal use
220  */
222 
223  /** Internal use
224  */
226 
227  /** Internal use
228  */
230  mrpt::poses::CPose2D& outSample) const;
231 
232  /** Internal use
233  */
235 
236  /** Auxiliary matrix
237  */
240 
241 }; // End of class def.
242 
243 } // namespace mrpt::obs
244 
Options for the gaussian model, which generates a CPosePDFGaussian object in poseChange using a close...
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...
MRPT_FILL_ENUM_MEMBER(mrpt::obs::CActionRobotMovement2D, emOdometry)
void drawSingleSample_modelThrun(mrpt::poses::CPose2D &outSample) const
The sample generator for the model "computeFromOdometry_modelThrun", internally called when the user ...
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...
constexpr double DEG2RAD(const double x)
Degrees to radians.
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.
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
virtual void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the action contents and dump it to the output str...
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...
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...
constexpr double RAD2DEG(const double x)
Radians to degrees.
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
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 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020