MRPT  1.9.9
CBeacon.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 "maps-precomp.h" // Precomp header
11 
12 #include <mrpt/maps/CBeacon.h>
13 #include <mrpt/maps/CBeaconMap.h>
14 #include <mrpt/math/geometry.h>
15 #include <mrpt/math/ops_matrices.h>
16 #include <mrpt/obs/CObservation.h>
17 #include <mrpt/opengl/CEllipsoid.h>
20 #include <mrpt/opengl/CText.h>
22 #include <mrpt/system/os.h>
23 #include <Eigen/Dense>
24 
25 using namespace mrpt;
26 using namespace mrpt::maps;
27 using namespace mrpt::obs;
28 using namespace mrpt::math;
29 using namespace mrpt::system;
30 using namespace mrpt::poses;
31 using namespace std;
32 
34 
35 uint8_t CBeacon::serializeGetVersion() const { return 0; }
37 {
38  uint32_t i = m_ID;
39  uint32_t j = m_typePDF;
40  out << i << j << m_locationMC << m_locationGauss << m_locationSOG;
41 }
42 
44 {
45  switch (version)
46  {
47  case 0:
48  {
49  uint32_t i, j;
50  in >> i >> j >> m_locationMC >> m_locationGauss >> m_locationSOG;
51  m_ID = i;
52  m_typePDF = static_cast<TTypePDF>(j);
53  }
54  break;
55  default:
57  };
58 }
59 
60 /*---------------------------------------------------------------
61  getMean
62  ---------------------------------------------------------------*/
63 void CBeacon::getMean(CPoint3D& p) const
64 {
66  switch (m_typePDF)
67  {
68  case pdfMonteCarlo:
69  m_locationMC.getMean(p);
70  break;
71  case pdfGauss:
72  m_locationGauss.getMean(p);
73  break;
74  case pdfSOG:
75  m_locationSOG.getMean(p);
76  break;
77  default:
78  THROW_EXCEPTION("ERROR: Invalid 'm_typePDF' value");
79  };
80  MRPT_END
81 }
82 
83 std::tuple<CMatrixDouble33, CPoint3D> CBeacon::getCovarianceAndMean() const
84 {
86  switch (m_typePDF)
87  {
88  case pdfMonteCarlo:
89  return m_locationMC.getCovarianceAndMean();
90  case pdfGauss:
91  return m_locationGauss.getCovarianceAndMean();
92  case pdfSOG:
93  return m_locationSOG.getCovarianceAndMean();
94  default:
95  THROW_EXCEPTION("ERROR: Invalid 'm_typePDF' value");
96  };
97  MRPT_END
98 }
99 
100 /*---------------------------------------------------------------
101  bayesianFusion
102  ---------------------------------------------------------------*/
104  const CPointPDF& p1, const CPointPDF& p2,
105  const double minMahalanobisDistToDrop)
106 {
107  MRPT_START
108  switch (m_typePDF)
109  {
110  case pdfMonteCarlo:
111  m_locationMC.bayesianFusion(p1, p2, minMahalanobisDistToDrop);
112  break;
113  case pdfGauss:
114  m_locationGauss.bayesianFusion(p1, p2, minMahalanobisDistToDrop);
115  break;
116  case pdfSOG:
117  m_locationSOG.bayesianFusion(p1, p2, minMahalanobisDistToDrop);
118  break;
119  default:
120  THROW_EXCEPTION("ERROR: Invalid 'm_typePDF' value");
121  };
122  MRPT_END
123 }
124 
125 /*---------------------------------------------------------------
126  drawSingleSample
127  ---------------------------------------------------------------*/
128 void CBeacon::drawSingleSample(CPoint3D& outSample) const
129 {
130  MRPT_START
131  switch (m_typePDF)
132  {
133  case pdfMonteCarlo:
134  m_locationMC.drawSingleSample(outSample);
135  break;
136  case pdfGauss:
137  m_locationGauss.drawSingleSample(outSample);
138  break;
139  case pdfSOG:
140  m_locationSOG.drawSingleSample(outSample);
141  break;
142  default:
143  THROW_EXCEPTION("ERROR: Invalid 'm_typePDF' value");
144  };
145  MRPT_END
146 }
147 
148 /*---------------------------------------------------------------
149  copyFrom
150  ---------------------------------------------------------------*/
152 {
153  MRPT_START
154  switch (m_typePDF)
155  {
156  case pdfMonteCarlo:
157  m_locationMC.copyFrom(o);
158  break;
159  case pdfGauss:
160  m_locationGauss.copyFrom(o);
161  break;
162  case pdfSOG:
163  m_locationSOG.copyFrom(o);
164  break;
165  default:
166  THROW_EXCEPTION("ERROR: Invalid 'm_typePDF' value");
167  };
168  MRPT_END
169 }
170 
171 /*---------------------------------------------------------------
172  saveToTextFile
173  ---------------------------------------------------------------*/
174 bool CBeacon::saveToTextFile(const std::string& file) const
175 {
176  MRPT_START
177  switch (m_typePDF)
178  {
179  case pdfMonteCarlo:
180  return m_locationMC.saveToTextFile(file);
181  break;
182  case pdfGauss:
183  return m_locationGauss.saveToTextFile(file);
184  break;
185  case pdfSOG:
186  return m_locationSOG.saveToTextFile(file);
187  break;
188  default:
189  THROW_EXCEPTION("ERROR: Invalid 'm_typePDF' value");
190  };
191  MRPT_END
192 }
193 
194 /*---------------------------------------------------------------
195  changeCoordinatesReference
196  ---------------------------------------------------------------*/
197 void CBeacon::changeCoordinatesReference(const CPose3D& newReferenceBase)
198 {
199  MRPT_START
200  switch (m_typePDF)
201  {
202  case pdfMonteCarlo:
203  m_locationMC.changeCoordinatesReference(newReferenceBase);
204  break;
205  case pdfGauss:
206  m_locationGauss.changeCoordinatesReference(newReferenceBase);
207  break;
208  case pdfSOG:
209  m_locationSOG.changeCoordinatesReference(newReferenceBase);
210  break;
211  default:
212  THROW_EXCEPTION("ERROR: Invalid 'm_typePDF' value");
213  };
214  MRPT_END
215 }
216 
217 /*---------------------------------------------------------------
218  getAs3DObject
219  ---------------------------------------------------------------*/
221 {
222  MRPT_START
223 
224  switch (m_typePDF)
225  {
226  case pdfMonteCarlo:
227  {
229  std::make_shared<opengl::CPointCloud>();
230  obj->setColor(1, 0, 0);
231 
232  obj->setPointSize(2.5);
233 
234  const size_t N = m_locationMC.m_particles.size();
235  obj->resize(N);
236 
237  for (size_t i = 0; i < N; i++)
238  obj->setPoint(
239  i, m_locationMC.m_particles[i].d->x,
240  m_locationMC.m_particles[i].d->y,
241  m_locationMC.m_particles[i].d->z);
242 
243  outObj->insert(obj);
244  }
245  break;
246  case pdfGauss:
247  {
249  std::make_shared<opengl::CEllipsoid>();
250 
251  obj->setPose(m_locationGauss.mean);
252  obj->setLineWidth(3);
253 
254  CMatrixDouble C = CMatrixDouble(m_locationGauss.cov);
255  if (C(2, 2) == 0) C.setSize(2, 2);
256  obj->setCovMatrix(C);
257 
258  obj->setQuantiles(3);
259  obj->enableDrawSolid3D(false);
260 
261  obj->setColor(1, 0, 0, 0.85);
262  outObj->insert(obj);
263  }
264  break;
265  case pdfSOG:
266  {
267  m_locationSOG.getAs3DObject(outObj);
268  }
269  break;
270  default:
271  THROW_EXCEPTION("ERROR: Invalid 'm_typePDF' value");
272  };
273 
274  opengl::CText::Ptr obj2 = std::make_shared<opengl::CText>();
275  obj2->setString(format("#%d", static_cast<int>(m_ID)));
276 
277  CPoint3D meanP;
278  this->getMean(meanP);
279  obj2->setLocation(meanP.x() + 0.10, meanP.y() + 0.10, meanP.z());
280  outObj->insert(obj2);
281 
282  MRPT_END
283 }
284 
285 /*---------------------------------------------------------------
286  getAsMatlabDrawCommands
287  ---------------------------------------------------------------*/
288 void CBeacon::getAsMatlabDrawCommands(std::vector<std::string>& out_Str) const
289 {
290  MRPT_START
291 
292  out_Str.clear();
293  char auxStr[1000];
294 
295  switch (m_typePDF)
296  {
297  case pdfMonteCarlo:
298  {
299  // xs=[...];
300  // ys=[...];
301  // plot(xs,ys,'.','MarkerSize',4);
302  size_t i, N = m_locationMC.m_particles.size();
303  std::string sx, sy;
304 
305  sx = "xs=[";
306  sy = "ys=[";
307  for (i = 0; i < N; i++)
308  {
309  os::sprintf(
310  auxStr, sizeof(auxStr), "%.3f%c",
311  m_locationMC.m_particles[i].d->x, (i == N - 1) ? ' ' : ',');
312  sx = sx + std::string(auxStr);
313  os::sprintf(
314  auxStr, sizeof(auxStr), "%.3f%c",
315  m_locationMC.m_particles[i].d->y, (i == N - 1) ? ' ' : ',');
316  sy = sy + std::string(auxStr);
317  }
318  sx = sx + "];";
319  sy = sy + "];";
320  out_Str.emplace_back(sx);
321  out_Str.emplace_back(sy);
322  out_Str.emplace_back("plot(xs,ys,'k.','MarkerSize',4);");
323  }
324  break;
325  case pdfGauss:
326  {
327  // m=[x y];
328  // C=[2x2]
329  // error_ellipse(C,m,'conf',0.997,'style','k');
330 
331  os::sprintf(
332  auxStr, sizeof(auxStr), "m=[%.3f %.3f];",
333  m_locationGauss.mean.x(), m_locationGauss.mean.y());
334  out_Str.emplace_back(auxStr);
335  os::sprintf(
336  auxStr, sizeof(auxStr), "C=[%e %e;%e %e];",
337  m_locationGauss.cov(0, 0), m_locationGauss.cov(0, 1),
338  m_locationGauss.cov(1, 0), m_locationGauss.cov(1, 1));
339  out_Str.emplace_back(auxStr);
340 
341  out_Str.emplace_back(
342  "error_ellipse(C,m,'conf',0.997,'style','k');");
343  }
344  break;
345  case pdfSOG:
346  {
347  for (const auto& it : m_locationSOG)
348  {
349  os::sprintf(
350  auxStr, sizeof(auxStr), "m=[%.3f %.3f];", it.val.mean.x(),
351  it.val.mean.y());
352  out_Str.emplace_back(auxStr);
353  os::sprintf(
354  auxStr, sizeof(auxStr), "C=[%e %e;%e %e];",
355  it.val.cov(0, 0), it.val.cov(0, 1), it.val.cov(1, 0),
356  it.val.cov(1, 1));
357  out_Str.emplace_back(auxStr);
358  out_Str.emplace_back(
359  "error_ellipse(C,m,'conf',0.997,'style','k');");
360  }
361  }
362  break;
363  default:
364  THROW_EXCEPTION("ERROR: Invalid 'm_typePDF' value");
365  };
366 
367  // The text:
368  CPoint3D meanP;
369  getMean(meanP);
370 
371  os::sprintf(
372  auxStr, sizeof(auxStr), "text(%f,%f,'#%i');", meanP.x(), meanP.y(),
373  static_cast<int>(m_ID));
374  out_Str.emplace_back(auxStr);
375 
376  MRPT_END
377 }
378 
379 /*---------------------------------------------------------------
380  generateObservationModelDistribution
381  Compute the observation model p(z_t|x_t) for a given observation (range value),
382 and return it as an approximate SOG.
383 * Note that if the beacon is a SOG itself, the number of gaussian modes will be
384 square.
385 * As a speed-up, if a "center point"+"maxDistanceFromCenter" is supplied
386 (maxDistanceFromCenter!=0), those modes farther than this sphere will be
387 discarded.
388 * Parameters such as the stdSigma of the sensor are gathered from "myBeaconMap"
389 * The result is one "ring" for each Gaussian mode that represent the beacon
390 position in this object.
391 * The position of the sensor on the robot is used to shift the resulting
392 densities such as they represent the position of the robot, not the sensor.
393 * \sa CBeaconMap::insertionOptions, generateRingSOG
394 
395  ---------------------------------------------------------------*/
397  float sensedRange, CPointPDFSOG& outPDF, const CBeaconMap* myBeaconMap,
398  const CPoint3D& sensorPntOnRobot, const CPoint3D& centerPoint,
399  float maxDistanceFromCenter) const
400 {
401  MRPT_START
402 
403  std::unique_ptr<CPointPDFSOG> beaconPos;
404  const CPointPDFSOG* beaconPosToUse = nullptr;
405 
406  if (m_typePDF == pdfGauss)
407  {
408  // Copy the gaussian to the SOG:
409  auto new_beaconPos = std::make_unique<CPointPDFSOG>(1);
410  new_beaconPos->push_back(CPointPDFSOG::TGaussianMode());
411  new_beaconPos->get(0).log_w = 0;
412  new_beaconPos->get(0).val = m_locationGauss;
413  beaconPos = std::move(new_beaconPos);
414  beaconPosToUse = beaconPos.get();
415  }
416  else
417  {
418  ASSERT_(m_typePDF == pdfSOG);
419  beaconPosToUse = static_cast<const CPointPDFSOG*>(&m_locationSOG);
420  }
421 
422  outPDF.clear();
423 
424  for (const auto& beaconPo : *beaconPosToUse)
425  {
426  // The center of the ring to be generated
427  CPoint3D ringCenter(
428  beaconPo.val.mean.x() - sensorPntOnRobot.x(),
429  beaconPo.val.mean.y() - sensorPntOnRobot.y(),
430  beaconPo.val.mean.z() - sensorPntOnRobot.z());
431 
432  size_t startIdx = outPDF.size();
433 
435  sensedRange, // Sensed range
436  outPDF, // The ouput (Append all !!)
437  myBeaconMap, // For params
438  ringCenter, // The center of the ring to be generated
439  &beaconPo.val
440  .cov, // The covariance to ADD to each mode, due to the
441  // composition of uncertainty
442  false, // clearPreviousContentsOutPDF
443  centerPoint,
444  maxDistanceFromCenter // Directly, do not create too far modes
445  );
446 
447  // Adjust the weights to the one of "this" mode:
448  for (size_t k = startIdx; k < outPDF.size(); k++)
449  outPDF.get(k).log_w = beaconPo.log_w;
450  }
451 
452  MRPT_END
453 }
454 
455 /*---------------------------------------------------------------
456  generateRingSOG
457  ---------------------------------------------------------------*/
459  float R, CPointPDFSOG& outPDF, const CBeaconMap* myBeaconMap,
460  const CPoint3D& sensorPnt,
461  const CMatrixDouble33* covarianceCompositionToAdd,
462  bool clearPreviousContentsOutPDF, const CPoint3D& centerPoint,
463  float maxDistanceFromCenter)
464 {
465  MRPT_START
466 
467  ASSERT_(myBeaconMap);
468 
469  // Compute the number of Gaussians:
470  const float minEl = DEG2RAD(myBeaconMap->insertionOptions.minElevation_deg);
471  const float maxEl = DEG2RAD(myBeaconMap->insertionOptions.maxElevation_deg);
472  ASSERT_(
473  myBeaconMap->insertionOptions.minElevation_deg <=
474  myBeaconMap->insertionOptions.maxElevation_deg);
475 
476  double el, th, A_ang;
477  const float maxDistBetweenGaussians =
478  myBeaconMap->insertionOptions.SOG_maxDistBetweenGaussians; // Meters
479 
480  // B: Number of gaussians in each cut to the sphere (XY,XZ,...)
481  size_t B = (size_t)(M_2PIf * R / maxDistBetweenGaussians) + 1;
482 
483  // Assure minimum B (maximum angular increment):
484  B = max(B, (size_t)30);
485 
486  // B must be even:
487  if (B % 2) B++;
488 
489  A_ang = M_2PI / B; // Angular increments between modes:
490 
491  // The diagonal basic covariance matrix.
492  // (0,0) is the variance in the direction "in->out" of the sphere
493  // (1,1),(2,2) is the var. in the two directions tangent to the sphere
494  CMatrixDouble33 S;
495  S(0, 0) = square(myBeaconMap->likelihoodOptions.rangeStd);
496  S(1, 1) = S(2, 2) = square(
497  A_ang * R /
498  myBeaconMap->insertionOptions
499  .SOG_separationConstant); // 4.0f * sensedRange * S(0,0);
500 
501  CPoint3D dir;
502 
503  // Create the SOG:
504  size_t modeIdx;
505  if (clearPreviousContentsOutPDF)
506  {
507  // Overwrite modes:
508  modeIdx = 0;
509  outPDF.resize(B * B);
510  }
511  else
512  {
513  // Append modes:
514  modeIdx = outPDF.size(); // Start here
515  outPDF.resize(outPDF.size() + B * B);
516  }
517 
518  size_t idxEl, idxTh; // idxEl:[0,B/2+1]
519 
520  for (idxEl = 0; idxEl <= (1 + B / 2); idxEl++)
521  {
522  el = minEl + idxEl * A_ang;
523  if (el > (maxEl + 0.5 * A_ang)) continue;
524 
525  size_t nThSteps = B;
526  // Except if we are in the top/bottom of the sphere:
527  if (fabs(cos(el)) < 1e-4)
528  {
529  nThSteps = 1;
530  }
531 
532  for (idxTh = 0; idxTh < nThSteps; idxTh++)
533  {
534  th = idxTh * A_ang;
535 
536  // Compute the mean of the new Gaussian:
537  dir.x((sensorPnt.x() + R * cos(th) * cos(el)));
538  dir.y((sensorPnt.y() + R * sin(th) * cos(el)));
539  dir.z((sensorPnt.z() + R * sin(el)));
540 
541  // If we are provided a radius for not creating modes out of it,
542  // check it:
543  bool reallyCreateIt = true;
544  if (maxDistanceFromCenter > 0)
545  reallyCreateIt =
546  dir.distanceTo(centerPoint) < maxDistanceFromCenter;
547 
548  if (reallyCreateIt)
549  {
550  // All have equal log-weights:
551  outPDF.get(modeIdx).log_w = 0;
552 
553  // The mean:
554  outPDF.get(modeIdx).val.mean = dir;
555 
556  // Compute the covariance:
557  dir = dir - sensorPnt;
558  // 3 perpendicular & normalized vectors.
560  dir.x(), dir.y(), dir.z());
561 
562  // out = H * S * H';
563  outPDF.get(modeIdx).val.cov = mrpt::math::multiply_HCHt(H, S);
564 
565  if (std::abs(minEl - maxEl) < 1e-6)
566  { // We are in 2D:
567  // 3rd column/row = 0
568  CMatrixDouble33& C33 = outPDF.get(modeIdx).val.cov;
569  C33(0, 2) = C33(2, 0) = C33(1, 2) = C33(2, 1) = C33(2, 2) =
570  0;
571  }
572 
573  // Add covariance for uncertainty composition?
574  if (covarianceCompositionToAdd)
575  outPDF.get(modeIdx).val.cov += *covarianceCompositionToAdd;
576 
577  // One more mode is used:
578  modeIdx++;
579 
580  } // end if reallyCreateIt
581 
582  } // end for idxTh
583  } // end for idxEl
584 
585  // resize to the number of really used modes:
586  outPDF.resize(modeIdx);
587 
588  MRPT_END
589 }
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
Definition: CBeacon.cpp:83
#define MRPT_START
Definition: exceptions.h:241
static void generateRingSOG(float sensedRange, mrpt::poses::CPointPDFSOG &outPDF, const CBeaconMap *myBeaconMap, const mrpt::poses::CPoint3D &sensorPnt, const mrpt::math::CMatrixDouble33 *covarianceCompositionToAdd=nullptr, bool clearPreviousContentsOutPDF=true, const mrpt::poses::CPoint3D &centerPoint=mrpt::poses::CPoint3D(0, 0, 0), float maxDistanceFromCenter=0)
This static method returns a SOG with ring-shape (or as a 3D sphere) that can be used to initialize a...
Definition: CBeacon.cpp:458
#define M_2PI
Definition: common.h:58
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
void copyFrom(const mrpt::poses::CPointPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
Definition: CBeacon.cpp:151
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CBeacon.cpp:36
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void getAsMatlabDrawCommands(std::vector< std::string > &out_Str) const
Gets a set of MATLAB commands which draw the current state of the beacon:
Definition: CBeacon.cpp:288
CPoint3D mean
The mean value.
void resize(const size_t N)
Resize the number of SOG modes.
void changeCoordinatesReference(const mrpt::poses::CPose3D &newReferenceBase) override
this = p (+) this.
Definition: CBeacon.cpp:197
Declares a class that represents a Probability Density function (PDF) of a 3D point ...
Definition: CPointPDFSOG.h:33
The struct for each mode:
Definition: CPointPDFSOG.h:40
void clear()
Clear all the gaussian modes.
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
STL namespace.
#define M_2PIf
Definition: common.h:62
double maxElevation_deg
Minimum and maximum elevation angles (in degrees) for inserting new beacons at the first observation:...
Definition: CBeaconMap.h:149
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
void push_back(const TGaussianMode &m)
Inserts a copy of the given mode into the SOG.
Definition: CPointPDFSOG.h:99
T square(const T x)
Inline function for the square of a number.
constexpr double DEG2RAD(const double x)
Degrees to radians.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
float SOG_maxDistBetweenGaussians
A parameter for initializing 2D/3D SOGs.
Definition: CBeaconMap.h:186
This namespace contains representation of robot actions and observations.
A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian, or Sum of Gaussians (SOG) representation (for range-only SLAM).
Definition: CBeaconMap.h:43
float SOG_separationConstant
Constant used to compute the std.
Definition: CBeaconMap.h:191
const TGaussianMode & get(size_t i) const
Access to individual beacons.
Definition: CPointPDFSOG.h:86
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
A class used to store a 3D point.
Definition: CPoint3D.h:31
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
bool saveToTextFile(const std::string &file) const override
Save PDF&#39;s particles to a text file.
Definition: CBeacon.cpp:174
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 bayesianFusion(const CPointPDF &p1, const CPointPDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)
Definition: CBeacon.cpp:103
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
OPENGL_SETOFOBJECTSPTR getAs3DObject() const
Returns a 3D representation of this PDF.
Definition: CPointPDF.h:94
double rangeStd
The standard deviation used for Beacon ranges likelihood (default=0.08m).
Definition: CBeaconMap.h:122
size_t size() const
Return the number of Gaussian modes.
Definition: CPointPDFSOG.h:108
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
CMatrixDouble33 generateAxisBaseFromDirection(double dx, double dy, double dz)
Computes an axis base (a set of three 3D normal vectors) with the given vector being the first of the...
Definition: geometry.cpp:2583
#define MRPT_END
Definition: exceptions.h:245
TTypePDF
See m_typePDF.
Definition: CBeacon.h:46
mrpt::maps::CBeaconMap::TLikelihoodOptions likelihoodOptions
void drawSingleSample(mrpt::poses::CPoint3D &outSample) const override
Draw a sample from the pdf.
Definition: CBeacon.cpp:128
mrpt::maps::CBeaconMap::TInsertionOptions insertionOptions
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
Definition: CPointPDF.h:36
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CBeacon.cpp:43
void generateObservationModelDistribution(float sensedRange, mrpt::poses::CPointPDFSOG &outPDF, const CBeaconMap *myBeaconMap, const mrpt::poses::CPoint3D &sensorPntOnRobot, const mrpt::poses::CPoint3D &centerPoint=mrpt::poses::CPoint3D(0, 0, 0), float maxDistanceFromCenter=0) const
Compute the observation model p(z_t|x_t) for a given observation (range value), and return it as an a...
Definition: CBeacon.cpp:396
The class for storing individual "beacon landmarks" under a variety of 3D position PDF distributions...
Definition: CBeacon.h:35
int sprintf(char *buf, size_t bufSize, const char *format,...) noexcept MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
Definition: os.cpp:191
void getMean(mrpt::poses::CPoint3D &mean_point) const override
Definition: CBeacon.cpp:63
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
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: 7e629e01a Sat Dec 14 00:05:55 2019 +0100 at sáb dic 14 00:15:10 CET 2019