Main MRPT website > C++ reference for MRPT 1.5.7
CActionRobotMovement2D.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 
13 #include <mrpt/utils/CStream.h>
16 #include <mrpt/random.h>
18 #include <mrpt/math/wrap2pi.h>
19 
20 using namespace mrpt::obs;
21 using namespace mrpt::utils;
22 using namespace mrpt::poses;
23 using namespace mrpt::math;
24 using namespace mrpt::random;
25 using namespace std;
26 
28 
29 /*---------------------------------------------------------------
30  Constructor
31  ---------------------------------------------------------------*/
33  poseChange(CPosePDFGaussian::Create()),
34  rawOdometryIncrementReading(),
35  estimationMethod(emOdometry),
36  hasEncodersInfo(false),
37  encoderLeftTicks(0),
38  encoderRightTicks(0),
39  hasVelocities(false),
40  velocityLocal(.0,.0,.0),
41  motionModelConfiguration(),
42  m_fastDrawGauss_Z(),
43  m_fastDrawGauss_M()
44 {
45 }
46 
47 /*---------------------------------------------------------------
48  Implements the writing to a CStream capability of CSerializable objects
49  ---------------------------------------------------------------*/
51 {
52  if (version)
53  *version = 7;
54  else
55  {
56  uint32_t i = static_cast<uint32_t>(estimationMethod);
57 
58  out << i;
59 
60  // Added in version 2:
61  // If the estimation method is emOdometry, save the rawOdo + config data instead of
62  // the PDF itself:
63  if ( estimationMethod == emOdometry )
64  {
65  // The odometry data:
66  out << rawOdometryIncrementReading;
67 
68  i = static_cast<uint32_t>(motionModelConfiguration.modelSelection);
69  out << i;
70  out << motionModelConfiguration.gaussianModel.a1
71  << motionModelConfiguration.gaussianModel.a2
72  << motionModelConfiguration.gaussianModel.a3
73  << motionModelConfiguration.gaussianModel.a4
74  << motionModelConfiguration.gaussianModel.minStdXY
75  << motionModelConfiguration.gaussianModel.minStdPHI;
76 
77  out << motionModelConfiguration.thrunModel.nParticlesCount
78  << motionModelConfiguration.thrunModel.alfa1_rot_rot
79  << motionModelConfiguration.thrunModel.alfa2_rot_trans
80  << motionModelConfiguration.thrunModel.alfa3_trans_trans
81  << motionModelConfiguration.thrunModel.alfa4_trans_rot
82  << motionModelConfiguration.thrunModel.additional_std_XY
83  << motionModelConfiguration.thrunModel.additional_std_phi;
84  }
85  else
86  {
87  // The PDF:
88  out << (*poseChange);
89  }
90 
91  // Added in version 1:
92  out << hasVelocities;
93  if (hasVelocities)
94  out << velocityLocal; // v7
95 
96  out << hasEncodersInfo;
97  if (hasEncodersInfo)
98  out << encoderLeftTicks << encoderRightTicks; // added if() in v7
99 
100  // Added in version 6
101  out << timestamp;
102  }
103 }
104 
105 /*---------------------------------------------------------------
106  Implements the reading from a CStream capability of CSerializable objects
107  ---------------------------------------------------------------*/
109 {
110  switch(version)
111  {
112  case 4:
113  case 5:
114  case 6:
115  case 7:
116  {
117  int32_t i;
118 
119  // Read the estimation method first:
120  in >> i;
121  estimationMethod = static_cast<TEstimationMethod>(i);
122 
123  // If the estimation method is emOdometry, save the rawOdo + config data instead of
124  // the PDF itself:
125  if ( estimationMethod == emOdometry )
126  {
127  // The odometry data:
128  in >> rawOdometryIncrementReading;
129 
130  in >> i;
131  motionModelConfiguration.modelSelection = static_cast<TDrawSampleMotionModel>(i);
132 
133  in >> motionModelConfiguration.gaussianModel.a1
134  >> motionModelConfiguration.gaussianModel.a2
135  >> motionModelConfiguration.gaussianModel.a3
136  >> motionModelConfiguration.gaussianModel.a4
137  >> motionModelConfiguration.gaussianModel.minStdXY
138  >> motionModelConfiguration.gaussianModel.minStdPHI;
139 
140  in >> i; motionModelConfiguration.thrunModel.nParticlesCount=i;
141  in >> motionModelConfiguration.thrunModel.alfa1_rot_rot
142  >> motionModelConfiguration.thrunModel.alfa2_rot_trans
143  >> motionModelConfiguration.thrunModel.alfa3_trans_trans
144  >> motionModelConfiguration.thrunModel.alfa4_trans_rot;
145 
146  if (version>=5)
147  {
148  in >> motionModelConfiguration.thrunModel.additional_std_XY
149  >> motionModelConfiguration.thrunModel.additional_std_phi;
150  }
151  else
152  {
153  motionModelConfiguration.thrunModel.additional_std_XY =
154  motionModelConfiguration.thrunModel.additional_std_phi = 0;
155  }
156 
157  // Re-build the PDF:
158  computeFromOdometry( rawOdometryIncrementReading,motionModelConfiguration );
159  }
160  else
161  {
162  // Read the PDF directly from the stream:
163  CPosePDFPtr pc;
164  in >> pc;
165  poseChange = pc;
166  }
167 
168  in >> hasVelocities;
169  if (version >= 7) {
170  if (hasVelocities)
171  in >> velocityLocal;
172  }
173  else {
174  float velocityLin, velocityAng;
175  in >> velocityLin >> velocityAng;
176  velocityLocal.vx = velocityLin;
177  velocityLocal.vy = .0f;
178  velocityLocal.omega = velocityAng;
179  }
180  in >> hasEncodersInfo;
181  if (version < 7 || hasEncodersInfo) {
182  in >> i; encoderLeftTicks = i;
183  in >> i; encoderRightTicks = i;
184  }
185  else {
186  encoderLeftTicks = 0;
187  encoderRightTicks = 0;
188  }
189 
190  if (version>=6)
191  in >> timestamp;
192  else timestamp = INVALID_TIMESTAMP;
193 
194  } break;
195 
196  case 3:
197  {
198  int32_t i;
199 
200  // Read the estimation method first:
201  in >> i;
202  estimationMethod = static_cast<TEstimationMethod>(i);
203 
204  // If the estimation method is emOdometry, save the rawOdo + config data instead of
205  // the PDF itself:
206  if ( estimationMethod == emOdometry )
207  {
208  // The odometry data:
209  in >> rawOdometryIncrementReading;
210 
211  in >> i;
212  motionModelConfiguration.modelSelection = static_cast<TDrawSampleMotionModel>(i);
213 
214  float dum1,dum2,dum3;
215 
216  in >> dum1 >> dum2 >> dum3 >> motionModelConfiguration.gaussianModel.minStdXY >> motionModelConfiguration.gaussianModel.minStdPHI;
217 
218  // Leave the default values for a1,a2,a3,a4:
219  in >> i; motionModelConfiguration.thrunModel.nParticlesCount=i;
220  in >> motionModelConfiguration.thrunModel.alfa1_rot_rot;
221  in >> motionModelConfiguration.thrunModel.alfa2_rot_trans >> motionModelConfiguration.thrunModel.alfa3_trans_trans >> motionModelConfiguration.thrunModel.alfa4_trans_rot;
222 
223  // Re-build the PDF:
224  computeFromOdometry( rawOdometryIncrementReading,motionModelConfiguration );
225  }
226  else
227  {
228  // Read the PDF directly from the stream:
229  CPosePDFPtr pc;
230  in >> pc;
231  poseChange = pc;
232  }
233 
234  in >> hasVelocities;
235  {
236  float velocityLin, velocityAng;
237  in >> velocityLin >> velocityAng;
238  velocityLocal.vx = velocityLin;
239  velocityLocal.vy = .0f;
240  velocityLocal.omega = velocityAng;
241  }
242  in >> hasEncodersInfo;
243 
244  in >> i; encoderLeftTicks=i;
245  in >> i; encoderRightTicks=i;
246 
247  } break;
248 
249  case 2:
250  {
251  int32_t i;
252 
253  // Read the estimation method first:
254  in >> i;
255  estimationMethod = static_cast<TEstimationMethod>(i);
256 
257  // If the estimation method is emOdometry, save the rawOdo + config data instead of
258  // the PDF itself:
259  if ( estimationMethod == emOdometry )
260  {
261  // The odometry data:
262  in >> rawOdometryIncrementReading;
263 
264 // TMotionModelOptions_OLD_VERSION_2 dummy;
265 // in.ReadBuffer( &dummy, sizeof(TMotionModelOptions_OLD_VERSION_2) );
266  uint8_t dummy[44];
267  in.ReadBuffer( &dummy, sizeof(dummy) );
268 
269  motionModelConfiguration = TMotionModelOptions();
270 
271  // Re-build the PDF:
272  computeFromOdometry( rawOdometryIncrementReading,motionModelConfiguration );
273  }
274  else
275  {
276  // Read the PDF directly from the stream:
277  CPosePDFPtr pc;
278  in >> pc;
279  poseChange = pc;
280  }
281 
282  in >> hasVelocities;
283  {
284  float velocityLin, velocityAng;
285  in >> velocityLin >> velocityAng;
286  velocityLocal.vx = velocityLin;
287  velocityLocal.vy = .0f;
288  velocityLocal.omega = velocityAng;
289  }
290  in >> hasEncodersInfo;
291  in >> i; encoderLeftTicks=i;
292  in >> i; encoderRightTicks=i;
293 
294  } break;
295  case 0:
296  case 1:
297  {
298  int32_t i;
299  {
300  CPosePDFPtr pc;
301  in >> pc;
302  poseChange = pc;
303  }
304  in >> i;
305 
306  // copy:
307  estimationMethod = static_cast<TEstimationMethod>(i);
308 
309  // Simulate the "rawOdometryIncrementReading" as the mean value:
310  if (estimationMethod==emOdometry)
311  poseChange->getMean( rawOdometryIncrementReading );
312  else rawOdometryIncrementReading = CPose2D(0,0,0);
313 
314  if (version>=1)
315  {
316  in >> hasVelocities;
317  {
318  float velocityLin, velocityAng;
319  in >> velocityLin >> velocityAng;
320  velocityLocal.vx = velocityLin;
321  velocityLocal.vy = .0f;
322  velocityLocal.omega = velocityAng;
323  }
324  in >> hasEncodersInfo;
325  in >> i; encoderLeftTicks = i;
326  in >> i; encoderRightTicks = i;
327  }
328  else
329  {
330  // Default values:
331  hasVelocities = hasEncodersInfo = false;
332  encoderLeftTicks= encoderRightTicks= 0;
333  velocityLocal = mrpt::math::TTwist2D(.0, .0, .0);
334  }
335 
336  } break;
337  default:
339 
340  };
341 
342 }
343 
344 /*---------------------------------------------------------------
345  computeFromEncoders
346  ---------------------------------------------------------------*/
348  double K_left,
349  double K_right,
350  double D )
351 {
352  if (hasEncodersInfo)
353  {
354  const double As = 0.5* ( K_right*encoderRightTicks + K_left*encoderLeftTicks );
355  const double Aphi = ( K_right*encoderRightTicks - K_left*encoderLeftTicks ) / D;
356 
357  double x,y;
358  if (Aphi!=0)
359  {
360  const double R = As/Aphi;
361  x=R*sin(Aphi);
362  y=R*(1-cos(Aphi));
363  }
364  else
365  {
366  x=As;
367  y=0;
368  }
369 
370  // Build the whole PDF with the current parameters:
371  computeFromOdometry( CPose2D(x,y,Aphi), motionModelConfiguration );
372  }
373 }
374 
375 /*---------------------------------------------------------------
376  computeFromOdometry
377  ---------------------------------------------------------------*/
379  const CPose2D &odometryIncrement,
380  const TMotionModelOptions &options)
381 {
382  // Set the members data:
383  estimationMethod = emOdometry;
384  rawOdometryIncrementReading = odometryIncrement;
385  motionModelConfiguration = options;
386 
387  if ( options.modelSelection == mmGaussian )
388  computeFromOdometry_modelGaussian( odometryIncrement, options );
389  else computeFromOdometry_modelThrun( odometryIncrement, options );
390 }
391 
392 
393 /*---------------------------------------------------------------
394  TMotionModelOptions
395  ---------------------------------------------------------------*/
397  modelSelection( CActionRobotMovement2D::mmGaussian ),
398  gaussianModel(),
399  thrunModel()
400 {
401  gaussianModel.a1 = 0.01f;
402  gaussianModel.a2 = RAD2DEG( 0.001f );
403  gaussianModel.a3 = DEG2RAD( 1.0f );
404  gaussianModel.a4 = 0.05f;
405 
406  gaussianModel.minStdXY = 0.01f;
408 
410  thrunModel.alfa1_rot_rot = 0.05f;
413  thrunModel.alfa4_trans_rot = RAD2DEG( 0.0001f );
414  thrunModel.additional_std_XY = 0.001f;
416 }
417 
418 /*---------------------------------------------------------------
419  computeFromOdometry_modelGaussian
420  ---------------------------------------------------------------*/
422  const CPose2D &odometryIncrement,
423  const TMotionModelOptions &o
424  )
425 {
426  // The Gaussian PDF:
427  // ---------------------------
428  poseChange = CPosePDFGaussian::Create();
429  CPosePDFGaussian *aux;
430  aux = static_cast<CPosePDFGaussian*>( poseChange.pointer() );
431 
432  // See http://www.mrpt.org/Probabilistic_Motion_Models
433  // -----------------------------------
434 
435  // Build the odometry noise matrix:
436  double Al = odometryIncrement.norm();
437  CMatrixDouble31 ODO_INCR = CMatrixDouble31(odometryIncrement);
438  CMatrixDouble33 C_ODO;
439  C_ODO(0,0) =square( o.gaussianModel.minStdXY + o.gaussianModel.a1*Al + o.gaussianModel.a2*fabs( odometryIncrement.phi() ) );
440  C_ODO(1,1) =square( o.gaussianModel.minStdXY + o.gaussianModel.a1*Al + o.gaussianModel.a2*fabs( odometryIncrement.phi() ) );
441  C_ODO(2,2) =square( o.gaussianModel.minStdPHI + o.gaussianModel.a3*Al + o.gaussianModel.a4*fabs( odometryIncrement.phi() ) );
442 
443  // Build the transformation matrix:
444  CMatrixDouble33 H;
445 
446  double cos_Aphi_2 = cos( odometryIncrement.phi()/2 );
447  double sin_Aphi_2 = sin( odometryIncrement.phi()/2 );
448 
449  H(0,0) = H(1,1) = cos_Aphi_2 ;
450  H(0,1) = - (H(1,0) = sin_Aphi_2);
451  H(2,2) = 1;
452 
453  // Build the Jacobian matrix:
454  CMatrixDouble33 J = H;
455  J(0,2) = -sin_Aphi_2 * ODO_INCR(0,0) - cos_Aphi_2 * ODO_INCR(1,0);
456  J(1,2) = cos_Aphi_2 * ODO_INCR(0,0) - sin_Aphi_2 * ODO_INCR(1,0);
457 
458  // The mean:
459  aux->mean = odometryIncrement;
460 
461  // The covariance:
462  J.multiply_HCHt( C_ODO, aux->cov );
463 
464 }
465 
466 /*---------------------------------------------------------------
467  computeFromOdometry_modelThrun
468  ---------------------------------------------------------------*/
470  const CPose2D &odometryIncrement,
471  const TMotionModelOptions &o
472  )
473 {
474  // The Gaussian PDF:
475  // ---------------------------
476  CPosePDFParticles *aux;
477  static CPose2D nullPose(0,0,0);
478 
479  poseChange = CPosePDFParticles::Create();
480  aux = static_cast<CPosePDFParticles*>( poseChange.pointer() );
481 
482  // Set the number of particles:
483  aux->resetDeterministic(nullPose, o.thrunModel.nParticlesCount );
484 
485  // ---------------------------------------------------------------------------------------------
486  // The following is an implementation from Thrun et al.'s book (Probabilistic Robotics),
487  // page 136. Here "odometryIncrement" actually represents the incremental odometry difference.
488  // ---------------------------------------------------------------------------------------------
489 
490  // The increments in odometry:
491  float Arot1 = ( odometryIncrement.y()!=0 || odometryIncrement.x()!=0) ?
492  atan2( odometryIncrement.y(), odometryIncrement.x() ) : 0;
493  float Atrans = odometryIncrement.norm();
494  float Arot2 = math::wrapToPi( odometryIncrement.phi() - Arot1 );
495 
496  // Draw samples:
497  for (size_t i=0;i<o.thrunModel.nParticlesCount;i++)
498  {
499  float Arot1_draw = Arot1 - (o.thrunModel.alfa1_rot_rot*fabs(Arot1)+o.thrunModel.alfa2_rot_trans*Atrans) * randomGenerator.drawGaussian1D_normalized();
500  float Atrans_draw = Atrans - (o.thrunModel.alfa3_trans_trans*Atrans+o.thrunModel.alfa4_trans_rot*(fabs(Arot1)+fabs(Arot2))) * randomGenerator.drawGaussian1D_normalized();
501  float Arot2_draw = Arot2 - (o.thrunModel.alfa1_rot_rot*fabs(Arot2)+o.thrunModel.alfa2_rot_trans*Atrans) * randomGenerator.drawGaussian1D_normalized();
502 
503  // Output:
507  aux->m_particles[i].d->normalizePhi();
508  }
509 }
510 
511 /*---------------------------------------------------------------
512  drawSingleSample
513  ---------------------------------------------------------------*/
515 {
516  // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
517  // the parameters to draw new samples:
519  {
521  drawSingleSample_modelGaussian( outSample );
522  else drawSingleSample_modelThrun( outSample );
523  }
524  else
525  {
526  // If is not odometry, just employ the stored distribution:
527  poseChange->drawSingleSample( outSample );
528  }
529 
530 }
531 
532 /*---------------------------------------------------------------
533  drawSingleSample_modelGaussian
534  ---------------------------------------------------------------*/
536  CPose2D &outSample ) const
537 {
538  // In the Gaussian case it is more efficient just to
539  // draw a sample from the already computed PDF:
540  poseChange->drawSingleSample( outSample );
541 }
542 
543 /*---------------------------------------------------------------
544  drawSingleSample_modelThrun
545  ---------------------------------------------------------------*/
547  CPose2D &outSample ) const
548 {
549  // ---------------------------------------------------------------------------------------------
550  // The following is an implementation from Thrun et al.'s book (Probabilistic Robotics),
551  // page 136. Here "odometryIncrement" actually represents the incremental odometry difference.
552  // ---------------------------------------------------------------------------------------------
553 
554  // The increments in odometry:
555  float Arot1 = ( rawOdometryIncrementReading.y()!=0 || rawOdometryIncrementReading.x()!=0) ?
557  float Atrans = rawOdometryIncrementReading.norm();
558  float Arot2 = math::wrapToPi( rawOdometryIncrementReading.phi() - Arot1 );
559 
563 
564  // Output:
565  outSample.x( Atrans_draw * cos( Arot1_draw ) + motionModelConfiguration.thrunModel.additional_std_XY * randomGenerator.drawGaussian1D_normalized() );
566  outSample.y( Atrans_draw * sin( Arot1_draw ) + motionModelConfiguration.thrunModel.additional_std_XY * randomGenerator.drawGaussian1D_normalized() );
568  outSample.normalizePhi();
569 }
570 
571 /*---------------------------------------------------------------
572  prepareFastDrawSingleSamples
573  ---------------------------------------------------------------*/
575 {
576  // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
577  // the parameters to draw new samples:
579  {
583  }
584 }
585 
586 /*---------------------------------------------------------------
587  fastDrawSingleSample
588  ---------------------------------------------------------------*/
590 {
591  // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
592  // the parameters to draw new samples:
594  {
597  else fastDrawSingleSample_modelThrun( outSample );
598  }
599  else
600  {
601  // If is not odometry, just employ the stored distribution:
602  poseChange->drawSingleSample( outSample );
603  }
604 }
605 
606 /*---------------------------------------------------------------
607  prepareFastDrawSingleSample_modelGaussian
608  ---------------------------------------------------------------*/
610 {
611  MRPT_START
612 
614 
615  CMatrixDouble33 D;
616 
617  const CPosePDFGaussian* gPdf = static_cast<const CPosePDFGaussian*> (poseChange.pointer());
618  const CMatrixDouble33 &cov = gPdf->cov;
619 
620  m_fastDrawGauss_M = gPdf->mean;
621 
622  /** Computes the eigenvalues/eigenvector decomposition of this matrix,
623  * so that: M = Z · D · Z<sup>T</sup>, where columns in Z are the
624  * eigenvectors and the diagonal matrix D contains the eigenvalues
625  * as diagonal elements, sorted in <i>ascending</i> order.
626  */
627  cov.eigenVectors( m_fastDrawGauss_Z, D );
628 
629  // Scale eigenvectors with eigenvalues:
630  D = D.array().sqrt().matrix();
632 
633  MRPT_END
634 }
635 
636 /*---------------------------------------------------------------
637  prepareFastDrawSingleSample_modelThrun
638  ---------------------------------------------------------------*/
640 {
641 }
642 
643 /*---------------------------------------------------------------
644  prepareFastDrawSingleSample_modelThrun
645  ---------------------------------------------------------------*/
647 {
648  CVectorFloat rndVector(3,0);
649  for (size_t i=0;i<3;i++)
650  {
652  for (size_t d=0;d<3;d++)
653  rndVector[d]+= ( m_fastDrawGauss_Z.get_unsafe(d,i)*rnd );
654  }
655 
656  outSample = CPose2D(
657  m_fastDrawGauss_M.x() + rndVector[0],
658  m_fastDrawGauss_M.y() + rndVector[1],
659  m_fastDrawGauss_M.phi() + rndVector[2] );
660 }
661 
662 /*---------------------------------------------------------------
663  prepareFastDrawSingleSample_modelThrun
664  ---------------------------------------------------------------*/
666 {
667  drawSingleSample_modelThrun( outSample );
668 }
struct OBS_IMPEXP mrpt::obs::CActionRobotMovement2D::TMotionModelOptions motionModelConfiguration
A namespace of pseudo-random numbers genrators of diferent distributions.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
mrpt::utils::poly_ptr_ptr< mrpt::poses::CPosePDFPtr > poseChange
The 2D pose change probabilistic estimation.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
CPose2D mean
The mean value.
CMatrixFixedNumeric< double, 3, 1 > CMatrixDouble31
Definition: eigen_frwds.h:53
TEstimationMethod estimationMethod
This fields indicates the way in which this estimation was obtained.
void drawSingleSample(mrpt::poses::CPose2D &outSample) const
Using this method instead of "poseChange->drawSingleSample()" may be more efficient in most situation...
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 drawSingleSample_modelThrun(mrpt::poses::CPose2D &outSample) const
The sample generator for the model "computeFromOdometry_modelThrun", internally called when the user ...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
T::value_type * pointer()
Definition: poly_ptr_ptr.h:69
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
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...
STL namespace.
The parameter to be passed to "computeFromOdometry".
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...
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
double drawGaussian1D_normalized(double *likelihood=NULL)
Generate a normalized (mean=0, std=1) normally distributed sample.
void drawSingleSample_modelGaussian(mrpt::poses::CPose2D &outSample) const
The sample generator for the model "computeFromOdometry_modelGaussian", internally called when the us...
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
unsigned char uint8_t
Definition: rptypes.h:43
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Represents a probabilistic 2D movement of the robot mobile base.
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.
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::math::CMatrixDouble33 m_fastDrawGauss_Z
Auxiliary matrix.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
#define MRPT_END
void fastDrawSingleSample(mrpt::poses::CPose2D &outSample) const
Faster version than "drawSingleSample", but requires a previous call to "prepareFastDrawSingleSamples...
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:135
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:174
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
float additional_std_XY
An additional noise added to the thrun model (std.
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&#39;s ...
void fastDrawSingleSample_modelThrun(mrpt::poses::CPose2D &outSample) const
Internal use.
This namespace contains representation of robot actions and observations.
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...
TEstimationMethod
A list of posible ways for estimating the content of a CActionRobotMovement2D object.
int version
Definition: mrpt_jpeglib.h:898
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
#define DEG2RAD
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
TDrawSampleMotionModel modelSelection
The model to be used.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
void fastDrawSingleSample_modelGaussian(mrpt::poses::CPose2D &outSample) const
Internal use.
Declares a class for storing a robot action.
Definition: obs/CAction.h:33
void prepareFastDrawSingleSamples() const
Call this before calling a high number of times "fastDrawSingleSample", which is much faster than "dr...
#define MRPT_START
__int32 int32_t
Definition: rptypes.h:48
#define RAD2DEG
void prepareFastDrawSingleSample_modelGaussian() const
Internal use.
void prepareFastDrawSingleSample_modelThrun() const
Internal use.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
const float R
void resetDeterministic(const CPose2D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose...
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:93
uint32_t nParticlesCount
The default number of particles to generate in a internal representation (anyway you can draw as many...
GLuint in
Definition: glext.h:6301
#define ASSERT_(f)
GLenum GLint GLint y
Definition: glext.h:3516
struct OBS_IMPEXP mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel gaussianModel
GLenum GLint x
Definition: glext.h:3516
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
struct OBS_IMPEXP mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel thrunModel
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
Definition: CPose2D.cpp:287
mrpt::poses::CPose2D rawOdometryIncrementReading
This is the raw odometry reading, and only is used when "estimationMethod" is "TEstimationMethod::emO...



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019