Main MRPT website > C++ reference for MRPT 1.9.9
CActionRobotMovement3D.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #ifndef CActionRobotMovement3D_H
10 #define CActionRobotMovement3D_H
11 
12 #include <mrpt/obs/CAction.h>
14 
15 namespace mrpt
16 {
17 namespace obs
18 {
19 /** Represents a probabilistic 3D (6D) movement.
20 * Currently this can be determined from visual odometry for full 6D, or from
21 * wheel encoders for 2D movements only.
22 * Here implemented the motion model from the next article: A. L. Ballardini, A.
23 * Furlan, A. Galbiati, M. Matteucci, F. Sacchi, D. G. Sorrenti An effective 6DoF
24 * motion model for 3D-6DoF Monte Carlo Localization 4th Workshop on Planning,
25 * Perception and Navigation for Intelligent Vehicles, IROS, 2012
26 * \ingroup mrpt_obs_grp
27 * \sa CAction
28 */
30 {
32 
33  public:
34  /** A list of posible ways for estimating the content of a
35  * CActionRobotMovement3D object.
36  */
38  {
41  };
42 
44 
45  /** The 3D pose change probabilistic estimation. It can be converted to/from
46  * these alternative classes:
47  * - mrpt::poses::CPose3DQuatPDFGaussian
48  */
50 
51  /** This is the raw odometry reading, and only is used when
52  * "estimationMethod" is "TEstimationMethod::emOdometry" */
54  /** This fields indicates the way this estimation was obtained.
55  */
57 
59  {
62  };
63 
64  /** The parameter to be passed to "computeFromOdometry". */
66  {
67  /** Default values loader. */
69 
70  /** The model to be used. */
72 
74  {
75  /** Options for the 6DOFModel model which generates a
76  * CPosePDFParticles object an then create from that
77  * CPosePDFGaussian object in poseChange */
79  float a1, a2, a3, a4, a5, a6, a7, a8, a9, a10;
80  /** An additional noise added to the 6DOF model (std. dev. in meters
81  * and radians). */
83  } mm6DOFModel;
84 
86 
87  /** Computes the PDF of the pose increment from an odometry reading and
88  * according to the given motion model (speed and encoder ticks information
89  * is not modified).
90  * According to the parameters in the passed struct, it will be called one
91  * the private sampling functions (see "see also" next).
92  * \sa computeFromOdometry_model6DOF
93  */
95  const mrpt::poses::CPose3D& odometryIncrement,
96  const TMotionModelOptions& options);
97  /** Computes the PDF of the pose increment from an odometry reading, using
98  * the motion model for 6 DOF.
99  * The source: A. L. Ballardini, A. Furlan, A. Galbiati, M. Matteucci, F.
100  * Sacchi, D. G. Sorrenti An effective 6DoF motion model for 3D-6DoF Monte
101  * Carlo Localization 4th Workshop on Planning, Perception and Navigation
102  * for Intelligent Vehicles, IROS, 2012
103  * \sa computeFromOdometry
104  */
106  const mrpt::poses::CPose3D& odometryIncrement,
107  const TMotionModelOptions& o);
108 
109  /** Each "true" entry means that the corresponding "velocities" element
110  * contains valid data - There are 6 entries.
111  */
112  std::vector<bool> hasVelocities;
113 
114  /** The velocity of the robot in each of 6D:
115  * v_x,v_y,v_z,v_yaw,v_pitch,v_roll (linear in meters/sec and angular in
116  * rad/sec).
117  */
119 
120 }; // End of class def.
121 
122 } // End of namespace
123 } // End of namespace
124 
125 #endif
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:40
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a9
float a9
Definition: CActionRobotMovement3D.h:79
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions
The parameter to be passed to "computeFromOdometry".
Definition: CActionRobotMovement3D.h:65
mrpt::obs::CActionRobotMovement3D::TDrawSampleMotionModel
TDrawSampleMotionModel
Definition: CActionRobotMovement3D.h:58
mrpt::obs::CActionRobotMovement3D::TEstimationMethod
TEstimationMethod
A list of posible ways for estimating the content of a CActionRobotMovement3D object.
Definition: CActionRobotMovement3D.h:37
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a5
float a5
Definition: CActionRobotMovement3D.h:79
mrpt::obs::CActionRobotMovement3D::computeFromOdometry_model6DOF
void computeFromOdometry_model6DOF(const mrpt::poses::CPose3D &odometryIncrement, const TMotionModelOptions &o)
Computes the PDF of the pose increment from an odometry reading, using the motion model for 6 DOF.
Definition: CActionRobotMovement3D.cpp:112
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: eigen_frwds.h:44
mrpt::obs::CActionRobotMovement3D::motionModelConfiguration
struct mrpt::obs::CActionRobotMovement3D::TMotionModelOptions motionModelConfiguration
mrpt::obs::CActionRobotMovement3D::rawOdometryIncrementReading
mrpt::poses::CPose3D rawOdometryIncrementReading
This is the raw odometry reading, and only is used when "estimationMethod" is "TEstimationMethod::emO...
Definition: CActionRobotMovement3D.h:53
mrpt::obs::CActionRobotMovement3D
Represents a probabilistic 3D (6D) movement.
Definition: CActionRobotMovement3D.h:29
CPose3DPDFGaussian.h
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a8
float a8
Definition: CActionRobotMovement3D.h:79
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::obs::CActionRobotMovement3D::mmGaussian
@ mmGaussian
Definition: CActionRobotMovement3D.h:60
mrpt::obs::CActionRobotMovement3D::velocities
mrpt::math::CVectorFloat velocities
The velocity of the robot in each of 6D: v_x,v_y,v_z,v_yaw,v_pitch,v_roll (linear in meters/sec and a...
Definition: CActionRobotMovement3D.h:118
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a1
float a1
Definition: CActionRobotMovement3D.h:79
mrpt::obs::CActionRobotMovement3D::hasVelocities
std::vector< bool > hasVelocities
Each "true" entry means that the corresponding "velocities" element contains valid data - There are 6...
Definition: CActionRobotMovement3D.h:112
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel
Definition: CActionRobotMovement3D.h:73
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a3
float a3
Definition: CActionRobotMovement3D.h:79
mrpt::obs::CActionRobotMovement3D::computeFromOdometry
void computeFromOdometry(const mrpt::poses::CPose3D &odometryIncrement, const TMotionModelOptions &options)
Computes the PDF of the pose increment from an odometry reading and according to the given motion mod...
Definition: CActionRobotMovement3D.cpp:76
mrpt::obs::CActionRobotMovement3D::estimationMethod
TEstimationMethod estimationMethod
This fields indicates the way this estimation was obtained.
Definition: CActionRobotMovement3D.h:56
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::modelSelection
TDrawSampleMotionModel modelSelection
The model to be used.
Definition: CActionRobotMovement3D.h:71
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TMotionModelOptions
TMotionModelOptions()
Default values loader.
Definition: CActionRobotMovement3D.cpp:91
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a10
float a10
Definition: CActionRobotMovement3D.h:79
mrpt::obs::CActionRobotMovement3D::poseChange
mrpt::poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
Definition: CActionRobotMovement3D.h:49
mrpt::obs::CActionRobotMovement3D::mm6DOF
@ mm6DOF
Definition: CActionRobotMovement3D.h:61
CAction.h
mrpt::obs::CActionRobotMovement3D::emVisualOdometry
@ emVisualOdometry
Definition: CActionRobotMovement3D.h:40
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::additional_std_angle
float additional_std_angle
Definition: CActionRobotMovement3D.h:82
DEFINE_SERIALIZABLE
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Definition: CSerializable.h:102
mrpt::obs::CActionRobotMovement3D::emOdometry
@ emOdometry
Definition: CActionRobotMovement3D.h:39
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a2
float a2
Definition: CActionRobotMovement3D.h:79
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::nParticlesCount
uint32_t nParticlesCount
Options for the 6DOFModel model which generates a CPosePDFParticles object an then create from that C...
Definition: CActionRobotMovement3D.h:78
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::mm6DOFModel
struct mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel mm6DOFModel
mrpt::obs::CAction
Declares a class for storing a robot action.
Definition: CAction.h:27
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::additional_std_XYZ
float additional_std_XYZ
An additional noise added to the 6DOF model (std.
Definition: CActionRobotMovement3D.h:82
mrpt::obs::CActionRobotMovement3D::CActionRobotMovement3D
CActionRobotMovement3D()
Definition: CActionRobotMovement3D.cpp:27
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a7
float a7
Definition: CActionRobotMovement3D.h:79
uint32_t
unsigned __int32 uint32_t
Definition: rptypes.h:47
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a4
float a4
Definition: CActionRobotMovement3D.h:79
mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::a6
float a6
Definition: CActionRobotMovement3D.h:79



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST