MRPT  2.0.4
CActionRobotMovement3D.cpp
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 
10 #include "obs-precomp.h" // Precompiled headers
11 
14 #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 uint8_t CActionRobotMovement3D::serializeGetVersion() const { return 1; }
27 {
28  out.WriteAs<uint32_t>(estimationMethod);
29  out << poseChange;
30  out << hasVelocities << velocities;
31  out << timestamp;
32 }
33 
35  mrpt::serialization::CArchive& in, uint8_t version)
36 {
37  switch (version)
38  {
39  case 0:
40  case 1:
41  {
42  uint32_t i;
43 
44  // Read the estimation method first:
45  in >> i;
46  estimationMethod = static_cast<TEstimationMethod>(i);
47 
48  in >> poseChange;
49  in >> hasVelocities >> velocities;
50 
51  if (version >= 1)
52  in >> timestamp;
53  else
54  timestamp = INVALID_TIMESTAMP;
55  }
56  break;
57  default:
59  };
60 }
61 
63  const CPose3D& odometryIncrement, const TMotionModelOptions& options)
64 {
65  // Set the members data:
66  estimationMethod = emOdometry;
67  rawOdometryIncrementReading = odometryIncrement;
68  motionModelConfiguration = options;
69 
70  if (options.modelSelection == mm6DOF)
71  computeFromOdometry_model6DOF(odometryIncrement, options);
72 }
73 
74 /*---------------------------------------------------------------
75  computeFromOdometry_model6DOF
76  ---------------------------------------------------------------*/
78  const CPose3D& odometryIncrement, const TMotionModelOptions& o)
79 {
80  // The Gaussian PDF:
81  // ---------------------------
83  const mrpt::math::TPose3D nullPose(0, 0, 0, 0, 0, 0);
84 
85  mrpt::poses::CPose3DPDF::Ptr poseChangeTemp =
86  std::make_shared<CPose3DPDFParticles>();
87  aux = dynamic_cast<CPose3DPDFParticles*>(poseChangeTemp.get());
88 
89  // Set the number of particles:
91 
92  // The motion model: A. L. Ballardini, A. Furlan, A. Galbiati, M. Matteucci,
93  // F. Sacchi, D. G. Sorrenti An effective 6DoF motion model for 3D-6DoF
94  // Monte Carlo Localization 4th Workshop on Planning, Perception and
95  // Navigation for Intelligent Vehicles, IROS, 2012
96 
97  /*
98  The brief description:
99  Find XYZ deltas: dX=x(t)-x(t-1); dY=y(t)-y(t-1); dZ=z(t)-z(t-1)
100  Find angular deltas (normalised): dRoll=roll(t)-roll(t-1);
101  dPitch=pitch(t)-pitch(t-1); dYaw=yaw(t)-yaw(t-1)
102 
103  Here t - is time step
104 
105  Calculate the odometry for 6DOF:
106 
107 
108  yaw1 = atan2 (dY,dX)
109  pitch1 = atan2 (dZ , sqrt(dX^2 + dY^2 + dZ^2) )
110  trans = sqrt(dX^2 + dY^2 + dZ^2)
111  roll = dRoll
112  yaw2= dYaw
113  pitch2= dPitch
114 
115 
116  Model the error:
117 
118 
119  yaw1_draw = yaw1 + sample(a1 * yaw1 + a2 * trans )
120  pitch1_draw = pitch1 + sample(a3 * dZ )
121  trans_draw = trans + sample( a4 * trans + a5 * yaw2 + a6 * (roll
122  + pitch2 ) )
123  roll_draw = roll + sample(a7 * roll)
124  pitch2_draw = pitch2 + sample(a8 * pitch2 )
125  yaw2_draw = yaw2 + sample(a9 * yaw2 + a10 * trans )
126 
127  Here sample() - sampling from zero mean Gaussian distribution
128 
129  Calculate the spherical coordinates:
130  // Original:
131  x = trans_draw * sin( pitch1_draw ) * cos ( yaw1_draw )
132  y = trans_draw * sin( pitch1_draw ) * sin ( yaw1_draw )
133  z = trans_draw * cos( pitch1_draw )
134 
135  // Corrected (?) by JLBC (Jun 2019)
136  x = trans_draw * cos( pitch1_draw ) * cos ( yaw1_draw )
137  y = trans_draw * cos( pitch1_draw ) * sin ( yaw1_draw )
138  z = -trans_draw * sin( pitch1_draw )
139 
140  roll = roll_draw
141  pitch = pitch1_draw + pitch2_draw
142  yaw = yaw1_draw + yaw2_draw
143 
144  normalize_angle(roll, pitch, yaw )
145  */
146 
147  // The increments in odometry:
148  double Ayaw1 = (odometryIncrement.y() != 0 || odometryIncrement.x() != 0)
149  ? atan2(odometryIncrement.y(), odometryIncrement.x())
150  : 0;
151 
152  double Atrans = odometryIncrement.norm();
153 
154  double Apitch1 =
155  (odometryIncrement.y() != 0 || odometryIncrement.x() != 0 ||
156  odometryIncrement.z() != 0)
157  ? atan2(odometryIncrement.z(), odometryIncrement.norm())
158  : 0;
159 
160  double Aroll = odometryIncrement.roll();
161  double Apitch2 = odometryIncrement.pitch();
162  double Ayaw2 = odometryIncrement.yaw();
163 
164  const auto stdxyz = motionModelConfiguration.mm6DOFModel.additional_std_XYZ;
165  auto& rnd = mrpt::random::getRandomGenerator();
166 
167  // Draw samples:
168  for (size_t i = 0; i < o.mm6DOFModel.nParticlesCount; i++)
169  {
170  auto& ith_part = aux->m_particles[i].d;
171  double Ayaw1_draw =
172  Ayaw1 + (o.mm6DOFModel.a1 * Ayaw1 + o.mm6DOFModel.a2 * Atrans) *
173  rnd.drawGaussian1D_normalized();
174  double Apitch1_draw =
175  Apitch1 + (o.mm6DOFModel.a3 * odometryIncrement.z()) *
176  rnd.drawGaussian1D_normalized();
177  double Atrans_draw =
178  Atrans + (o.mm6DOFModel.a4 * Atrans + o.mm6DOFModel.a5 * Ayaw2 +
179  o.mm6DOFModel.a6 * (Aroll + Apitch2)) *
180  rnd.drawGaussian1D_normalized();
181 
182  double Aroll_draw = Aroll + (o.mm6DOFModel.a7 * Aroll) *
183  rnd.drawGaussian1D_normalized();
184  double Apitch2_draw = Apitch2 + (o.mm6DOFModel.a8 * Apitch2) *
185  rnd.drawGaussian1D_normalized();
186  double Ayaw2_draw =
187  Ayaw2 + (o.mm6DOFModel.a9 * Ayaw2 + o.mm6DOFModel.a10 * Atrans) *
188  rnd.drawGaussian1D_normalized();
189 
190  // Output:
191  ith_part.x = Atrans_draw * cos(Apitch1_draw) * cos(Ayaw1_draw) +
192  stdxyz * rnd.drawGaussian1D_normalized();
193  ith_part.y = Atrans_draw * cos(Apitch1_draw) * sin(Ayaw1_draw) +
194  stdxyz * rnd.drawGaussian1D_normalized();
195  ith_part.z = -Atrans_draw * sin(Apitch1_draw) +
196  stdxyz * rnd.drawGaussian1D_normalized();
197 
198  ith_part.yaw =
199  Ayaw1_draw + Ayaw2_draw +
200  motionModelConfiguration.mm6DOFModel.additional_std_angle *
201  rnd.drawGaussian1D_normalized();
202  ith_part.pitch =
203  Apitch1_draw + Apitch2_draw +
204  motionModelConfiguration.mm6DOFModel.additional_std_angle *
205  rnd.drawGaussian1D_normalized();
206  ith_part.roll =
207  Aroll_draw +
208  motionModelConfiguration.mm6DOFModel.additional_std_angle *
209  rnd.drawGaussian1D_normalized();
210  }
211 
212  poseChange.copyFrom(*poseChangeTemp);
213 }
214 
216 {
218 
219  o << "Robot Movement (as a gaussian pose change):\n";
220  o << poseChange << "\n";
221 }
A namespace of pseudo-random numbers generators of diferent distributions.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
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:552
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
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...
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...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
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...
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.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:256
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:558
Declares a class for storing a robot action.
Definition: CAction.h:24
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:54
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
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).
Definition: TPose3D.h:24
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the action contents and dump it to the output str...
Definition: CAction.cpp:22
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...
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.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020