MRPT  2.0.4
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-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
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  // Re-build the PDF to have a consistent object state:
36  computeFromOdometry(rawOdometryIncrementReading, motionModelConfiguration);
37 }
38 
39 uint8_t CActionRobotMovement2D::serializeGetVersion() const { return 7; }
42 {
43  out.WriteAs<uint32_t>(estimationMethod);
44  // Added in version 2:
45  // If the estimation method is emOdometry, save the rawOdo + config data
46  // instead of
47  // the PDF itself:
48  if (estimationMethod == emOdometry)
49  {
50  // The odometry data:
51  out << rawOdometryIncrementReading;
52  out.WriteAs<uint32_t>(motionModelConfiguration.modelSelection);
53  const auto& gm = motionModelConfiguration.gaussianModel;
54  out.WriteAs<float>(gm.a1);
55  out.WriteAs<float>(gm.a2);
56  out.WriteAs<float>(gm.a3);
57  out.WriteAs<float>(gm.a4);
58  out.WriteAs<float>(gm.minStdXY);
59  out.WriteAs<float>(gm.minStdPHI);
60 
61  out << motionModelConfiguration.thrunModel.nParticlesCount
62  << motionModelConfiguration.thrunModel.alfa1_rot_rot
63  << motionModelConfiguration.thrunModel.alfa2_rot_trans
64  << motionModelConfiguration.thrunModel.alfa3_trans_trans
65  << motionModelConfiguration.thrunModel.alfa4_trans_rot
66  << motionModelConfiguration.thrunModel.additional_std_XY
67  << motionModelConfiguration.thrunModel.additional_std_phi;
68  }
69  else
70  {
71  // The PDF:
72  out << (*poseChange);
73  }
74 
75  // Added in version 1:
76  out << hasVelocities;
77  if (hasVelocities) out << velocityLocal; // v7
78 
79  out << hasEncodersInfo;
80  if (hasEncodersInfo)
81  out << encoderLeftTicks << encoderRightTicks; // added if() in v7
82 
83  // Added in version 6
84  out << timestamp;
85 }
86 
88  mrpt::serialization::CArchive& in, uint8_t version)
89 {
90  switch (version)
91  {
92  case 4:
93  case 5:
94  case 6:
95  case 7:
96  {
97  int32_t i;
98 
99  // Read the estimation method first:
100  in >> i;
101  estimationMethod = static_cast<TEstimationMethod>(i);
102 
103  // If the estimation method is emOdometry, save the rawOdo + config
104  // data instead of
105  // the PDF itself:
106  if (estimationMethod == emOdometry)
107  {
108  // The odometry data:
109  in >> rawOdometryIncrementReading;
110 
111  in >> i;
112  motionModelConfiguration.modelSelection =
113  static_cast<TDrawSampleMotionModel>(i);
114 
115  auto& gm = motionModelConfiguration.gaussianModel;
116  gm.a1 = in.ReadAs<float>();
117  gm.a2 = in.ReadAs<float>();
118  gm.a3 = in.ReadAs<float>();
119  gm.a4 = in.ReadAs<float>();
120  gm.minStdXY = in.ReadAs<float>();
121  gm.minStdPHI = in.ReadAs<float>();
122 
123  in >> i;
124  motionModelConfiguration.thrunModel.nParticlesCount = i;
125  in >> motionModelConfiguration.thrunModel.alfa1_rot_rot >>
126  motionModelConfiguration.thrunModel.alfa2_rot_trans >>
127  motionModelConfiguration.thrunModel.alfa3_trans_trans >>
128  motionModelConfiguration.thrunModel.alfa4_trans_rot;
129 
130  if (version >= 5)
131  {
132  in >>
133  motionModelConfiguration.thrunModel.additional_std_XY >>
134  motionModelConfiguration.thrunModel.additional_std_phi;
135  }
136  else
137  {
138  motionModelConfiguration.thrunModel.additional_std_XY =
139  motionModelConfiguration.thrunModel.additional_std_phi =
140  0;
141  }
142 
143  // Re-build the PDF:
144  computeFromOdometry(
145  rawOdometryIncrementReading, motionModelConfiguration);
146  }
147  else
148  {
149  // Read the PDF directly from the stream:
150  CPosePDF::Ptr pc;
151  in >> pc;
152  poseChange = pc;
153  }
154 
155  in >> hasVelocities;
156  if (version >= 7)
157  {
158  if (hasVelocities) in >> velocityLocal;
159  }
160  else
161  {
162  float velocityLin, velocityAng;
163  in >> velocityLin >> velocityAng;
164  velocityLocal.vx = velocityLin;
165  velocityLocal.vy = .0f;
166  velocityLocal.omega = velocityAng;
167  }
168  in >> hasEncodersInfo;
169  if (version < 7 || hasEncodersInfo)
170  {
171  in >> i;
172  encoderLeftTicks = i;
173  in >> i;
174  encoderRightTicks = i;
175  }
176  else
177  {
178  encoderLeftTicks = 0;
179  encoderRightTicks = 0;
180  }
181 
182  if (version >= 6)
183  in >> timestamp;
184  else
185  timestamp = INVALID_TIMESTAMP;
186  }
187  break;
188 
189  case 3:
190  {
191  int32_t i;
192 
193  // Read the estimation method first:
194  in >> i;
195  estimationMethod = static_cast<TEstimationMethod>(i);
196 
197  // If the estimation method is emOdometry, save the rawOdo + config
198  // data instead of
199  // the PDF itself:
200  if (estimationMethod == emOdometry)
201  {
202  // The odometry data:
203  in >> rawOdometryIncrementReading;
204 
205  in >> i;
206  motionModelConfiguration.modelSelection =
207  static_cast<TDrawSampleMotionModel>(i);
208 
209  float dum1, dum2, dum3;
210 
211  in >> dum1 >> dum2 >> dum3 >>
212  motionModelConfiguration.gaussianModel.minStdXY >>
213  motionModelConfiguration.gaussianModel.minStdPHI;
214 
215  // Leave the default values for a1,a2,a3,a4:
216  in >> i;
217  motionModelConfiguration.thrunModel.nParticlesCount = i;
218  in >> motionModelConfiguration.thrunModel.alfa1_rot_rot;
219  in >> motionModelConfiguration.thrunModel.alfa2_rot_trans >>
220  motionModelConfiguration.thrunModel.alfa3_trans_trans >>
221  motionModelConfiguration.thrunModel.alfa4_trans_rot;
222 
223  // Re-build the PDF:
224  computeFromOdometry(
225  rawOdometryIncrementReading, motionModelConfiguration);
226  }
227  else
228  {
229  // Read the PDF directly from the stream:
230  CPosePDF::Ptr pc;
231  in >> pc;
232  poseChange = pc;
233  }
234 
235  in >> hasVelocities;
236  {
237  float velocityLin, velocityAng;
238  in >> velocityLin >> velocityAng;
239  velocityLocal.vx = velocityLin;
240  velocityLocal.vy = .0f;
241  velocityLocal.omega = velocityAng;
242  }
243  in >> hasEncodersInfo;
244 
245  in >> i;
246  encoderLeftTicks = i;
247  in >> i;
248  encoderRightTicks = i;
249  }
250  break;
251 
252  case 2:
253  {
254  int32_t i;
255 
256  // Read the estimation method first:
257  in >> i;
258  estimationMethod = static_cast<TEstimationMethod>(i);
259 
260  // If the estimation method is emOdometry, save the rawOdo + config
261  // data instead of
262  // the PDF itself:
263  if (estimationMethod == emOdometry)
264  {
265  // The odometry data:
266  in >> rawOdometryIncrementReading;
267 
268  // TMotionModelOptions_OLD_VERSION_2 dummy;
269  // in.ReadBuffer( &dummy,
270  // sizeof(TMotionModelOptions_OLD_VERSION_2) );
271  uint8_t dummy[44];
272  in.ReadBuffer(&dummy, sizeof(dummy));
273 
274  motionModelConfiguration = TMotionModelOptions();
275 
276  // Re-build the PDF:
277  computeFromOdometry(
278  rawOdometryIncrementReading, motionModelConfiguration);
279  }
280  else
281  {
282  // Read the PDF directly from the stream:
283  CPosePDF::Ptr pc;
284  in >> pc;
285  poseChange = pc;
286  }
287 
288  in >> hasVelocities;
289  {
290  float velocityLin, velocityAng;
291  in >> velocityLin >> velocityAng;
292  velocityLocal.vx = velocityLin;
293  velocityLocal.vy = .0f;
294  velocityLocal.omega = velocityAng;
295  }
296  in >> hasEncodersInfo;
297  in >> i;
298  encoderLeftTicks = i;
299  in >> i;
300  encoderRightTicks = i;
301  }
302  break;
303  case 0:
304  case 1:
305  {
306  int32_t i;
307  {
308  CPosePDF::Ptr pc;
309  in >> pc;
310  poseChange = pc;
311  }
312  in >> i;
313 
314  // copy:
315  estimationMethod = static_cast<TEstimationMethod>(i);
316 
317  // Simulate the "rawOdometryIncrementReading" as the mean value:
318  if (estimationMethod == emOdometry)
319  poseChange->getMean(rawOdometryIncrementReading);
320  else
321  rawOdometryIncrementReading = CPose2D(0, 0, 0);
322 
323  if (version >= 1)
324  {
325  in >> hasVelocities;
326  {
327  float velocityLin, velocityAng;
328  in >> velocityLin >> velocityAng;
329  velocityLocal.vx = velocityLin;
330  velocityLocal.vy = .0f;
331  velocityLocal.omega = velocityAng;
332  }
333  in >> hasEncodersInfo;
334  in >> i;
335  encoderLeftTicks = i;
336  in >> i;
337  encoderRightTicks = i;
338  }
339  else
340  {
341  // Default values:
342  hasVelocities = hasEncodersInfo = false;
343  encoderLeftTicks = encoderRightTicks = 0;
344  velocityLocal = mrpt::math::TTwist2D(.0, .0, .0);
345  }
346  }
347  break;
348  default:
350  };
351 }
352 
353 /*---------------------------------------------------------------
354  computeFromEncoders
355  ---------------------------------------------------------------*/
357  double K_left, double K_right, double D)
358 {
359  if (hasEncodersInfo)
360  {
361  const double As =
362  0.5 * (K_right * encoderRightTicks + K_left * encoderLeftTicks);
363  const double Aphi =
364  (K_right * encoderRightTicks - K_left * encoderLeftTicks) / D;
365 
366  double x, y;
367  if (Aphi != 0)
368  {
369  const double R = As / Aphi;
370  x = R * sin(Aphi);
371  y = R * (1 - cos(Aphi));
372  }
373  else
374  {
375  x = As;
376  y = 0;
377  }
378 
379  // Build the whole PDF with the current parameters:
380  computeFromOdometry(CPose2D(x, y, Aphi), motionModelConfiguration);
381  }
382 }
383 
384 /*---------------------------------------------------------------
385  computeFromOdometry
386  ---------------------------------------------------------------*/
388  const CPose2D& odometryIncrement, const TMotionModelOptions& options)
389 {
390  // Set the members data:
391  estimationMethod = emOdometry;
392  rawOdometryIncrementReading = odometryIncrement;
393  motionModelConfiguration = options;
394 
395  if (options.modelSelection == mmGaussian)
396  computeFromOdometry_modelGaussian(odometryIncrement, options);
397  else
398  computeFromOdometry_modelThrun(odometryIncrement, options);
399 }
400 
401 /*---------------------------------------------------------------
402  computeFromOdometry_modelGaussian
403  ---------------------------------------------------------------*/
405  const CPose2D& odometryIncrement, const TMotionModelOptions& o)
406 {
407  // The Gaussian PDF:
408  // ---------------------------
409  poseChange = std::make_shared<CPosePDFGaussian>();
410  auto* aux = dynamic_cast<CPosePDFGaussian*>(poseChange.get());
411  ASSERT_(aux != nullptr);
412 
413  // See https://www.mrpt.org/Probabilistic_Motion_Models
414  // -----------------------------------
415 
416  // Build the odometry noise matrix:
417  double Al = odometryIncrement.norm();
418  auto ODO_INCR = CMatrixDouble31(odometryIncrement);
419  CMatrixDouble33 C_ODO;
420  C_ODO(0, 0) = square(
422  o.gaussianModel.a2 * fabs(odometryIncrement.phi()));
423  C_ODO(1, 1) = square(
425  o.gaussianModel.a2 * fabs(odometryIncrement.phi()));
426  C_ODO(2, 2) = square(
428  o.gaussianModel.a4 * fabs(odometryIncrement.phi()));
429 
430  // Build the transformation matrix:
431  CMatrixDouble33 H;
432 
433  double cos_Aphi_2 = cos(odometryIncrement.phi() / 2);
434  double sin_Aphi_2 = sin(odometryIncrement.phi() / 2);
435 
436  H(0, 0) = H(1, 1) = cos_Aphi_2;
437  H(0, 1) = -(H(1, 0) = sin_Aphi_2);
438  H(2, 2) = 1;
439 
440  // Build the Jacobian matrix:
441  CMatrixDouble33 J = H;
442  J(0, 2) = -sin_Aphi_2 * ODO_INCR(0, 0) - cos_Aphi_2 * ODO_INCR(1, 0);
443  J(1, 2) = cos_Aphi_2 * ODO_INCR(0, 0) - sin_Aphi_2 * ODO_INCR(1, 0);
444 
445  // The mean:
446  aux->mean = odometryIncrement;
447 
448  // The covariance:
449  aux->cov = mrpt::math::multiply_HCHt(J, C_ODO);
450 }
451 
452 /*---------------------------------------------------------------
453  computeFromOdometry_modelThrun
454  ---------------------------------------------------------------*/
456  const CPose2D& odometryIncrement, const TMotionModelOptions& o)
457 {
458  // The Gaussian PDF:
459  // ---------------------------
460  const mrpt::math::TPose2D nullPose(0, 0, 0);
461 
462  poseChange = CPosePDFParticles::Create();
463  auto* aux = dynamic_cast<CPosePDFParticles*>(poseChange.get());
464  ASSERT_(aux != nullptr);
465 
466  // Set the number of particles:
467  aux->resetDeterministic(nullPose, o.thrunModel.nParticlesCount);
468 
469  // ---------------------------------------------------------------------------------------------
470  // The following is an implementation from Thrun et al.'s book
471  // (Probabilistic Robotics),
472  // page 136. Here "odometryIncrement" actually represents the incremental
473  // odometry difference.
474  // ---------------------------------------------------------------------------------------------
475 
476  // The increments in odometry:
477  double Arot1 = (odometryIncrement.y() != 0 || odometryIncrement.x() != 0)
478  ? atan2(odometryIncrement.y(), odometryIncrement.x())
479  : 0;
480  double Atrans = odometryIncrement.norm();
481  double Arot2 = math::wrapToPi(odometryIncrement.phi() - Arot1);
482 
483  // Draw samples:
484  for (size_t i = 0; i < o.thrunModel.nParticlesCount; i++)
485  {
486  double Arot1_draw =
487  Arot1 - (o.thrunModel.alfa1_rot_rot * fabs(Arot1) +
488  o.thrunModel.alfa2_rot_trans * Atrans) *
490  double Atrans_draw =
491  Atrans -
492  (o.thrunModel.alfa3_trans_trans * Atrans +
493  o.thrunModel.alfa4_trans_rot * (fabs(Arot1) + fabs(Arot2))) *
495  double Arot2_draw =
496  Arot2 - (o.thrunModel.alfa1_rot_rot * fabs(Arot2) +
497  o.thrunModel.alfa2_rot_trans * Atrans) *
499 
500  // Output:
501  aux->m_particles[i].d.x =
502  Atrans_draw * cos(Arot1_draw) +
503  motionModelConfiguration.thrunModel.additional_std_XY *
505  aux->m_particles[i].d.y =
506  Atrans_draw * sin(Arot1_draw) +
507  motionModelConfiguration.thrunModel.additional_std_XY *
509  aux->m_particles[i].d.phi =
510  Arot1_draw + Arot2_draw +
511  motionModelConfiguration.thrunModel.additional_std_phi *
513  aux->m_particles[i].d.normalizePhi();
514  }
515 }
516 
517 /*---------------------------------------------------------------
518  drawSingleSample
519  ---------------------------------------------------------------*/
521 {
522  // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
523  // the parameters to draw new samples:
524  if (estimationMethod == emOdometry)
525  {
526  if (motionModelConfiguration.modelSelection == mmGaussian)
527  drawSingleSample_modelGaussian(outSample);
528  else
529  drawSingleSample_modelThrun(outSample);
530  }
531  else
532  {
533  // If is not odometry, just employ the stored distribution:
534  poseChange->drawSingleSample(outSample);
535  }
536 }
537 
538 /*---------------------------------------------------------------
539  drawSingleSample_modelGaussian
540  ---------------------------------------------------------------*/
542  CPose2D& outSample) const
543 {
544  // In the Gaussian case it is more efficient just to
545  // draw a sample from the already computed PDF:
546  poseChange->drawSingleSample(outSample);
547 }
548 
549 /*---------------------------------------------------------------
550  drawSingleSample_modelThrun
551  ---------------------------------------------------------------*/
553  CPose2D& outSample) const
554 {
555  // ---------------------------------------------------------------------------------------------
556  // The following is an implementation from Thrun et al.'s book
557  // (Probabilistic Robotics),
558  // page 136. Here "odometryIncrement" actually represents the incremental
559  // odometry difference.
560  // ---------------------------------------------------------------------------------------------
561 
562  // The increments in odometry:
563  double Arot1 = (rawOdometryIncrementReading.y() != 0 ||
564  rawOdometryIncrementReading.x() != 0)
565  ? atan2(
566  rawOdometryIncrementReading.y(),
567  rawOdometryIncrementReading.x())
568  : 0;
569  double Atrans = rawOdometryIncrementReading.norm();
570  double Arot2 = math::wrapToPi(rawOdometryIncrementReading.phi() - Arot1);
571 
572  double Arot1_draw =
573  Arot1 -
574  (motionModelConfiguration.thrunModel.alfa1_rot_rot * fabs(Arot1) +
575  motionModelConfiguration.thrunModel.alfa2_rot_trans * Atrans) *
577  double Atrans_draw =
578  Atrans -
579  (motionModelConfiguration.thrunModel.alfa3_trans_trans * Atrans +
580  motionModelConfiguration.thrunModel.alfa4_trans_rot *
581  (fabs(Arot1) + fabs(Arot2))) *
583  double Arot2_draw =
584  Arot2 -
585  (motionModelConfiguration.thrunModel.alfa1_rot_rot * fabs(Arot2) +
586  motionModelConfiguration.thrunModel.alfa2_rot_trans * Atrans) *
588 
589  // Output:
590  outSample.x(
591  Atrans_draw * cos(Arot1_draw) +
592  motionModelConfiguration.thrunModel.additional_std_XY *
593  getRandomGenerator().drawGaussian1D_normalized());
594  outSample.y(
595  Atrans_draw * sin(Arot1_draw) +
596  motionModelConfiguration.thrunModel.additional_std_XY *
597  getRandomGenerator().drawGaussian1D_normalized());
598  outSample.phi(
599  Arot1_draw + Arot2_draw +
600  motionModelConfiguration.thrunModel.additional_std_phi *
601  getRandomGenerator().drawGaussian1D_normalized());
602  outSample.normalizePhi();
603 }
604 
605 /*---------------------------------------------------------------
606  prepareFastDrawSingleSamples
607  ---------------------------------------------------------------*/
609 {
610  // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
611  // the parameters to draw new samples:
612  if (estimationMethod == emOdometry)
613  {
614  if (motionModelConfiguration.modelSelection == mmGaussian)
615  prepareFastDrawSingleSample_modelGaussian();
616  else
617  prepareFastDrawSingleSample_modelThrun();
618  }
619 }
620 
621 /*---------------------------------------------------------------
622  fastDrawSingleSample
623  ---------------------------------------------------------------*/
625 {
626  // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
627  // the parameters to draw new samples:
628  if (estimationMethod == emOdometry)
629  {
630  if (motionModelConfiguration.modelSelection == mmGaussian)
631  fastDrawSingleSample_modelGaussian(outSample);
632  else
633  fastDrawSingleSample_modelThrun(outSample);
634  }
635  else
636  {
637  // If is not odometry, just employ the stored distribution:
638  poseChange->drawSingleSample(outSample);
639  }
640 }
641 
642 /*---------------------------------------------------------------
643  prepareFastDrawSingleSample_modelGaussian
644  ---------------------------------------------------------------*/
646 {
647  MRPT_START
648 
649  ASSERT_(IS_CLASS(*poseChange, CPosePDFGaussian));
650 
651  const auto* gPdf = dynamic_cast<const CPosePDFGaussian*>(poseChange.get());
652  ASSERT_(gPdf != nullptr);
653  const CMatrixDouble33& cov = gPdf->cov;
654 
655  m_fastDrawGauss_M = gPdf->mean;
656 
657  /** Computes the eigenvalues/eigenvector decomposition of this matrix,
658  * so that: M = Z · D · Z<sup>T</sup>, where columns in Z are the
659  * eigenvectors and the diagonal matrix D contains the eigenvalues
660  * as diagonal elements, sorted in <i>ascending</i> order.
661  */
662  std::vector<double> eigvals;
663  cov.eig_symmetric(m_fastDrawGauss_Z, eigvals);
664  CMatrixDouble33 D;
665  D.setDiagonal(eigvals);
666 
667  // Scale eigenvectors with eigenvalues:
668  D.asEigen() = D.array().sqrt().matrix();
669  m_fastDrawGauss_Z = m_fastDrawGauss_Z * D;
670 
671  MRPT_END
672 }
673 
674 /*---------------------------------------------------------------
675  prepareFastDrawSingleSample_modelThrun
676  ---------------------------------------------------------------*/
678 /*---------------------------------------------------------------
679  prepareFastDrawSingleSample_modelThrun
680  ---------------------------------------------------------------*/
682  CPose2D& outSample) const
683 {
684  CVectorDouble rndVector;
685  rndVector.assign(3, 0.0f);
686  for (size_t i = 0; i < 3; i++)
687  {
689  for (size_t d = 0; d < 3; d++)
690  rndVector[d] += (m_fastDrawGauss_Z(d, i) * rnd);
691  }
692 
693  outSample = CPose2D(
694  m_fastDrawGauss_M.x() + rndVector[0],
695  m_fastDrawGauss_M.y() + rndVector[1],
696  m_fastDrawGauss_M.phi() + rndVector[2]);
697 }
698 
699 /*---------------------------------------------------------------
700  prepareFastDrawSingleSample_modelThrun
701  ---------------------------------------------------------------*/
703  CPose2D& outSample) const
704 {
705  drawSingleSample_modelThrun(outSample);
706 }
707 
709 {
711 
712  CPose2D Ap;
713  CMatrixDouble33 mat;
714  poseChange->getCovarianceAndMean(mat, Ap);
715 
716  o << "Robot Movement (as a gaussian pose change):\n";
717  o << " Mean = " << Ap << "\n";
718 
719  o << format(" Covariance: DET=%e\n", mat.det());
720 
721  o << format(" %e %e %e\n", mat(0, 0), mat(0, 1), mat(0, 2));
722  o << format(" %e %e %e\n", mat(1, 0), mat(1, 1), mat(1, 2));
723  o << format(" %e %e %e\n", mat(2, 0), mat(2, 1), mat(2, 2));
724 
725  o << "\n";
726 
727  o << "Actual odometry increment reading: " << rawOdometryIncrementReading
728  << "\n";
729 
730  o << format(
731  "Actual PDF class is: '%s'\n",
732  poseChange->GetRuntimeClass()->className);
733 
734  if (poseChange->GetRuntimeClass() == CLASS_ID(CPosePDFParticles))
735  {
737  std::dynamic_pointer_cast<CPosePDFParticles>(poseChange.get_ptr());
738  o << format(
739  " (Particle count = %u)\n", (unsigned)aux->m_particles.size());
740  }
741  o << "\n";
742 
743  o << "Estimation method: "
745  estimationMethod)
746  << "\n";
747 
748  // Additional data:
749  if (hasEncodersInfo)
750  o << format(
751  "Encoder info: deltaL=%i deltaR=%i\n", encoderLeftTicks,
752  encoderRightTicks);
753  else
754  o << "Encoder info: Not available!\n";
755 
756  if (hasVelocities)
757  o << "Velocity info: v=" << velocityLocal.asString() << "\n";
758  else
759  o << "Velocity info: Not available!\n";
760 }
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
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
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.
static std::string value2name(const ENUMTYPE val)
Gives the textual name for a given enum value.
Definition: TEnumType.h:115
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.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
CMatrixFixed< double, 3, 1 > CMatrixDouble31
Definition: CMatrixFixed.h:372
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 phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
STORED_TYPE ReadAs()
De-serialize a variable and returns it by value.
Definition: CArchive.h:155
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...
Scalar det() const
Determinant of matrix.
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:146
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...
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
virtual void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the action contents and dump it to the output str...
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
return_t square(const num_t x)
Inline function for the square of a number.
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
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
Lightweight 2D pose.
Definition: TPose2D.h:22
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the action contents and dump it to the output str...
Definition: CAction.cpp:22
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
Definition: CArchive.cpp:25
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 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020