Main MRPT website > C++ reference for MRPT 1.9.9
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-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 
16 #include <mrpt/random.h>
18 #include <mrpt/math/wrap2pi.h>
19 
20 using namespace mrpt::obs;
21 using namespace mrpt::poses;
22 using namespace mrpt::math;
23 using namespace mrpt::random;
24 using namespace std;
25 
27 
28 // Note: dont move this to the .h, to avoid having to include the definition of
29 // the full pose type
31  : poseChange(mrpt::poses::CPosePDFGaussian::Create())
32 {
33 }
34 
38 {
39  out.WriteAs<uint32_t>(estimationMethod);
40  // Added in version 2:
41  // If the estimation method is emOdometry, save the rawOdo + config data
42  // instead of
43  // the PDF itself:
44  if (estimationMethod == emOdometry)
45  {
46  // The odometry data:
47  out << rawOdometryIncrementReading;
48  out.WriteAs<uint32_t>(motionModelConfiguration.modelSelection);
49  out << motionModelConfiguration.gaussianModel.a1
50  << motionModelConfiguration.gaussianModel.a2
51  << motionModelConfiguration.gaussianModel.a3
52  << motionModelConfiguration.gaussianModel.a4
53  << motionModelConfiguration.gaussianModel.minStdXY
54  << motionModelConfiguration.gaussianModel.minStdPHI;
55 
56  out << motionModelConfiguration.thrunModel.nParticlesCount
57  << motionModelConfiguration.thrunModel.alfa1_rot_rot
58  << motionModelConfiguration.thrunModel.alfa2_rot_trans
59  << motionModelConfiguration.thrunModel.alfa3_trans_trans
60  << motionModelConfiguration.thrunModel.alfa4_trans_rot
61  << motionModelConfiguration.thrunModel.additional_std_XY
62  << motionModelConfiguration.thrunModel.additional_std_phi;
63  }
64  else
65  {
66  // The PDF:
67  out << (*poseChange);
68  }
69 
70  // Added in version 1:
71  out << hasVelocities;
72  if (hasVelocities) out << velocityLocal; // v7
73 
74  out << hasEncodersInfo;
75  if (hasEncodersInfo)
76  out << encoderLeftTicks << encoderRightTicks; // added if() in v7
77 
78  // Added in version 6
79  out << timestamp;
80 }
81 
84 {
85  switch (version)
86  {
87  case 4:
88  case 5:
89  case 6:
90  case 7:
91  {
92  int32_t i;
93 
94  // Read the estimation method first:
95  in >> i;
96  estimationMethod = static_cast<TEstimationMethod>(i);
97 
98  // If the estimation method is emOdometry, save the rawOdo + config
99  // data instead of
100  // the PDF itself:
101  if (estimationMethod == emOdometry)
102  {
103  // The odometry data:
104  in >> rawOdometryIncrementReading;
105 
106  in >> i;
107  motionModelConfiguration.modelSelection =
108  static_cast<TDrawSampleMotionModel>(i);
109 
110  in >> motionModelConfiguration.gaussianModel.a1 >>
111  motionModelConfiguration.gaussianModel.a2 >>
112  motionModelConfiguration.gaussianModel.a3 >>
113  motionModelConfiguration.gaussianModel.a4 >>
114  motionModelConfiguration.gaussianModel.minStdXY >>
115  motionModelConfiguration.gaussianModel.minStdPHI;
116 
117  in >> i;
118  motionModelConfiguration.thrunModel.nParticlesCount = i;
119  in >> motionModelConfiguration.thrunModel.alfa1_rot_rot >>
120  motionModelConfiguration.thrunModel.alfa2_rot_trans >>
121  motionModelConfiguration.thrunModel.alfa3_trans_trans >>
122  motionModelConfiguration.thrunModel.alfa4_trans_rot;
123 
124  if (version >= 5)
125  {
126  in >>
127  motionModelConfiguration.thrunModel.additional_std_XY >>
128  motionModelConfiguration.thrunModel.additional_std_phi;
129  }
130  else
131  {
132  motionModelConfiguration.thrunModel.additional_std_XY =
133  motionModelConfiguration.thrunModel.additional_std_phi =
134  0;
135  }
136 
137  // Re-build the PDF:
138  computeFromOdometry(
139  rawOdometryIncrementReading, motionModelConfiguration);
140  }
141  else
142  {
143  // Read the PDF directly from the stream:
144  CPosePDF::Ptr pc;
145  in >> pc;
146  poseChange = pc;
147  }
148 
149  in >> hasVelocities;
150  if (version >= 7)
151  {
152  if (hasVelocities) in >> velocityLocal;
153  }
154  else
155  {
156  float velocityLin, velocityAng;
157  in >> velocityLin >> velocityAng;
158  velocityLocal.vx = velocityLin;
159  velocityLocal.vy = .0f;
160  velocityLocal.omega = velocityAng;
161  }
162  in >> hasEncodersInfo;
163  if (version < 7 || hasEncodersInfo)
164  {
165  in >> i;
166  encoderLeftTicks = i;
167  in >> i;
168  encoderRightTicks = i;
169  }
170  else
171  {
172  encoderLeftTicks = 0;
173  encoderRightTicks = 0;
174  }
175 
176  if (version >= 6)
177  in >> timestamp;
178  else
179  timestamp = INVALID_TIMESTAMP;
180  }
181  break;
182 
183  case 3:
184  {
185  int32_t i;
186 
187  // Read the estimation method first:
188  in >> i;
189  estimationMethod = static_cast<TEstimationMethod>(i);
190 
191  // If the estimation method is emOdometry, save the rawOdo + config
192  // data instead of
193  // the PDF itself:
194  if (estimationMethod == emOdometry)
195  {
196  // The odometry data:
197  in >> rawOdometryIncrementReading;
198 
199  in >> i;
200  motionModelConfiguration.modelSelection =
201  static_cast<TDrawSampleMotionModel>(i);
202 
203  float dum1, dum2, dum3;
204 
205  in >> dum1 >> dum2 >> dum3 >>
206  motionModelConfiguration.gaussianModel.minStdXY >>
207  motionModelConfiguration.gaussianModel.minStdPHI;
208 
209  // Leave the default values for a1,a2,a3,a4:
210  in >> i;
211  motionModelConfiguration.thrunModel.nParticlesCount = i;
212  in >> motionModelConfiguration.thrunModel.alfa1_rot_rot;
213  in >> motionModelConfiguration.thrunModel.alfa2_rot_trans >>
214  motionModelConfiguration.thrunModel.alfa3_trans_trans >>
215  motionModelConfiguration.thrunModel.alfa4_trans_rot;
216 
217  // Re-build the PDF:
218  computeFromOdometry(
219  rawOdometryIncrementReading, motionModelConfiguration);
220  }
221  else
222  {
223  // Read the PDF directly from the stream:
224  CPosePDF::Ptr pc;
225  in >> pc;
226  poseChange = pc;
227  }
228 
229  in >> hasVelocities;
230  {
231  float velocityLin, velocityAng;
232  in >> velocityLin >> velocityAng;
233  velocityLocal.vx = velocityLin;
234  velocityLocal.vy = .0f;
235  velocityLocal.omega = velocityAng;
236  }
237  in >> hasEncodersInfo;
238 
239  in >> i;
240  encoderLeftTicks = i;
241  in >> i;
242  encoderRightTicks = i;
243  }
244  break;
245 
246  case 2:
247  {
248  int32_t i;
249 
250  // Read the estimation method first:
251  in >> i;
252  estimationMethod = static_cast<TEstimationMethod>(i);
253 
254  // If the estimation method is emOdometry, save the rawOdo + config
255  // data instead of
256  // the PDF itself:
257  if (estimationMethod == emOdometry)
258  {
259  // The odometry data:
260  in >> rawOdometryIncrementReading;
261 
262  // TMotionModelOptions_OLD_VERSION_2 dummy;
263  // in.ReadBuffer( &dummy,
264  // sizeof(TMotionModelOptions_OLD_VERSION_2) );
265  uint8_t dummy[44];
266  in.ReadBuffer(&dummy, sizeof(dummy));
267 
268  motionModelConfiguration = TMotionModelOptions();
269 
270  // Re-build the PDF:
271  computeFromOdometry(
272  rawOdometryIncrementReading, motionModelConfiguration);
273  }
274  else
275  {
276  // Read the PDF directly from the stream:
277  CPosePDF::Ptr 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;
292  encoderLeftTicks = i;
293  in >> i;
294  encoderRightTicks = i;
295  }
296  break;
297  case 0:
298  case 1:
299  {
300  int32_t i;
301  {
302  CPosePDF::Ptr pc;
303  in >> pc;
304  poseChange = pc;
305  }
306  in >> i;
307 
308  // copy:
309  estimationMethod = static_cast<TEstimationMethod>(i);
310 
311  // Simulate the "rawOdometryIncrementReading" as the mean value:
312  if (estimationMethod == emOdometry)
313  poseChange->getMean(rawOdometryIncrementReading);
314  else
315  rawOdometryIncrementReading = CPose2D(0, 0, 0);
316 
317  if (version >= 1)
318  {
319  in >> hasVelocities;
320  {
321  float velocityLin, velocityAng;
322  in >> velocityLin >> velocityAng;
323  velocityLocal.vx = velocityLin;
324  velocityLocal.vy = .0f;
325  velocityLocal.omega = velocityAng;
326  }
327  in >> hasEncodersInfo;
328  in >> i;
329  encoderLeftTicks = i;
330  in >> i;
331  encoderRightTicks = i;
332  }
333  else
334  {
335  // Default values:
336  hasVelocities = hasEncodersInfo = false;
337  encoderLeftTicks = encoderRightTicks = 0;
338  velocityLocal = mrpt::math::TTwist2D(.0, .0, .0);
339  }
340  }
341  break;
342  default:
344  };
345 }
346 
347 /*---------------------------------------------------------------
348  computeFromEncoders
349  ---------------------------------------------------------------*/
351  double K_left, double K_right, double D)
352 {
353  if (hasEncodersInfo)
354  {
355  const double As =
356  0.5 * (K_right * encoderRightTicks + K_left * encoderLeftTicks);
357  const double Aphi =
358  (K_right * encoderRightTicks - K_left * encoderLeftTicks) / D;
359 
360  double x, y;
361  if (Aphi != 0)
362  {
363  const double R = As / Aphi;
364  x = R * sin(Aphi);
365  y = R * (1 - cos(Aphi));
366  }
367  else
368  {
369  x = As;
370  y = 0;
371  }
372 
373  // Build the whole PDF with the current parameters:
374  computeFromOdometry(CPose2D(x, y, Aphi), motionModelConfiguration);
375  }
376 }
377 
378 /*---------------------------------------------------------------
379  computeFromOdometry
380  ---------------------------------------------------------------*/
382  const CPose2D& odometryIncrement, const TMotionModelOptions& options)
383 {
384  // Set the members data:
385  estimationMethod = emOdometry;
386  rawOdometryIncrementReading = odometryIncrement;
387  motionModelConfiguration = options;
388 
389  if (options.modelSelection == mmGaussian)
390  computeFromOdometry_modelGaussian(odometryIncrement, options);
391  else
392  computeFromOdometry_modelThrun(odometryIncrement, options);
393 }
394 
395 /*---------------------------------------------------------------
396  TMotionModelOptions
397  ---------------------------------------------------------------*/
399  : modelSelection(CActionRobotMovement2D::mmGaussian),
400  gaussianModel(),
401  thrunModel()
402 {
403  gaussianModel.a1 = 0.01f;
404  gaussianModel.a2 = RAD2DEG(0.001f);
405  gaussianModel.a3 = DEG2RAD(1.0f);
406  gaussianModel.a4 = 0.05f;
407 
408  gaussianModel.minStdXY = 0.01f;
410 
412  thrunModel.alfa1_rot_rot = 0.05f;
416  thrunModel.additional_std_XY = 0.001f;
418 }
419 
420 /*---------------------------------------------------------------
421  computeFromOdometry_modelGaussian
422  ---------------------------------------------------------------*/
424  const CPose2D& odometryIncrement, const TMotionModelOptions& o)
425 {
426  // The Gaussian PDF:
427  // ---------------------------
428  poseChange = mrpt::make_aligned_shared<CPosePDFGaussian>();
429  CPosePDFGaussian* aux;
430  aux = static_cast<CPosePDFGaussian*>(poseChange.get());
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(
441  o.gaussianModel.a2 * fabs(odometryIncrement.phi()));
442  C_ODO(1, 1) = square(
444  o.gaussianModel.a2 * fabs(odometryIncrement.phi()));
445  C_ODO(2, 2) = square(
447  o.gaussianModel.a4 * fabs(odometryIncrement.phi()));
448 
449  // Build the transformation matrix:
450  CMatrixDouble33 H;
451 
452  double cos_Aphi_2 = cos(odometryIncrement.phi() / 2);
453  double sin_Aphi_2 = sin(odometryIncrement.phi() / 2);
454 
455  H(0, 0) = H(1, 1) = cos_Aphi_2;
456  H(0, 1) = -(H(1, 0) = sin_Aphi_2);
457  H(2, 2) = 1;
458 
459  // Build the Jacobian matrix:
460  CMatrixDouble33 J = H;
461  J(0, 2) = -sin_Aphi_2 * ODO_INCR(0, 0) - cos_Aphi_2 * ODO_INCR(1, 0);
462  J(1, 2) = cos_Aphi_2 * ODO_INCR(0, 0) - sin_Aphi_2 * ODO_INCR(1, 0);
463 
464  // The mean:
465  aux->mean = odometryIncrement;
466 
467  // The covariance:
468  J.multiply_HCHt(C_ODO, aux->cov);
469 }
470 
471 /*---------------------------------------------------------------
472  computeFromOdometry_modelThrun
473  ---------------------------------------------------------------*/
475  const CPose2D& odometryIncrement, const TMotionModelOptions& o)
476 {
477  // The Gaussian PDF:
478  // ---------------------------
479  CPosePDFParticles* aux;
480  const mrpt::math::TPose2D nullPose(0, 0, 0);
481 
482  poseChange = mrpt::make_aligned_shared<CPosePDFParticles>();
483  aux = static_cast<CPosePDFParticles*>(poseChange.get());
484 
485  // Set the number of particles:
487 
488  // ---------------------------------------------------------------------------------------------
489  // The following is an implementation from Thrun et al.'s book
490  // (Probabilistic Robotics),
491  // page 136. Here "odometryIncrement" actually represents the incremental
492  // odometry difference.
493  // ---------------------------------------------------------------------------------------------
494 
495  // The increments in odometry:
496  float Arot1 = (odometryIncrement.y() != 0 || odometryIncrement.x() != 0)
497  ? atan2(odometryIncrement.y(), odometryIncrement.x())
498  : 0;
499  float Atrans = odometryIncrement.norm();
500  float Arot2 = math::wrapToPi(odometryIncrement.phi() - Arot1);
501 
502  // Draw samples:
503  for (size_t i = 0; i < o.thrunModel.nParticlesCount; i++)
504  {
505  float Arot1_draw = Arot1 -
506  (o.thrunModel.alfa1_rot_rot * fabs(Arot1) +
507  o.thrunModel.alfa2_rot_trans * Atrans) *
509  float Atrans_draw =
510  Atrans -
511  (o.thrunModel.alfa3_trans_trans * Atrans +
512  o.thrunModel.alfa4_trans_rot * (fabs(Arot1) + fabs(Arot2))) *
514  float Arot2_draw = Arot2 -
515  (o.thrunModel.alfa1_rot_rot * fabs(Arot2) +
516  o.thrunModel.alfa2_rot_trans * Atrans) *
518 
519  // Output:
520  aux->m_particles[i].d.x =
521  Atrans_draw * cos(Arot1_draw) +
524  aux->m_particles[i].d.y =
525  Atrans_draw * sin(Arot1_draw) +
528  aux->m_particles[i].d.phi =
529  Arot1_draw + Arot2_draw +
532  aux->m_particles[i].d.normalizePhi();
533  }
534 }
535 
536 /*---------------------------------------------------------------
537  drawSingleSample
538  ---------------------------------------------------------------*/
540 {
541  // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
542  // the parameters to draw new samples:
544  {
547  else
548  drawSingleSample_modelThrun(outSample);
549  }
550  else
551  {
552  // If is not odometry, just employ the stored distribution:
553  poseChange->drawSingleSample(outSample);
554  }
555 }
556 
557 /*---------------------------------------------------------------
558  drawSingleSample_modelGaussian
559  ---------------------------------------------------------------*/
561  CPose2D& outSample) const
562 {
563  // In the Gaussian case it is more efficient just to
564  // draw a sample from the already computed PDF:
565  poseChange->drawSingleSample(outSample);
566 }
567 
568 /*---------------------------------------------------------------
569  drawSingleSample_modelThrun
570  ---------------------------------------------------------------*/
572  CPose2D& outSample) const
573 {
574  // ---------------------------------------------------------------------------------------------
575  // The following is an implementation from Thrun et al.'s book
576  // (Probabilistic Robotics),
577  // page 136. Here "odometryIncrement" actually represents the incremental
578  // odometry difference.
579  // ---------------------------------------------------------------------------------------------
580 
581  // The increments in odometry:
582  float Arot1 = (rawOdometryIncrementReading.y() != 0 ||
584  ? atan2(
587  : 0;
588  float Atrans = rawOdometryIncrementReading.norm();
589  float Arot2 = math::wrapToPi(rawOdometryIncrementReading.phi() - Arot1);
590 
591  float Arot1_draw =
592  Arot1 -
596  float Atrans_draw =
597  Atrans -
600  (fabs(Arot1) + fabs(Arot2))) *
602  float Arot2_draw =
603  Arot2 -
607 
608  // Output:
609  outSample.x(
610  Atrans_draw * cos(Arot1_draw) +
612  getRandomGenerator().drawGaussian1D_normalized());
613  outSample.y(
614  Atrans_draw * sin(Arot1_draw) +
616  getRandomGenerator().drawGaussian1D_normalized());
617  outSample.phi(
618  Arot1_draw + Arot2_draw +
620  getRandomGenerator().drawGaussian1D_normalized());
621  outSample.normalizePhi();
622 }
623 
624 /*---------------------------------------------------------------
625  prepareFastDrawSingleSamples
626  ---------------------------------------------------------------*/
628 {
629  // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
630  // the parameters to draw new samples:
632  {
635  else
637  }
638 }
639 
640 /*---------------------------------------------------------------
641  fastDrawSingleSample
642  ---------------------------------------------------------------*/
644 {
645  // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
646  // the parameters to draw new samples:
648  {
651  else
653  }
654  else
655  {
656  // If is not odometry, just employ the stored distribution:
657  poseChange->drawSingleSample(outSample);
658  }
659 }
660 
661 /*---------------------------------------------------------------
662  prepareFastDrawSingleSample_modelGaussian
663  ---------------------------------------------------------------*/
665 {
666  MRPT_START
667 
669 
670  CMatrixDouble33 D;
671 
672  const CPosePDFGaussian* gPdf =
673  static_cast<const CPosePDFGaussian*>(poseChange.get());
674  const CMatrixDouble33& cov = gPdf->cov;
675 
676  m_fastDrawGauss_M = gPdf->mean;
677 
678  /** Computes the eigenvalues/eigenvector decomposition of this matrix,
679  * so that: M = Z · D · Z<sup>T</sup>, where columns in Z are the
680  * eigenvectors and the diagonal matrix D contains the eigenvalues
681  * as diagonal elements, sorted in <i>ascending</i> order.
682  */
683  cov.eigenVectors(m_fastDrawGauss_Z, D);
684 
685  // Scale eigenvectors with eigenvalues:
686  D = D.array().sqrt().matrix();
688 
689  MRPT_END
690 }
691 
692 /*---------------------------------------------------------------
693  prepareFastDrawSingleSample_modelThrun
694  ---------------------------------------------------------------*/
696 /*---------------------------------------------------------------
697  prepareFastDrawSingleSample_modelThrun
698  ---------------------------------------------------------------*/
700  CPose2D& outSample) const
701 {
702  CVectorFloat rndVector(3, 0);
703  for (size_t i = 0; i < 3; i++)
704  {
706  for (size_t d = 0; d < 3; d++)
707  rndVector[d] += (m_fastDrawGauss_Z.get_unsafe(d, i) * rnd);
708  }
709 
710  outSample = CPose2D(
711  m_fastDrawGauss_M.x() + rndVector[0],
712  m_fastDrawGauss_M.y() + rndVector[1],
713  m_fastDrawGauss_M.phi() + rndVector[2]);
714 }
715 
716 /*---------------------------------------------------------------
717  prepareFastDrawSingleSample_modelThrun
718  ---------------------------------------------------------------*/
720  CPose2D& outSample) const
721 {
722  drawSingleSample_modelThrun(outSample);
723 }
A namespace of pseudo-random numbers generators of diferent distributions.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#define MRPT_START
Definition: exceptions.h:262
CPose2D mean
The mean value.
double RAD2DEG(const double x)
Radians to degrees.
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 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.
double DEG2RAD(const double x)
Degrees to radians.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
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.
void WriteAs(const TYPE_FROM_ACTUAL &value)
Definition: CArchive.h:152
The parameter to be passed to "computeFromOdometry".
void resetDeterministic(const mrpt::math::TPose2D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose...
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.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
void drawSingleSample_modelGaussian(mrpt::poses::CPose2D &outSample) const
The sample generator for the model "computeFromOdometry_modelGaussian", internally called when the us...
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
Represents a probabilistic 2D movement of the robot mobile base.
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
CMatrixFixedNumeric< double, 3, 1 > CMatrixDouble31
Definition: eigen_frwds.h:62
mrpt::math::CMatrixDouble33 m_fastDrawGauss_Z
Auxiliary matrix.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
void fastDrawSingleSample(mrpt::poses::CPose2D &outSample) const
Faster version than "drawSingleSample", but requires a previous call to "prepareFastDrawSingleSamples...
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:253
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.
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
TDrawSampleMotionModel modelSelection
The model to be used.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:15
void fastDrawSingleSample_modelGaussian(mrpt::poses::CPose2D &outSample) const
Internal use.
struct mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel gaussianModel
Declares a class for storing a robot action.
Definition: CAction.h:27
void prepareFastDrawSingleSamples() const
Call this before calling a high number of times "fastDrawSingleSample", which is much faster than "dr...
__int32 int32_t
Definition: rptypes.h:46
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
void prepareFastDrawSingleSample_modelGaussian() const
Internal use.
struct mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_ThrunModel thrunModel
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDF::Ptr > poseChange
The 2D pose change probabilistic estimation.
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:40
const float R
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:82
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:103
uint32_t nParticlesCount
The default number of particles to generate in a internal representation (anyway you can draw as many...
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
Lightweight 2D pose.
GLenum GLint GLint y
Definition: glext.h:3538
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:148
GLenum GLint x
Definition: glext.h:3538
unsigned __int32 uint32_t
Definition: rptypes.h:47
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
struct mrpt::obs::CActionRobotMovement2D::TMotionModelOptions motionModelConfiguration
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
Definition: CPose2D.cpp:292
mrpt::poses::CPose2D rawOdometryIncrementReading
This is the raw odometry reading, and only is used when "estimationMethod" is "TEstimationMethod::emO...
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
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: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019