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 
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 
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 
410  // See https://www.mrpt.org/Probabilistic_Motion_Models
411  // -----------------------------------
412 
413  // Build the odometry noise matrix:
414  double Al = odometryIncrement.norm();
415  auto ODO_INCR = CMatrixDouble31(odometryIncrement);
416  CMatrixDouble33 C_ODO;
417  C_ODO(0, 0) = square(
419  o.gaussianModel.a2 * fabs(odometryIncrement.phi()));
420  C_ODO(1, 1) = square(
422  o.gaussianModel.a2 * fabs(odometryIncrement.phi()));
423  C_ODO(2, 2) = square(
425  o.gaussianModel.a4 * fabs(odometryIncrement.phi()));
426 
427  // Build the transformation matrix:
428  CMatrixDouble33 H;
429 
430  double cos_Aphi_2 = cos(odometryIncrement.phi() / 2);
431  double sin_Aphi_2 = sin(odometryIncrement.phi() / 2);
432 
433  H(0, 0) = H(1, 1) = cos_Aphi_2;
434  H(0, 1) = -(H(1, 0) = sin_Aphi_2);
435  H(2, 2) = 1;
436 
437  // Build the Jacobian matrix:
438  CMatrixDouble33 J = H;
439  J(0, 2) = -sin_Aphi_2 * ODO_INCR(0, 0) - cos_Aphi_2 * ODO_INCR(1, 0);
440  J(1, 2) = cos_Aphi_2 * ODO_INCR(0, 0) - sin_Aphi_2 * ODO_INCR(1, 0);
441 
442  // The mean:
443  aux->mean = odometryIncrement;
444 
445  // The covariance:
446  aux->cov = mrpt::math::multiply_HCHt(J, C_ODO);
447 }
448 
449 /*---------------------------------------------------------------
450  computeFromOdometry_modelThrun
451  ---------------------------------------------------------------*/
453  const CPose2D& odometryIncrement, const TMotionModelOptions& o)
454 {
455  // The Gaussian PDF:
456  // ---------------------------
457  const mrpt::math::TPose2D nullPose(0, 0, 0);
458 
459  poseChange = CPosePDFParticles::Create();
460  auto* aux = dynamic_cast<CPosePDFParticles*>(poseChange.get());
461 
462  // Set the number of particles:
464 
465  // ---------------------------------------------------------------------------------------------
466  // The following is an implementation from Thrun et al.'s book
467  // (Probabilistic Robotics),
468  // page 136. Here "odometryIncrement" actually represents the incremental
469  // odometry difference.
470  // ---------------------------------------------------------------------------------------------
471 
472  // The increments in odometry:
473  float Arot1 = (odometryIncrement.y() != 0 || odometryIncrement.x() != 0)
474  ? atan2(odometryIncrement.y(), odometryIncrement.x())
475  : 0;
476  float Atrans = odometryIncrement.norm();
477  float Arot2 = math::wrapToPi(odometryIncrement.phi() - Arot1);
478 
479  // Draw samples:
480  for (size_t i = 0; i < o.thrunModel.nParticlesCount; i++)
481  {
482  float Arot1_draw =
483  Arot1 - (o.thrunModel.alfa1_rot_rot * fabs(Arot1) +
484  o.thrunModel.alfa2_rot_trans * Atrans) *
486  float Atrans_draw =
487  Atrans -
488  (o.thrunModel.alfa3_trans_trans * Atrans +
489  o.thrunModel.alfa4_trans_rot * (fabs(Arot1) + fabs(Arot2))) *
491  float Arot2_draw =
492  Arot2 - (o.thrunModel.alfa1_rot_rot * fabs(Arot2) +
493  o.thrunModel.alfa2_rot_trans * Atrans) *
495 
496  // Output:
497  aux->m_particles[i].d.x =
498  Atrans_draw * cos(Arot1_draw) +
499  motionModelConfiguration.thrunModel.additional_std_XY *
501  aux->m_particles[i].d.y =
502  Atrans_draw * sin(Arot1_draw) +
503  motionModelConfiguration.thrunModel.additional_std_XY *
505  aux->m_particles[i].d.phi =
506  Arot1_draw + Arot2_draw +
507  motionModelConfiguration.thrunModel.additional_std_phi *
509  aux->m_particles[i].d.normalizePhi();
510  }
511 }
512 
513 /*---------------------------------------------------------------
514  drawSingleSample
515  ---------------------------------------------------------------*/
517 {
518  // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
519  // the parameters to draw new samples:
520  if (estimationMethod == emOdometry)
521  {
522  if (motionModelConfiguration.modelSelection == mmGaussian)
523  drawSingleSample_modelGaussian(outSample);
524  else
525  drawSingleSample_modelThrun(outSample);
526  }
527  else
528  {
529  // If is not odometry, just employ the stored distribution:
530  poseChange->drawSingleSample(outSample);
531  }
532 }
533 
534 /*---------------------------------------------------------------
535  drawSingleSample_modelGaussian
536  ---------------------------------------------------------------*/
538  CPose2D& outSample) const
539 {
540  // In the Gaussian case it is more efficient just to
541  // draw a sample from the already computed PDF:
542  poseChange->drawSingleSample(outSample);
543 }
544 
545 /*---------------------------------------------------------------
546  drawSingleSample_modelThrun
547  ---------------------------------------------------------------*/
549  CPose2D& outSample) const
550 {
551  // ---------------------------------------------------------------------------------------------
552  // The following is an implementation from Thrun et al.'s book
553  // (Probabilistic Robotics),
554  // page 136. Here "odometryIncrement" actually represents the incremental
555  // odometry difference.
556  // ---------------------------------------------------------------------------------------------
557 
558  // The increments in odometry:
559  float Arot1 = (rawOdometryIncrementReading.y() != 0 ||
560  rawOdometryIncrementReading.x() != 0)
561  ? atan2(
562  rawOdometryIncrementReading.y(),
563  rawOdometryIncrementReading.x())
564  : 0;
565  float Atrans = rawOdometryIncrementReading.norm();
566  float Arot2 = math::wrapToPi(rawOdometryIncrementReading.phi() - Arot1);
567 
568  float Arot1_draw =
569  Arot1 -
570  (motionModelConfiguration.thrunModel.alfa1_rot_rot * fabs(Arot1) +
571  motionModelConfiguration.thrunModel.alfa2_rot_trans * Atrans) *
573  float Atrans_draw =
574  Atrans -
575  (motionModelConfiguration.thrunModel.alfa3_trans_trans * Atrans +
576  motionModelConfiguration.thrunModel.alfa4_trans_rot *
577  (fabs(Arot1) + fabs(Arot2))) *
579  float Arot2_draw =
580  Arot2 -
581  (motionModelConfiguration.thrunModel.alfa1_rot_rot * fabs(Arot2) +
582  motionModelConfiguration.thrunModel.alfa2_rot_trans * Atrans) *
584 
585  // Output:
586  outSample.x(
587  Atrans_draw * cos(Arot1_draw) +
588  motionModelConfiguration.thrunModel.additional_std_XY *
589  getRandomGenerator().drawGaussian1D_normalized());
590  outSample.y(
591  Atrans_draw * sin(Arot1_draw) +
592  motionModelConfiguration.thrunModel.additional_std_XY *
593  getRandomGenerator().drawGaussian1D_normalized());
594  outSample.phi(
595  Arot1_draw + Arot2_draw +
596  motionModelConfiguration.thrunModel.additional_std_phi *
597  getRandomGenerator().drawGaussian1D_normalized());
598  outSample.normalizePhi();
599 }
600 
601 /*---------------------------------------------------------------
602  prepareFastDrawSingleSamples
603  ---------------------------------------------------------------*/
605 {
606  // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
607  // the parameters to draw new samples:
608  if (estimationMethod == emOdometry)
609  {
610  if (motionModelConfiguration.modelSelection == mmGaussian)
611  prepareFastDrawSingleSample_modelGaussian();
612  else
613  prepareFastDrawSingleSample_modelThrun();
614  }
615 }
616 
617 /*---------------------------------------------------------------
618  fastDrawSingleSample
619  ---------------------------------------------------------------*/
621 {
622  // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
623  // the parameters to draw new samples:
624  if (estimationMethod == emOdometry)
625  {
626  if (motionModelConfiguration.modelSelection == mmGaussian)
627  fastDrawSingleSample_modelGaussian(outSample);
628  else
629  fastDrawSingleSample_modelThrun(outSample);
630  }
631  else
632  {
633  // If is not odometry, just employ the stored distribution:
634  poseChange->drawSingleSample(outSample);
635  }
636 }
637 
638 /*---------------------------------------------------------------
639  prepareFastDrawSingleSample_modelGaussian
640  ---------------------------------------------------------------*/
642 {
643  MRPT_START
644 
645  ASSERT_(IS_CLASS(*poseChange, CPosePDFGaussian));
646 
647  const auto* gPdf = dynamic_cast<const CPosePDFGaussian*>(poseChange.get());
648  ASSERT_(gPdf != nullptr);
649  const CMatrixDouble33& cov = gPdf->cov;
650 
651  m_fastDrawGauss_M = gPdf->mean;
652 
653  /** Computes the eigenvalues/eigenvector decomposition of this matrix,
654  * so that: M = Z · D · Z<sup>T</sup>, where columns in Z are the
655  * eigenvectors and the diagonal matrix D contains the eigenvalues
656  * as diagonal elements, sorted in <i>ascending</i> order.
657  */
658  std::vector<double> eigvals;
659  cov.eig_symmetric(m_fastDrawGauss_Z, eigvals);
660  CMatrixDouble33 D;
661  D.setDiagonal(eigvals);
662 
663  // Scale eigenvectors with eigenvalues:
664  D.asEigen() = D.array().sqrt().matrix();
665  m_fastDrawGauss_Z = m_fastDrawGauss_Z * D;
666 
667  MRPT_END
668 }
669 
670 /*---------------------------------------------------------------
671  prepareFastDrawSingleSample_modelThrun
672  ---------------------------------------------------------------*/
674 /*---------------------------------------------------------------
675  prepareFastDrawSingleSample_modelThrun
676  ---------------------------------------------------------------*/
678  CPose2D& outSample) const
679 {
680  CVectorFloat rndVector(3, 0);
681  for (size_t i = 0; i < 3; i++)
682  {
684  for (size_t d = 0; d < 3; d++)
685  rndVector[d] += (m_fastDrawGauss_Z(d, i) * rnd);
686  }
687 
688  outSample = CPose2D(
689  m_fastDrawGauss_M.x() + rndVector[0],
690  m_fastDrawGauss_M.y() + rndVector[1],
691  m_fastDrawGauss_M.phi() + rndVector[2]);
692 }
693 
694 /*---------------------------------------------------------------
695  prepareFastDrawSingleSample_modelThrun
696  ---------------------------------------------------------------*/
698  CPose2D& outSample) const
699 {
700  drawSingleSample_modelThrun(outSample);
701 }
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.
void WriteAs(const TYPE_FROM_ACTUAL &value)
Definition: CArchive.h:157
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...
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...
unsigned char uint8_t
Definition: rptypes.h:44
#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
T square(const T x)
Inline function for the square of a number.
#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...
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.
#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...
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:148
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...
__int32 int32_t
Definition: rptypes.h:49
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:53
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
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
unsigned __int32 uint32_t
Definition: rptypes.h:50
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: 3316834cb Wed Aug 21 11:42:56 2019 +0200 at mié ago 21 11:50:11 CEST 2019