Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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>
13 #include <mrpt/utils/CStream.h>
16 
17 using namespace mrpt;
18 using namespace mrpt::obs;
19 using namespace mrpt::utils;
20 using namespace mrpt::poses;
21 using namespace mrpt::random;
22 using namespace mrpt::utils;
23 
25 
26 /*---------------------------------------------------------------
27  Constructor
28  ---------------------------------------------------------------*/
30  poseChange(),
31  rawOdometryIncrementReading(),
32  estimationMethod( emOdometry ),
33  motionModelConfiguration(),
34  hasVelocities(6,false),
35  velocities(6)
36 {
37  velocities.assign(.0);
38 }
39 
40 
41 /*---------------------------------------------------------------
42  Implements the writing to a CStream capability of CSerializable objects
43  ---------------------------------------------------------------*/
45 {
46  if (version)
47  *version = 1;
48  else
49  {
50  uint32_t i = static_cast<uint32_t>( estimationMethod );
51 
52  out << i;
53 
54  // The PDF:
55  out << poseChange;
56 
57  out << hasVelocities << velocities;
58 
59  out << timestamp;
60  }
61 }
62 
63 /*---------------------------------------------------------------
64  Implements the reading from a CStream capability of CSerializable objects
65  ---------------------------------------------------------------*/
67 {
68  switch(version)
69  {
70  case 0:
71  case 1:
72  {
73  uint32_t i;
74 
75  // Read the estimation method first:
76  in >> i;
77  estimationMethod = static_cast<TEstimationMethod>(i);
78 
79  in >> poseChange;
80  in >> hasVelocities >> velocities;
81 
82  if (version>=1)
83  in >> timestamp;
84  else timestamp = INVALID_TIMESTAMP;
85  } break;
86  default:
88 
89  };
90 
91 }
92 
93 
95  const CPose3D &odometryIncrement,
96  const TMotionModelOptions &options)
97 {
98  // Set the members data:
99  estimationMethod = emOdometry;
100  rawOdometryIncrementReading = odometryIncrement;
101  motionModelConfiguration = options;
102 
103  if ( options.modelSelection == mm6DOF )
104  computeFromOdometry_model6DOF( odometryIncrement, options );
105 }
106 
107 
108 /*---------------------------------------------------------------
109  TMotionModelOptions
110  ---------------------------------------------------------------*/
112 modelSelection(mm6DOF),
113  mm6DOFModel()
114 {
115 
117  mm6DOFModel.a1 = 0.05f;
118  mm6DOFModel.a2 = 0.05f;
119  mm6DOFModel.a3 = 0.05f;
120  mm6DOFModel.a4 = 0.05f;
121  mm6DOFModel.a5 = 0.05f;
122  mm6DOFModel.a6 = 0.05f;
123  mm6DOFModel.a7 = 0.05f;
124  mm6DOFModel.a8 = 0.05f;
125  mm6DOFModel.a9 = 0.05f;
126  mm6DOFModel.a10 = 0.05f;
129 
130 }
131 
132 /*---------------------------------------------------------------
133  computeFromOdometry_model6DOF
134  ---------------------------------------------------------------*/
136  const CPose3D &odometryIncrement,
137  const TMotionModelOptions &o
138  )
139 {
140  // The Gaussian PDF:
141  // ---------------------------
142  CPose3DPDFParticles *aux;
143  static CPose3D nullPose(0,0,0,0,0,0);
144 
145  mrpt::poses::CPose3DPDFPtr poseChangeTemp = CPose3DPDFParticles::Create();
146  aux = static_cast<CPose3DPDFParticles*>( poseChangeTemp.pointer() );
147 
148  // Set the number of particles:
150 
151  //The motion model: A. L. Ballardini, A. Furlan, A. Galbiati, M. Matteucci, F. Sacchi, D. G. Sorrenti An effective 6DoF motion model for 3D-6DoF Monte Carlo Localization 4th Workshop on Planning, Perception and Navigation for Intelligent Vehicles, IROS, 2012
152 
153  /*
154  The brief description:
155  Find XYZ deltas: dX=x(t)-x(t-1); dY=y(t)-y(t-1); dZ=z(t)-z(t-1)
156  Find angular deltas (normalised): dRoll=roll(t)-roll(t-1); dPitch=pitch(t)-pitch(t-1); dYaw=yaw(t)-yaw(t-1)
157 
158  Here t - is time step
159 
160  Calculate the odometry for 6DOF:
161 
162 
163  yaw1 = atan2 (dY,dX)
164  pitch1 = atan2 (dZ , sqrt(dX^2 + dY^2 + dZ^2) )
165  trans = sqrt(dX^2 + dY^2 + dZ^2)
166  roll = dRoll
167  yaw2= dYaw
168  pitch2= dPitch
169 
170 
171  Model the error:
172 
173 
174  yaw1_draw = yaw1 + sample(a1 * yaw1 + a2 * trans )
175  pitch1_draw = pitch1 + sample(a3 * dZ )
176  trans_draw = trans + sample( a4 * trans + a5 * yaw2 + a6 * (roll + pitch2 ) )
177  roll_draw = roll + sample(a7 * roll)
178  pitch2_draw = pitch2 + sample(a8 * pitch2 )
179  yaw2_draw = yaw2 + sample(a9 * yaw2 + a10 * trans )
180 
181  Here sample() - sampling from zero mean Gaussian distribution
182 
183  Calculate the spherical coordinates:
184  x = trans_draw * sin( pitch1_draw ) * cos ( yaw1_draw )
185  y = trans_draw * sin( pitch1_draw ) * sin ( yaw1_draw )
186  z = trans_draw * cos( pitch1_draw )
187  roll = roll_draw
188  pitch = pitch1_draw + pitch2_draw
189  yaw = yaw1_draw + yaw2_draw
190 
191  normalize_angle(roll, pitch, yaw )
192  */
193 
194 
195  // The increments in odometry:
196  float Ayaw1 = ( odometryIncrement.y()!=0 || odometryIncrement.x()!=0) ?
197  atan2( odometryIncrement.y(), odometryIncrement.x() ) : 0;
198 
199  float Atrans = odometryIncrement.norm();
200 
201  float Apitch1 = ( odometryIncrement.y()!=0 || odometryIncrement.x()!=0 || odometryIncrement.z()!=0) ?
202  atan2( odometryIncrement.z(), odometryIncrement.norm()) : 0;
203 
204  float Aroll = odometryIncrement.roll();
205  float Apitch2 = odometryIncrement.pitch();
206  float Ayaw2 = odometryIncrement.yaw();
207 
208 
209 
210  // Draw samples:
211  for (size_t i=0;i<o.mm6DOFModel.nParticlesCount;i++)
212  {
213  float Ayaw1_draw = Ayaw1 + (o.mm6DOFModel.a1*Ayaw1 +o.mm6DOFModel.a2*Atrans ) * randomGenerator.drawGaussian1D_normalized();
214  float Apitch1_draw= Apitch1 + (o.mm6DOFModel.a3*odometryIncrement.z())* randomGenerator.drawGaussian1D_normalized();
215  float Atrans_draw = Atrans + (o.mm6DOFModel.a4*Atrans+ o.mm6DOFModel.a5*Ayaw2+ o.mm6DOFModel.a6*(Aroll+Apitch2)) * randomGenerator.drawGaussian1D_normalized();
216 
217  float Aroll_draw=Aroll+(o.mm6DOFModel.a7*Aroll) * randomGenerator.drawGaussian1D_normalized();
218  float Apitch2_draw=Apitch2+(o.mm6DOFModel.a8*Apitch2)* randomGenerator.drawGaussian1D_normalized();
219  float Ayaw2_draw=Ayaw2 +(o.mm6DOFModel.a9*Ayaw2+o.mm6DOFModel.a10*Atrans)* randomGenerator.drawGaussian1D_normalized();
220 
221  // Output:
222  aux->m_particles[i].d->x( Atrans_draw * sin( Apitch1_draw )*cos(Ayaw1_draw) + motionModelConfiguration.mm6DOFModel.additional_std_XYZ * randomGenerator.drawGaussian1D_normalized() );
223  aux->m_particles[i].d->y( Atrans_draw * sin( Apitch1_draw )*sin(Ayaw1_draw) + motionModelConfiguration.mm6DOFModel.additional_std_XYZ * randomGenerator.drawGaussian1D_normalized() );
225 
226 
228  double new_pitch= Apitch1_draw + Apitch2_draw + motionModelConfiguration.mm6DOFModel.additional_std_angle * randomGenerator.drawGaussian1D_normalized() ;
230 
231  aux->m_particles[i].d->setYawPitchRoll(new_yaw,new_pitch,new_roll);
232  aux->m_particles[i].d->normalizeAngles();
233 
234  }
235 
236  poseChange.copyFrom(*poseChangeTemp);
237 }
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
mrpt::poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
void copyFrom(const CPose3DPDF &o) MRPT_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.
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
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...
float additional_std_XYZ
An additional noise added to the 6DOF model (std.
double drawGaussian1D_normalized(double *likelihood=NULL)
Generate a normalized (mean=0, std=1) normally distributed sample.
GLuint in
Definition: glew.h:7146
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...
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
The parameter to be passed to "computeFromOdometry".
Represents a probabilistic 3D (6D) movement.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
CParticleList m_particles
The array of particles.
uint32_t nParticlesCount
Options for the 6DOFModel model which generates a CPosePDFParticles object an then create from that C...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
TEstimationMethod
A list of posible ways for estimating the content of a CActionRobotMovement3D object.
int version
Definition: mrpt_jpeglib.h:898
struct OBS_IMPEXP mrpt::obs::CActionRobotMovement3D::TMotionModelOptions motionModelConfiguration
#define DEG2RAD
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
Declares a class for storing a robot action.
Definition: obs/CAction.h:33
TDrawSampleMotionModel modelSelection
The model to be used.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
void resetDeterministic(const CPose3D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose...
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:174
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
struct OBS_IMPEXP mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel mm6DOFModel
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
unsigned __int32 uint32_t
Definition: rptypes.h:49
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018