MRPT  1.9.9
CActionRobotMovement3D.cpp
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 
10 #include "obs-precomp.h" // Precompiled headers
11 
12 #include <mrpt/random.h>
16 
17 using namespace mrpt;
18 using namespace mrpt::obs;
19 using namespace mrpt::poses;
20 using namespace mrpt::random;
21 
23 
24 /*---------------------------------------------------------------
25  Constructor
26  ---------------------------------------------------------------*/
28  : poseChange(),
29  rawOdometryIncrementReading(),
30  estimationMethod(emOdometry),
31  motionModelConfiguration(),
32  hasVelocities(6, false),
33  velocities(6)
34 {
35  velocities.assign(.0);
36 }
37 
41 {
42  out.WriteAs<uint32_t>(estimationMethod);
43  out << poseChange;
44  out << hasVelocities << velocities;
45  out << timestamp;
46 }
47 
50 {
51  switch (version)
52  {
53  case 0:
54  case 1:
55  {
56  uint32_t i;
57 
58  // Read the estimation method first:
59  in >> i;
60  estimationMethod = static_cast<TEstimationMethod>(i);
61 
62  in >> poseChange;
63  in >> hasVelocities >> velocities;
64 
65  if (version >= 1)
66  in >> timestamp;
67  else
68  timestamp = INVALID_TIMESTAMP;
69  }
70  break;
71  default:
73  };
74 }
75 
77  const CPose3D& odometryIncrement, const TMotionModelOptions& options)
78 {
79  // Set the members data:
80  estimationMethod = emOdometry;
81  rawOdometryIncrementReading = odometryIncrement;
82  motionModelConfiguration = options;
83 
84  if (options.modelSelection == mm6DOF)
85  computeFromOdometry_model6DOF(odometryIncrement, options);
86 }
87 
88 /*---------------------------------------------------------------
89  TMotionModelOptions
90  ---------------------------------------------------------------*/
92  : modelSelection(mm6DOF), mm6DOFModel()
93 {
95  mm6DOFModel.a1 = 0.05f;
96  mm6DOFModel.a2 = 0.05f;
97  mm6DOFModel.a3 = 0.05f;
98  mm6DOFModel.a4 = 0.05f;
99  mm6DOFModel.a5 = 0.05f;
100  mm6DOFModel.a6 = 0.05f;
101  mm6DOFModel.a7 = 0.05f;
102  mm6DOFModel.a8 = 0.05f;
103  mm6DOFModel.a9 = 0.05f;
104  mm6DOFModel.a10 = 0.05f;
107 }
108 
109 /*---------------------------------------------------------------
110  computeFromOdometry_model6DOF
111  ---------------------------------------------------------------*/
113  const CPose3D& odometryIncrement, const TMotionModelOptions& o)
114 {
115  // The Gaussian PDF:
116  // ---------------------------
117  CPose3DPDFParticles* aux;
118  const mrpt::math::TPose3D nullPose(0, 0, 0, 0, 0, 0);
119 
120  mrpt::poses::CPose3DPDF::Ptr poseChangeTemp =
121  mrpt::make_aligned_shared<CPose3DPDFParticles>();
122  aux = static_cast<CPose3DPDFParticles*>(poseChangeTemp.get());
123 
124  // Set the number of particles:
126 
127  // The motion model: A. L. Ballardini, A. Furlan, A. Galbiati, M. Matteucci,
128  // F. Sacchi, D. G. Sorrenti An effective 6DoF motion model for 3D-6DoF
129  // Monte Carlo Localization 4th Workshop on Planning, Perception and
130  // Navigation for Intelligent Vehicles, IROS, 2012
131 
132  /*
133  The brief description:
134  Find XYZ deltas: dX=x(t)-x(t-1); dY=y(t)-y(t-1); dZ=z(t)-z(t-1)
135  Find angular deltas (normalised): dRoll=roll(t)-roll(t-1);
136  dPitch=pitch(t)-pitch(t-1); dYaw=yaw(t)-yaw(t-1)
137 
138  Here t - is time step
139 
140  Calculate the odometry for 6DOF:
141 
142 
143  yaw1 = atan2 (dY,dX)
144  pitch1 = atan2 (dZ , sqrt(dX^2 + dY^2 + dZ^2) )
145  trans = sqrt(dX^2 + dY^2 + dZ^2)
146  roll = dRoll
147  yaw2= dYaw
148  pitch2= dPitch
149 
150 
151  Model the error:
152 
153 
154  yaw1_draw = yaw1 + sample(a1 * yaw1 + a2 * trans )
155  pitch1_draw = pitch1 + sample(a3 * dZ )
156  trans_draw = trans + sample( a4 * trans + a5 * yaw2 + a6 * (roll
157  + pitch2 ) )
158  roll_draw = roll + sample(a7 * roll)
159  pitch2_draw = pitch2 + sample(a8 * pitch2 )
160  yaw2_draw = yaw2 + sample(a9 * yaw2 + a10 * trans )
161 
162  Here sample() - sampling from zero mean Gaussian distribution
163 
164  Calculate the spherical coordinates:
165  x = trans_draw * sin( pitch1_draw ) * cos ( yaw1_draw )
166  y = trans_draw * sin( pitch1_draw ) * sin ( yaw1_draw )
167  z = trans_draw * cos( pitch1_draw )
168  roll = roll_draw
169  pitch = pitch1_draw + pitch2_draw
170  yaw = yaw1_draw + yaw2_draw
171 
172  normalize_angle(roll, pitch, yaw )
173  */
174 
175  // The increments in odometry:
176  float Ayaw1 = (odometryIncrement.y() != 0 || odometryIncrement.x() != 0)
177  ? atan2(odometryIncrement.y(), odometryIncrement.x())
178  : 0;
179 
180  float Atrans = odometryIncrement.norm();
181 
182  float Apitch1 = (odometryIncrement.y() != 0 || odometryIncrement.x() != 0 ||
183  odometryIncrement.z() != 0)
184  ? atan2(odometryIncrement.z(), odometryIncrement.norm())
185  : 0;
186 
187  float Aroll = odometryIncrement.roll();
188  float Apitch2 = odometryIncrement.pitch();
189  float Ayaw2 = odometryIncrement.yaw();
190 
191  // Draw samples:
192  for (size_t i = 0; i < o.mm6DOFModel.nParticlesCount; i++)
193  {
194  float Ayaw1_draw =
195  Ayaw1 +
196  (o.mm6DOFModel.a1 * Ayaw1 + o.mm6DOFModel.a2 * Atrans) *
198  float Apitch1_draw =
199  Apitch1 +
200  (o.mm6DOFModel.a3 * odometryIncrement.z()) *
202  float Atrans_draw =
203  Atrans +
204  (o.mm6DOFModel.a4 * Atrans + o.mm6DOFModel.a5 * Ayaw2 +
205  o.mm6DOFModel.a6 * (Aroll + Apitch2)) *
207 
208  float Aroll_draw = Aroll +
209  (o.mm6DOFModel.a7 * Aroll) *
211  float Apitch2_draw =
212  Apitch2 +
213  (o.mm6DOFModel.a8 * Apitch2) *
215  float Ayaw2_draw =
216  Ayaw2 +
217  (o.mm6DOFModel.a9 * Ayaw2 + o.mm6DOFModel.a10 * Atrans) *
219 
220  // Output:
221  aux->m_particles[i].d.x =
222  Atrans_draw * sin(Apitch1_draw) * cos(Ayaw1_draw) +
225  aux->m_particles[i].d.y =
226  Atrans_draw * sin(Apitch1_draw) * sin(Ayaw1_draw) +
229  aux->m_particles[i].d.z =
230  Atrans_draw * cos(Apitch1_draw) +
233 
234  double new_yaw =
235  Ayaw1_draw + Ayaw2_draw +
238  double new_pitch =
239  Apitch1_draw + Apitch2_draw +
242  double new_roll =
243  Aroll_draw +
246 
247  aux->m_particles[i].d.yaw = new_yaw;
248  aux->m_particles[i].d.pitch = new_pitch;
249  aux->m_particles[i].d.roll = new_roll;
250  }
251 
252  poseChange.copyFrom(*poseChangeTemp);
253 }
A namespace of pseudo-random numbers generators of diferent distributions.
struct mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel mm6DOFModel
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
mrpt::poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
double DEG2RAD(const double x)
Degrees to radians.
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...
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:532
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:526
void WriteAs(const TYPE_FROM_ACTUAL &value)
Definition: CArchive.h:156
struct mrpt::obs::CActionRobotMovement3D::TMotionModelOptions motionModelConfiguration
float additional_std_XYZ
An additional noise added to the 6DOF model (std.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
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...
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
The parameter to be passed to "computeFromOdometry".
Represents a probabilistic 3D (6D) movement.
uint32_t nParticlesCount
Options for the 6DOFModel model which generates a CPosePDFParticles object an then create from that C...
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:253
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
TEstimationMethod
A list of posible ways for estimating the content of a CActionRobotMovement3D object.
This namespace contains representation of robot actions and observations.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:538
Declares a class for storing a robot action.
Definition: CAction.h:27
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TDrawSampleMotionModel modelSelection
The model to be used.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
GLuint in
Definition: glext.h:7274
void resetDeterministic(const mrpt::math::TPose3D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose...
unsigned __int32 uint32_t
Definition: rptypes.h:47
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
double drawGaussian1D_normalized()
Generate a normalized (mean=0, std=1) normally distributed sample.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020