MRPT  1.9.9
CActionRobotMovement2D.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-2019, 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 
12 #include <mrpt/math/ops_matrices.h>
14 #include <mrpt/math/wrap2pi.h>
18 #include <mrpt/random.h>
20 #include <Eigen/Dense>
21 
22 using namespace mrpt::obs;
23 using namespace mrpt::poses;
24 using namespace mrpt::math;
25 using namespace mrpt::random;
26 using namespace std;
27 
29 
30 // Note: dont move this to the .h, to avoid having to include the definition of
31 // the full pose type
33  : poseChange(mrpt::poses::CPosePDFGaussian::Create())
34 {
35 }
36 
37 uint8_t CActionRobotMovement2D::serializeGetVersion() const { return 7; }
40 {
41  out.WriteAs<uint32_t>(estimationMethod);
42  // Added in version 2:
43  // If the estimation method is emOdometry, save the rawOdo + config data
44  // instead of
45  // the PDF itself:
46  if (estimationMethod == emOdometry)
47  {
48  // The odometry data:
49  out << rawOdometryIncrementReading;
50  out.WriteAs<uint32_t>(motionModelConfiguration.modelSelection);
51  const auto& gm = motionModelConfiguration.gaussianModel;
52  out.WriteAs<float>(gm.a1);
53  out.WriteAs<float>(gm.a2);
54  out.WriteAs<float>(gm.a3);
55  out.WriteAs<float>(gm.a4);
56  out.WriteAs<float>(gm.minStdXY);
57  out.WriteAs<float>(gm.minStdPHI);
58 
59  out << motionModelConfiguration.thrunModel.nParticlesCount
60  << motionModelConfiguration.thrunModel.alfa1_rot_rot
61  << motionModelConfiguration.thrunModel.alfa2_rot_trans
62  << motionModelConfiguration.thrunModel.alfa3_trans_trans
63  << motionModelConfiguration.thrunModel.alfa4_trans_rot
64  << motionModelConfiguration.thrunModel.additional_std_XY
65  << motionModelConfiguration.thrunModel.additional_std_phi;
66  }
67  else
68  {
69  // The PDF:
70  out << (*poseChange);
71  }
72 
73  // Added in version 1:
74  out << hasVelocities;
75  if (hasVelocities) out << velocityLocal; // v7
76 
77  out << hasEncodersInfo;
78  if (hasEncodersInfo)
79  out << encoderLeftTicks << encoderRightTicks; // added if() in v7
80 
81  // Added in version 6
82  out << timestamp;
83 }
84 
86  mrpt::serialization::CArchive& in, uint8_t version)
87 {
88  switch (version)
89  {
90  case 4:
91  case 5:
92  case 6:
93  case 7:
94  {
95  int32_t i;
96 
97  // Read the estimation method first:
98  in >> i;
99  estimationMethod = static_cast<TEstimationMethod>(i);
100 
101  // If the estimation method is emOdometry, save the rawOdo + config
102  // data instead of
103  // the PDF itself:
104  if (estimationMethod == emOdometry)
105  {
106  // The odometry data:
107  in >> rawOdometryIncrementReading;
108 
109  in >> i;
110  motionModelConfiguration.modelSelection =
111  static_cast<TDrawSampleMotionModel>(i);
112 
113  auto& gm = motionModelConfiguration.gaussianModel;
114  gm.a1 = in.ReadAs<float>();
115  gm.a2 = in.ReadAs<float>();
116  gm.a3 = in.ReadAs<float>();
117  gm.a4 = in.ReadAs<float>();
118  gm.minStdXY = in.ReadAs<float>();
119  gm.minStdPHI = in.ReadAs<float>();
120 
121  in >> i;
122  motionModelConfiguration.thrunModel.nParticlesCount = i;
123  in >> motionModelConfiguration.thrunModel.alfa1_rot_rot >>
124  motionModelConfiguration.thrunModel.alfa2_rot_trans >>
125  motionModelConfiguration.thrunModel.alfa3_trans_trans >>
126  motionModelConfiguration.thrunModel.alfa4_trans_rot;
127 
128  if (version >= 5)
129  {
130  in >>
131  motionModelConfiguration.thrunModel.additional_std_XY >>
132  motionModelConfiguration.thrunModel.additional_std_phi;
133  }
134  else
135  {
136  motionModelConfiguration.thrunModel.additional_std_XY =
137  motionModelConfiguration.thrunModel.additional_std_phi =
138  0;
139  }
140 
141  // Re-build the PDF:
142  computeFromOdometry(
143  rawOdometryIncrementReading, motionModelConfiguration);
144  }
145  else
146  {
147  // Read the PDF directly from the stream:
148  CPosePDF::Ptr pc;
149  in >> pc;
150  poseChange = pc;
151  }
152 
153  in >> hasVelocities;
154  if (version >= 7)
155  {
156  if (hasVelocities) in >> velocityLocal;
157  }
158  else
159  {
160  float velocityLin, velocityAng;
161  in >> velocityLin >> velocityAng;
162  velocityLocal.vx = velocityLin;
163  velocityLocal.vy = .0f;
164  velocityLocal.omega = velocityAng;
165  }
166  in >> hasEncodersInfo;
167  if (version < 7 || hasEncodersInfo)
168  {
169  in >> i;
170  encoderLeftTicks = i;
171  in >> i;
172  encoderRightTicks = i;
173  }
174  else
175  {
176  encoderLeftTicks = 0;
177  encoderRightTicks = 0;
178  }
179 
180  if (version >= 6)
181  in >> timestamp;
182  else
183  timestamp = INVALID_TIMESTAMP;
184  }
185  break;
186 
187  case 3:
188  {
189  int32_t i;
190 
191  // Read the estimation method first:
192  in >> i;
193  estimationMethod = static_cast<TEstimationMethod>(i);
194 
195  // If the estimation method is emOdometry, save the rawOdo + config
196  // data instead of
197  // the PDF itself:
198  if (estimationMethod == emOdometry)
199  {
200  // The odometry data:
201  in >> rawOdometryIncrementReading;
202 
203  in >> i;
204  motionModelConfiguration.modelSelection =
205  static_cast<TDrawSampleMotionModel>(i);
206 
207  float dum1, dum2, dum3;
208 
209  in >> dum1 >> dum2 >> dum3 >>
210  motionModelConfiguration.gaussianModel.minStdXY >>
211  motionModelConfiguration.gaussianModel.minStdPHI;
212 
213  // Leave the default values for a1,a2,a3,a4:
214  in >> i;
215  motionModelConfiguration.thrunModel.nParticlesCount = i;
216  in >> motionModelConfiguration.thrunModel.alfa1_rot_rot;
217  in >> motionModelConfiguration.thrunModel.alfa2_rot_trans >>
218  motionModelConfiguration.thrunModel.alfa3_trans_trans >>
219  motionModelConfiguration.thrunModel.alfa4_trans_rot;
220 
221  // Re-build the PDF:
222  computeFromOdometry(
223  rawOdometryIncrementReading, motionModelConfiguration);
224  }
225  else
226  {
227  // Read the PDF directly from the stream:
228  CPosePDF::Ptr pc;
229  in >> pc;
230  poseChange = pc;
231  }
232 
233  in >> hasVelocities;
234  {
235  float velocityLin, velocityAng;
236  in >> velocityLin >> velocityAng;
237  velocityLocal.vx = velocityLin;
238  velocityLocal.vy = .0f;
239  velocityLocal.omega = velocityAng;
240  }
241  in >> hasEncodersInfo;
242 
243  in >> i;
244  encoderLeftTicks = i;
245  in >> i;
246  encoderRightTicks = i;
247  }
248  break;
249 
250  case 2:
251  {
252  int32_t i;
253 
254  // Read the estimation method first:
255  in >> i;
256  estimationMethod = static_cast<TEstimationMethod>(i);
257 
258  // If the estimation method is emOdometry, save the rawOdo + config
259  // data instead of
260  // the PDF itself:
261  if (estimationMethod == emOdometry)
262  {
263  // The odometry data:
264  in >> rawOdometryIncrementReading;
265 
266  // TMotionModelOptions_OLD_VERSION_2 dummy;
267  // in.ReadBuffer( &dummy,
268  // sizeof(TMotionModelOptions_OLD_VERSION_2) );
269  uint8_t dummy[44];
270  in.ReadBuffer(&dummy, sizeof(dummy));
271 
272  motionModelConfiguration = TMotionModelOptions();
273 
274  // Re-build the PDF:
275  computeFromOdometry(
276  rawOdometryIncrementReading, motionModelConfiguration);
277  }
278  else
279  {
280  // Read the PDF directly from the stream:
281  CPosePDF::Ptr pc;
282  in >> pc;
283  poseChange = pc;
284  }
285 
286  in >> hasVelocities;
287  {
288  float velocityLin, velocityAng;
289  in >> velocityLin >> velocityAng;
290  velocityLocal.vx = velocityLin;
291  velocityLocal.vy = .0f;
292  velocityLocal.omega = velocityAng;
293  }
294  in >> hasEncodersInfo;
295  in >> i;
296  encoderLeftTicks = i;
297  in >> i;
298  encoderRightTicks = i;
299  }
300  break;
301  case 0:
302  case 1:
303  {
304  int32_t i;
305  {
306  CPosePDF::Ptr pc;
307  in >> pc;
308  poseChange = pc;
309  }
310  in >> i;
311 
312  // copy:
313  estimationMethod = static_cast<TEstimationMethod>(i);
314 
315  // Simulate the "rawOdometryIncrementReading" as the mean value:
316  if (estimationMethod == emOdometry)
317  poseChange->getMean(rawOdometryIncrementReading);
318  else
319  rawOdometryIncrementReading = CPose2D(0, 0, 0);
320 
321  if (version >= 1)
322  {
323  in >> hasVelocities;
324  {
325  float velocityLin, velocityAng;
326  in >> velocityLin >> velocityAng;
327  velocityLocal.vx = velocityLin;
328  velocityLocal.vy = .0f;
329  velocityLocal.omega = velocityAng;
330  }
331  in >> hasEncodersInfo;
332  in >> i;
333  encoderLeftTicks = i;
334  in >> i;
335  encoderRightTicks = i;
336  }
337  else
338  {
339  // Default values:
340  hasVelocities = hasEncodersInfo = false;
341  encoderLeftTicks = encoderRightTicks = 0;
342  velocityLocal = mrpt::math::TTwist2D(.0, .0, .0);
343  }
344  }
345  break;
346  default:
348  };
349 }
350 
351 /*---------------------------------------------------------------
352  computeFromEncoders
353  ---------------------------------------------------------------*/
355  double K_left, double K_right, double D)
356 {
357  if (hasEncodersInfo)
358  {
359  const double As =
360  0.5 * (K_right * encoderRightTicks + K_left * encoderLeftTicks);
361  const double Aphi =
362  (K_right * encoderRightTicks - K_left * encoderLeftTicks) / D;
363 
364  double x, y;
365  if (Aphi != 0)
366  {
367  const double R = As / Aphi;
368  x = R * sin(Aphi);
369  y = R * (1 - cos(Aphi));
370  }
371  else
372  {
373  x = As;
374  y = 0;
375  }
376 
377  // Build the whole PDF with the current parameters:
378  computeFromOdometry(CPose2D(x, y, Aphi), motionModelConfiguration);
379  }
380 }
381 
382 /*---------------------------------------------------------------
383  computeFromOdometry
384  ---------------------------------------------------------------*/
386  const CPose2D& odometryIncrement, const TMotionModelOptions& options)
387 {
388  // Set the members data:
389  estimationMethod = emOdometry;
390  rawOdometryIncrementReading = odometryIncrement;
391  motionModelConfiguration = options;
392 
393  if (options.modelSelection == mmGaussian)
394  computeFromOdometry_modelGaussian(odometryIncrement, options);
395  else
396  computeFromOdometry_modelThrun(odometryIncrement, options);
397 }
398 
399 /*---------------------------------------------------------------
400  computeFromOdometry_modelGaussian
401  ---------------------------------------------------------------*/
403  const CPose2D& odometryIncrement, const TMotionModelOptions& o)
404 {
405  // The Gaussian PDF:
406  // ---------------------------
407  poseChange = std::make_shared<CPosePDFGaussian>();
408  auto* aux = dynamic_cast<CPosePDFGaussian*>(poseChange.get());
409  ASSERT_(aux != nullptr);
410 
411  // See https://www.mrpt.org/Probabilistic_Motion_Models
412  // -----------------------------------
413 
414  // Build the odometry noise matrix:
415  double Al = odometryIncrement.norm();
416  auto ODO_INCR = CMatrixDouble31(odometryIncrement);
417  CMatrixDouble33 C_ODO;
418  C_ODO(0, 0) = square(
420  o.gaussianModel.a2 * fabs(odometryIncrement.phi()));
421  C_ODO(1, 1) = square(
423  o.gaussianModel.a2 * fabs(odometryIncrement.phi()));
424  C_ODO(2, 2) = square(
426  o.gaussianModel.a4 * fabs(odometryIncrement.phi()));
427 
428  // Build the transformation matrix:
429  CMatrixDouble33 H;
430 
431  double cos_Aphi_2 = cos(odometryIncrement.phi() / 2);
432  double sin_Aphi_2 = sin(odometryIncrement.phi() / 2);
433 
434  H(0, 0) = H(1, 1) = cos_Aphi_2;
435  H(0, 1) = -(H(1, 0) = sin_Aphi_2);
436  H(2, 2) = 1;
437 
438  // Build the Jacobian matrix:
439  CMatrixDouble33 J = H;
440  J(0, 2) = -sin_Aphi_2 * ODO_INCR(0, 0) - cos_Aphi_2 * ODO_INCR(1, 0);
441  J(1, 2) = cos_Aphi_2 * ODO_INCR(0, 0) - sin_Aphi_2 * ODO_INCR(1, 0);
442 
443  // The mean:
444  aux->mean = odometryIncrement;
445 
446  // The covariance:
447  aux->cov = mrpt::math::multiply_HCHt(J, C_ODO);
448 }
449 
450 /*---------------------------------------------------------------
451  computeFromOdometry_modelThrun
452  ---------------------------------------------------------------*/
454  const CPose2D& odometryIncrement, const TMotionModelOptions& o)
455 {
456  // The Gaussian PDF:
457  // ---------------------------
458  const mrpt::math::TPose2D nullPose(0, 0, 0);
459 
460  poseChange = CPosePDFParticles::Create();
461  auto* aux = dynamic_cast<CPosePDFParticles*>(poseChange.get());
462  ASSERT_(aux != nullptr);
463 
464  // Set the number of particles:
465  aux->resetDeterministic(nullPose, o.thrunModel.nParticlesCount);
466 
467  // ---------------------------------------------------------------------------------------------
468  // The following is an implementation from Thrun et al.'s book
469  // (Probabilistic Robotics),
470  // page 136. Here "odometryIncrement" actually represents the incremental
471  // odometry difference.
472  // ---------------------------------------------------------------------------------------------
473 
474  // The increments in odometry:
475  float Arot1 = (odometryIncrement.y() != 0 || odometryIncrement.x() != 0)
476  ? atan2(odometryIncrement.y(), odometryIncrement.x())
477  : 0;
478  float Atrans = odometryIncrement.norm();
479  float Arot2 = math::wrapToPi(odometryIncrement.phi() - Arot1);
480 
481  // Draw samples:
482  for (size_t i = 0; i < o.thrunModel.nParticlesCount; i++)
483  {
484  float Arot1_draw =
485  Arot1 - (o.thrunModel.alfa1_rot_rot * fabs(Arot1) +
486  o.thrunModel.alfa2_rot_trans * Atrans) *
488  float Atrans_draw =
489  Atrans -
490  (o.thrunModel.alfa3_trans_trans * Atrans +
491  o.thrunModel.alfa4_trans_rot * (fabs(Arot1) + fabs(Arot2))) *
493  float Arot2_draw =
494  Arot2 - (o.thrunModel.alfa1_rot_rot * fabs(Arot2) +
495  o.thrunModel.alfa2_rot_trans * Atrans) *
497 
498  // Output:
499  aux->m_particles[i].d.x =
500  Atrans_draw * cos(Arot1_draw) +
501  motionModelConfiguration.thrunModel.additional_std_XY *
503  aux->m_particles[i].d.y =
504  Atrans_draw * sin(Arot1_draw) +
505  motionModelConfiguration.thrunModel.additional_std_XY *
507  aux->m_particles[i].d.phi =
508  Arot1_draw + Arot2_draw +
509  motionModelConfiguration.thrunModel.additional_std_phi *
511  aux->m_particles[i].d.normalizePhi();
512  }
513 }
514 
515 /*---------------------------------------------------------------
516  drawSingleSample
517  ---------------------------------------------------------------*/
519 {
520  // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
521  // the parameters to draw new samples:
522  if (estimationMethod == emOdometry)
523  {
524  if (motionModelConfiguration.modelSelection == mmGaussian)
525  drawSingleSample_modelGaussian(outSample);
526  else
527  drawSingleSample_modelThrun(outSample);
528  }
529  else
530  {
531  // If is not odometry, just employ the stored distribution:
532  poseChange->drawSingleSample(outSample);
533  }
534 }
535 
536 /*---------------------------------------------------------------
537  drawSingleSample_modelGaussian
538  ---------------------------------------------------------------*/
540  CPose2D& outSample) const
541 {
542  // In the Gaussian case it is more efficient just to
543  // draw a sample from the already computed PDF:
544  poseChange->drawSingleSample(outSample);
545 }
546 
547 /*---------------------------------------------------------------
548  drawSingleSample_modelThrun
549  ---------------------------------------------------------------*/
551  CPose2D& outSample) const
552 {
553  // ---------------------------------------------------------------------------------------------
554  // The following is an implementation from Thrun et al.'s book
555  // (Probabilistic Robotics),
556  // page 136. Here "odometryIncrement" actually represents the incremental
557  // odometry difference.
558  // ---------------------------------------------------------------------------------------------
559 
560  // The increments in odometry:
561  float Arot1 = (rawOdometryIncrementReading.y() != 0 ||
562  rawOdometryIncrementReading.x() != 0)
563  ? atan2(
564  rawOdometryIncrementReading.y(),
565  rawOdometryIncrementReading.x())
566  : 0;
567  float Atrans = rawOdometryIncrementReading.norm();
568  float Arot2 = math::wrapToPi(rawOdometryIncrementReading.phi() - Arot1);
569 
570  float Arot1_draw =
571  Arot1 -
572  (motionModelConfiguration.thrunModel.alfa1_rot_rot * fabs(Arot1) +
573  motionModelConfiguration.thrunModel.alfa2_rot_trans * Atrans) *
575  float Atrans_draw =
576  Atrans -
577  (motionModelConfiguration.thrunModel.alfa3_trans_trans * Atrans +
578  motionModelConfiguration.thrunModel.alfa4_trans_rot *
579  (fabs(Arot1) + fabs(Arot2))) *
581  float Arot2_draw =
582  Arot2 -
583  (motionModelConfiguration.thrunModel.alfa1_rot_rot * fabs(Arot2) +
584  motionModelConfiguration.thrunModel.alfa2_rot_trans * Atrans) *
586 
587  // Output:
588  outSample.x(
589  Atrans_draw * cos(Arot1_draw) +
590  motionModelConfiguration.thrunModel.additional_std_XY *
591  getRandomGenerator().drawGaussian1D_normalized());
592  outSample.y(
593  Atrans_draw * sin(Arot1_draw) +
594  motionModelConfiguration.thrunModel.additional_std_XY *
595  getRandomGenerator().drawGaussian1D_normalized());
596  outSample.phi(
597  Arot1_draw + Arot2_draw +
598  motionModelConfiguration.thrunModel.additional_std_phi *
599  getRandomGenerator().drawGaussian1D_normalized());
600  outSample.normalizePhi();
601 }
602 
603 /*---------------------------------------------------------------
604  prepareFastDrawSingleSamples
605  ---------------------------------------------------------------*/
607 {
608  // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
609  // the parameters to draw new samples:
610  if (estimationMethod == emOdometry)
611  {
612  if (motionModelConfiguration.modelSelection == mmGaussian)
613  prepareFastDrawSingleSample_modelGaussian();
614  else
615  prepareFastDrawSingleSample_modelThrun();
616  }
617 }
618 
619 /*---------------------------------------------------------------
620  fastDrawSingleSample
621  ---------------------------------------------------------------*/
623 {
624  // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
625  // the parameters to draw new samples:
626  if (estimationMethod == emOdometry)
627  {
628  if (motionModelConfiguration.modelSelection == mmGaussian)
629  fastDrawSingleSample_modelGaussian(outSample);
630  else
631  fastDrawSingleSample_modelThrun(outSample);
632  }
633  else
634  {
635  // If is not odometry, just employ the stored distribution:
636  poseChange->drawSingleSample(outSample);
637  }
638 }
639 
640 /*---------------------------------------------------------------
641  prepareFastDrawSingleSample_modelGaussian
642  ---------------------------------------------------------------*/
644 {
645  MRPT_START
646 
647  ASSERT_(IS_CLASS(*poseChange, CPosePDFGaussian));
648 
649  const auto* gPdf = dynamic_cast<const CPosePDFGaussian*>(poseChange.get());
650  ASSERT_(gPdf != nullptr);
651  const CMatrixDouble33& cov = gPdf->cov;
652 
653  m_fastDrawGauss_M = gPdf->mean;
654 
655  /** Computes the eigenvalues/eigenvector decomposition of this matrix,
656  * so that: M = Z · D · Z<sup>T</sup>, where columns in Z are the
657  * eigenvectors and the diagonal matrix D contains the eigenvalues
658  * as diagonal elements, sorted in <i>ascending</i> order.
659  */
660  std::vector<double> eigvals;
661  cov.eig_symmetric(m_fastDrawGauss_Z, eigvals);
662  CMatrixDouble33 D;
663  D.setDiagonal(eigvals);
664 
665  // Scale eigenvectors with eigenvalues:
666  D.asEigen() = D.array().sqrt().matrix();
667  m_fastDrawGauss_Z = m_fastDrawGauss_Z * D;
668 
669  MRPT_END
670 }
671 
672 /*---------------------------------------------------------------
673  prepareFastDrawSingleSample_modelThrun
674  ---------------------------------------------------------------*/
676 /*---------------------------------------------------------------
677  prepareFastDrawSingleSample_modelThrun
678  ---------------------------------------------------------------*/
680  CPose2D& outSample) const
681 {
682  CVectorFloat rndVector;
683  rndVector.assign(3, 0.0f);
684  for (size_t i = 0; i < 3; i++)
685  {
687  for (size_t d = 0; d < 3; d++)
688  rndVector[d] += (m_fastDrawGauss_Z(d, i) * rnd);
689  }
690 
691  outSample = CPose2D(
692  m_fastDrawGauss_M.x() + rndVector[0],
693  m_fastDrawGauss_M.y() + rndVector[1],
694  m_fastDrawGauss_M.phi() + rndVector[2]);
695 }
696 
697 /*---------------------------------------------------------------
698  prepareFastDrawSingleSample_modelThrun
699  ---------------------------------------------------------------*/
701  CPose2D& outSample) const
702 {
703  drawSingleSample_modelThrun(outSample);
704 }
A namespace of pseudo-random numbers generators of diferent distributions.
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:241
Template for column vectors of dynamic size, compatible with Eigen.
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)
To be added to all CSerializable-classes implementation files.
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
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...
bool eig_symmetric(Derived &eVecs, std::vector< Scalar > &eVals, bool sorted=true) const
Read: eig()
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...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Definition: TTwist2D.h:19
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
Represents a probabilistic 2D movement of the robot mobile base.
This base provides a set of functions for maths stuff.
CMatrixFixed< double, 3, 1 > CMatrixDouble31
Definition: CMatrixFixed.h:357
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...
T square(const T x)
Inline function for the square of a number.
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.
void assign(const std::size_t N, const Scalar value)
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.
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:133
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
__int32 int32_t
Definition: glext.h:3455
CMatrixDouble 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:149
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:256
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
TDrawSampleMotionModel modelSelection
The model to be used.
void fastDrawSingleSample_modelGaussian(mrpt::poses::CPose2D &outSample) const
Internal use.
Declares a class for storing a robot action.
Definition: CAction.h:24
void prepareFastDrawSingleSamples() const
Call this before calling a high number of times "fastDrawSingleSample", which is much faster than "dr...
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:54
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:39
const float R
mrpt::vision::TStereoCalibResults out
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
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:245
GLuint in
Definition: glext.h:7391
Lightweight 2D pose.
Definition: TPose2D.h:22
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
GLenum GLint GLint y
Definition: glext.h:3542
GLenum GLint x
Definition: glext.h:3542
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
void setDiagonal(const std::size_t N, const Scalar value)
Resize to NxN, set all entries to zero, except the main diagonal which is set to value ...
Definition: MatrixBase.h:34
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
Definition: CPose2D.cpp:333
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.
void multiply_HCHt(const MAT_H &H, const MAT_C &C, MAT_R &R, bool accumResultInOutput=false)
R = H * C * H^t.
Definition: ops_matrices.h:28



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: e47402b84 Wed Oct 23 01:09:07 2019 +0200 at mié oct 23 01:10:13 CEST 2019