MRPT  2.0.4
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 
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::TOptions_GaussianModel
TOptions_GaussianModel(double a1_, double a2_, double a3_, double a4_, double minStdXY_, double minStdPHI_)
Definition: CActionRobotMovement2D.h:94
mrpt::obs::CActionRobotMovement2D::m_fastDrawGauss_M
mrpt::poses::CPose2D m_fastDrawGauss_M
Definition: CActionRobotMovement2D.h:239
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::minStdPHI
double minStdPHI
Additional uncertainty: [degrees].
Definition: CActionRobotMovement2D.h:117
mrpt::obs::CActionRobotMovement2D::emScan2DMatching
@ emScan2DMatching
Definition: CActionRobotMovement2D.h:41
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::alfa3_trans_trans
float alfa3_trans_trans
Definition: CActionRobotMovement2D.h:135
mrpt::obs::CActionRobotMovement2D::drawSingleSample_modelGaussian
void drawSingleSample_modelGaussian(mrpt::poses::CPose2D &outSample) const
The sample generator for the model "computeFromOdometry_modelGaussian", internally called when the us...
Definition: CActionRobotMovement2D.cpp:541
mrpt::obs::CActionRobotMovement2D::prepareFastDrawSingleSample_modelThrun
void prepareFastDrawSingleSample_modelThrun() const
Internal use.
Definition: CActionRobotMovement2D.cpp:677
MRPT_ENUM_TYPE_END
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::modelSelection
TDrawSampleMotionModel modelSelection
The model to be used.
Definition: CActionRobotMovement2D.h:84
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel
Options for the Thrun's model, which generates a CPosePDFParticles object in poseChange using a Monte...
Definition: CActionRobotMovement2D.h:127
mrpt::obs::CActionRobotMovement2D::velocityAng
double velocityAng() const
Definition: CActionRobotMovement2D.h:71
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::gaussianModel
TOptions_GaussianModel gaussianModel
Definition: CActionRobotMovement2D.h:120
DEFINE_SERIALIZABLE
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Definition: CSerializable.h:152
mrpt::obs::CActionRobotMovement2D::hasVelocities
bool hasVelocities
If "true" means that "velocityLin" and "velocityAng" contain valid values.
Definition: CActionRobotMovement2D.h:65
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::a2
double a2
Ratio of uncertainty: [meter/degree].
Definition: CActionRobotMovement2D.h:109
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::a4
double a4
Ratio of uncertainty: [degree/degree].
Definition: CActionRobotMovement2D.h:113
mrpt::obs::CActionRobotMovement2D::drawSingleSample
void drawSingleSample(mrpt::poses::CPose2D &outSample) const
Using this method instead of "poseChange->drawSingleSample()" may be more efficient in most situation...
Definition: CActionRobotMovement2D.cpp:520
mrpt::math::TTwist2D::vx
double vx
Velocity components: X,Y (m/s)
Definition: TTwist2D.h:26
mrpt::obs::CActionRobotMovement2D
Represents a probabilistic 2D movement of the robot mobile base.
Definition: CActionRobotMovement2D.h:30
MRPT_ENUM_TYPE_BEGIN
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
CPose2D.h
mrpt::obs::CActionRobotMovement2D::drawSingleSample_modelThrun
void drawSingleSample_modelThrun(mrpt::poses::CPose2D &outSample) const
The sample generator for the model "computeFromOdometry_modelThrun", internally called when the user ...
Definition: CActionRobotMovement2D.cpp:552
mrpt::obs::CActionRobotMovement2D::computeFromOdometry_modelThrun
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's ...
Definition: CActionRobotMovement2D.cpp:455
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::obs::CActionRobotMovement2D::m_fastDrawGauss_Z
mrpt::math::CMatrixDouble33 m_fastDrawGauss_Z
Auxiliary matrix.
Definition: CActionRobotMovement2D.h:238
mrpt::obs::CActionRobotMovement2D::velocityLin
double velocityLin() const
Definition: CActionRobotMovement2D.h:70
mrpt::obs::CActionRobotMovement2D::estimationMethod
TEstimationMethod estimationMethod
This fields indicates the way in which this estimation was obtained.
Definition: CActionRobotMovement2D.h:52
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::thrunModel
TOptions_ThrunModel thrunModel
Definition: CActionRobotMovement2D.h:144
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::alfa4_trans_rot
float alfa4_trans_rot
Definition: CActionRobotMovement2D.h:136
mrpt::obs::CActionRobotMovement2D::emOdometry
@ emOdometry
Definition: CActionRobotMovement2D.h:40
mrpt::obs::CActionRobotMovement2D::getDescriptionAsText
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...
Definition: CActionRobotMovement2D.cpp:708
TTwist2D.h
mrpt::math::CMatrixFixed< double, 3, 3 >
mrpt::obs::CActionRobotMovement2D::rawOdometryIncrementReading
mrpt::poses::CPose2D rawOdometryIncrementReading
This is the raw odometry reading, and only is used when "estimationMethod" is "TEstimationMethod::emO...
Definition: CActionRobotMovement2D.h:50
CPosePDF.h
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::RAD2DEG
constexpr double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:56
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::minStdXY
double minStdXY
Additional uncertainty: [meters].
Definition: CActionRobotMovement2D.h:115
mrpt::obs::CActionRobotMovement2D::mmGaussian
@ mmGaussian
Definition: CActionRobotMovement2D.h:74
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::a3
double a3
Ratio of uncertainty: [degree/meter].
Definition: CActionRobotMovement2D.h:111
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TMotionModelOptions
TMotionModelOptions()=default
Default values loader.
TEnumType.h
mrpt::obs::CActionRobotMovement2D::CActionRobotMovement2D
CActionRobotMovement2D()
Definition: CActionRobotMovement2D.cpp:32
mrpt::obs::CActionRobotMovement2D::fastDrawSingleSample
void fastDrawSingleSample(mrpt::poses::CPose2D &outSample) const
Faster version than "drawSingleSample", but requires a previous call to "prepareFastDrawSingleSamples...
Definition: CActionRobotMovement2D.cpp:624
mrpt::obs::CActionRobotMovement2D::encoderRightTicks
int32_t encoderRightTicks
Definition: CActionRobotMovement2D.h:61
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDF::Ptr >
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::additional_std_XY
float additional_std_XY
An additional noise added to the thrun model (std.
Definition: CActionRobotMovement2D.h:140
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel
Options for the gaussian model, which generates a CPosePDFGaussian object in poseChange using a close...
Definition: CActionRobotMovement2D.h:91
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
mrpt::obs::CActionRobotMovement2D::mmThrun
@ mmThrun
Definition: CActionRobotMovement2D.h:75
mrpt::obs::CActionRobotMovement2D::computeFromOdometry_modelGaussian
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...
Definition: CActionRobotMovement2D.cpp:404
CAction.h
mrpt::obs::CActionRobotMovement2D::computeFromEncoders
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...
Definition: CActionRobotMovement2D.cpp:356
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::nParticlesCount
uint32_t nParticlesCount
The default number of particles to generate in a internal representation (anyway you can draw as many...
Definition: CActionRobotMovement2D.h:132
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::alfa2_rot_trans
float alfa2_rot_trans
Definition: CActionRobotMovement2D.h:134
mrpt::obs::CActionRobotMovement2D::velocityLocal
mrpt::math::TTwist2D velocityLocal
If "hasVelocities"=true, the robot velocity in local (robot frame, +X forward) coordinates.
Definition: CActionRobotMovement2D.h:68
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::a1
double a1
Ratio of uncertainty: [meter/meter].
Definition: CActionRobotMovement2D.h:107
deepcopy_poly_ptr.h
mrpt::obs::CActionRobotMovement2D::hasEncodersInfo
bool hasEncodersInfo
If "true" means that "encoderLeftTicks" and "encoderRightTicks" contain valid values.
Definition: CActionRobotMovement2D.h:56
mrpt::obs::CActionRobotMovement2D::poseChange
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDF::Ptr > poseChange
The 2D pose change probabilistic estimation.
Definition: CActionRobotMovement2D.h:47
mrpt::obs::CActionRobotMovement2D::prepareFastDrawSingleSample_modelGaussian
void prepareFastDrawSingleSample_modelGaussian() const
Internal use.
Definition: CActionRobotMovement2D.cpp:645
mrpt::obs::CActionRobotMovement2D::prepareFastDrawSingleSamples
void prepareFastDrawSingleSamples() const
Call this before calling a high number of times "fastDrawSingleSample", which is much faster than "dr...
Definition: CActionRobotMovement2D.cpp:608
mrpt::obs::CAction
Declares a class for storing a robot action.
Definition: CAction.h:24
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::alfa1_rot_rot
float alfa1_rot_rot
Definition: CActionRobotMovement2D.h:133
mrpt::obs::CActionRobotMovement2D::computeFromOdometry
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...
Definition: CActionRobotMovement2D.cpp:387
mrpt::obs::CActionRobotMovement2D::fastDrawSingleSample_modelThrun
void fastDrawSingleSample_modelThrun(mrpt::poses::CPose2D &outSample) const
Internal use.
Definition: CActionRobotMovement2D.cpp:702
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions
The parameter to be passed to "computeFromOdometry".
Definition: CActionRobotMovement2D.h:78
mrpt::obs::CActionRobotMovement2D::fastDrawSingleSample_modelGaussian
void fastDrawSingleSample_modelGaussian(mrpt::poses::CPose2D &outSample) const
Internal use.
Definition: CActionRobotMovement2D.cpp:681
mrpt::obs::CActionRobotMovement2D::motionModelConfiguration
TMotionModelOptions motionModelConfiguration
Definition: CActionRobotMovement2D.h:147
mrpt::math::TTwist2D::omega
double omega
Angular velocity (rad/s)
Definition: TTwist2D.h:28
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel::additional_std_phi
float additional_std_phi
Definition: CActionRobotMovement2D.h:141
mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::TOptions_GaussianModel
TOptions_GaussianModel()=default
mrpt::obs::CActionRobotMovement2D::TEstimationMethod
TEstimationMethod
A list of posible ways for estimating the content of a CActionRobotMovement2D object.
Definition: CActionRobotMovement2D.h:38
mrpt::obs::CActionRobotMovement2D::encoderLeftTicks
int32_t encoderLeftTicks
For odometry only: the ticks count for each wheel FROM the last reading (positive means FORWARD,...
Definition: CActionRobotMovement2D.h:61
mrpt::obs::CActionRobotMovement2D::TDrawSampleMotionModel
TDrawSampleMotionModel
Definition: CActionRobotMovement2D.h:72
mrpt::math::TTwist2D
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Definition: TTwist2D.h:19
MRPT_FILL_ENUM_MEMBER
MRPT_FILL_ENUM_MEMBER(mrpt::obs::CActionRobotMovement2D, emOdometry)



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sat Jun 27 14:00:59 UTC 2020