MRPT  2.0.4
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-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 "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>
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::CEllipsoid3D>();
250 
251  obj->setPose(m_locationGauss.mean);
252  obj->setLineWidth(3);
253 
254  CMatrixDouble C = CMatrixDouble(m_locationGauss.cov);
255  obj->setCovMatrix(C);
256 
257  obj->setQuantiles(3);
258  obj->enableDrawSolid3D(false);
259 
260  obj->setColor(1, 0, 0, 0.85f);
261  outObj->insert(obj);
262  }
263  break;
264  case pdfSOG:
265  {
266  m_locationSOG.getAs3DObject(outObj);
267  }
268  break;
269  default:
270  THROW_EXCEPTION("ERROR: Invalid 'm_typePDF' value");
271  };
272 
273  opengl::CText::Ptr obj2 = std::make_shared<opengl::CText>();
274  obj2->setString(format("#%d", static_cast<int>(m_ID)));
275 
276  CPoint3D meanP;
277  this->getMean(meanP);
278  obj2->setLocation(meanP.x() + 0.10, meanP.y() + 0.10, meanP.z());
279  outObj->insert(obj2);
280 
281  MRPT_END
282 }
283 
284 /*---------------------------------------------------------------
285  getAsMatlabDrawCommands
286  ---------------------------------------------------------------*/
287 void CBeacon::getAsMatlabDrawCommands(std::vector<std::string>& out_Str) const
288 {
289  MRPT_START
290 
291  out_Str.clear();
292  char auxStr[1000];
293 
294  switch (m_typePDF)
295  {
296  case pdfMonteCarlo:
297  {
298  // xs=[...];
299  // ys=[...];
300  // plot(xs,ys,'.','MarkerSize',4);
301  size_t i, N = m_locationMC.m_particles.size();
302  std::string sx, sy;
303 
304  sx = "xs=[";
305  sy = "ys=[";
306  for (i = 0; i < N; i++)
307  {
308  os::sprintf(
309  auxStr, sizeof(auxStr), "%.3f%c",
310  m_locationMC.m_particles[i].d->x, (i == N - 1) ? ' ' : ',');
311  sx = sx + std::string(auxStr);
312  os::sprintf(
313  auxStr, sizeof(auxStr), "%.3f%c",
314  m_locationMC.m_particles[i].d->y, (i == N - 1) ? ' ' : ',');
315  sy = sy + std::string(auxStr);
316  }
317  sx = sx + "];";
318  sy = sy + "];";
319  out_Str.emplace_back(sx);
320  out_Str.emplace_back(sy);
321  out_Str.emplace_back("plot(xs,ys,'k.','MarkerSize',4);");
322  }
323  break;
324  case pdfGauss:
325  {
326  // m=[x y];
327  // C=[2x2]
328  // error_ellipse(C,m,'conf',0.997,'style','k');
329 
330  os::sprintf(
331  auxStr, sizeof(auxStr), "m=[%.3f %.3f];",
332  m_locationGauss.mean.x(), m_locationGauss.mean.y());
333  out_Str.emplace_back(auxStr);
334  os::sprintf(
335  auxStr, sizeof(auxStr), "C=[%e %e;%e %e];",
336  m_locationGauss.cov(0, 0), m_locationGauss.cov(0, 1),
337  m_locationGauss.cov(1, 0), m_locationGauss.cov(1, 1));
338  out_Str.emplace_back(auxStr);
339 
340  out_Str.emplace_back(
341  "error_ellipse(C,m,'conf',0.997,'style','k');");
342  }
343  break;
344  case pdfSOG:
345  {
346  for (const auto& it : m_locationSOG)
347  {
348  os::sprintf(
349  auxStr, sizeof(auxStr), "m=[%.3f %.3f];", it.val.mean.x(),
350  it.val.mean.y());
351  out_Str.emplace_back(auxStr);
352  os::sprintf(
353  auxStr, sizeof(auxStr), "C=[%e %e;%e %e];",
354  it.val.cov(0, 0), it.val.cov(0, 1), it.val.cov(1, 0),
355  it.val.cov(1, 1));
356  out_Str.emplace_back(auxStr);
357  out_Str.emplace_back(
358  "error_ellipse(C,m,'conf',0.997,'style','k');");
359  }
360  }
361  break;
362  default:
363  THROW_EXCEPTION("ERROR: Invalid 'm_typePDF' value");
364  };
365 
366  // The text:
367  CPoint3D meanP;
368  getMean(meanP);
369 
370  os::sprintf(
371  auxStr, sizeof(auxStr), "text(%f,%f,'#%i');", meanP.x(), meanP.y(),
372  static_cast<int>(m_ID));
373  out_Str.emplace_back(auxStr);
374 
375  MRPT_END
376 }
377 
378 /*---------------------------------------------------------------
379  generateObservationModelDistribution
380  Compute the observation model p(z_t|x_t) for a given observation (range value),
381 and return it as an approximate SOG.
382 * Note that if the beacon is a SOG itself, the number of gaussian modes will be
383 square.
384 * As a speed-up, if a "center point"+"maxDistanceFromCenter" is supplied
385 (maxDistanceFromCenter!=0), those modes farther than this sphere will be
386 discarded.
387 * Parameters such as the stdSigma of the sensor are gathered from "myBeaconMap"
388 * The result is one "ring" for each Gaussian mode that represent the beacon
389 position in this object.
390 * The position of the sensor on the robot is used to shift the resulting
391 densities such as they represent the position of the robot, not the sensor.
392 * \sa CBeaconMap::insertionOptions, generateRingSOG
393 
394  ---------------------------------------------------------------*/
396  float sensedRange, CPointPDFSOG& outPDF, const CBeaconMap* myBeaconMap,
397  const CPoint3D& sensorPntOnRobot, const CPoint3D& centerPoint,
398  float maxDistanceFromCenter) const
399 {
400  MRPT_START
401 
402  std::unique_ptr<CPointPDFSOG> beaconPos;
403  const CPointPDFSOG* beaconPosToUse = nullptr;
404 
405  if (m_typePDF == pdfGauss)
406  {
407  // Copy the gaussian to the SOG:
408  auto new_beaconPos = std::make_unique<CPointPDFSOG>(1);
409  new_beaconPos->push_back(CPointPDFSOG::TGaussianMode());
410  new_beaconPos->get(0).log_w = 0;
411  new_beaconPos->get(0).val = m_locationGauss;
412  beaconPos = std::move(new_beaconPos);
413  beaconPosToUse = beaconPos.get();
414  }
415  else
416  {
417  ASSERT_(m_typePDF == pdfSOG);
418  beaconPosToUse = static_cast<const CPointPDFSOG*>(&m_locationSOG);
419  }
420 
421  outPDF.clear();
422 
423  for (const auto& beaconPo : *beaconPosToUse)
424  {
425  // The center of the ring to be generated
426  CPoint3D ringCenter(
427  beaconPo.val.mean.x() - sensorPntOnRobot.x(),
428  beaconPo.val.mean.y() - sensorPntOnRobot.y(),
429  beaconPo.val.mean.z() - sensorPntOnRobot.z());
430 
431  size_t startIdx = outPDF.size();
432 
434  sensedRange, // Sensed range
435  outPDF, // The ouput (Append all !!)
436  myBeaconMap, // For params
437  ringCenter, // The center of the ring to be generated
438  &beaconPo.val
439  .cov, // The covariance to ADD to each mode, due to the
440  // composition of uncertainty
441  false, // clearPreviousContentsOutPDF
442  centerPoint,
443  maxDistanceFromCenter // Directly, do not create too far modes
444  );
445 
446  // Adjust the weights to the one of "this" mode:
447  for (size_t k = startIdx; k < outPDF.size(); k++)
448  outPDF.get(k).log_w = beaconPo.log_w;
449  }
450 
451  MRPT_END
452 }
453 
454 /*---------------------------------------------------------------
455  generateRingSOG
456  ---------------------------------------------------------------*/
458  float R, CPointPDFSOG& outPDF, const CBeaconMap* myBeaconMap,
459  const CPoint3D& sensorPnt,
460  const CMatrixDouble33* covarianceCompositionToAdd,
461  bool clearPreviousContentsOutPDF, const CPoint3D& centerPoint,
462  float maxDistanceFromCenter)
463 {
464  MRPT_START
465 
466  ASSERT_(myBeaconMap);
467 
468  // Compute the number of Gaussians:
469  const double minEl =
471  const double maxEl =
473  ASSERT_(
474  myBeaconMap->insertionOptions.minElevation_deg <=
475  myBeaconMap->insertionOptions.maxElevation_deg);
476 
477  double el, th, A_ang;
478  const float maxDistBetweenGaussians =
479  myBeaconMap->insertionOptions.SOG_maxDistBetweenGaussians; // Meters
480 
481  // B: Number of gaussians in each cut to the sphere (XY,XZ,...)
482  size_t B = (size_t)(M_2PIf * R / maxDistBetweenGaussians) + 1;
483 
484  // Assure minimum B (maximum angular increment):
485  B = max(B, (size_t)30);
486 
487  // B must be even:
488  if (B % 2) B++;
489 
490  A_ang = M_2PI / B; // Angular increments between modes:
491 
492  // The diagonal basic covariance matrix.
493  // (0,0) is the variance in the direction "in->out" of the sphere
494  // (1,1),(2,2) is the var. in the two directions tangent to the sphere
495  CMatrixDouble33 S;
496  S(0, 0) = square(myBeaconMap->likelihoodOptions.rangeStd);
497  S(1, 1) = S(2, 2) = square(
498  A_ang * R /
499  myBeaconMap->insertionOptions
500  .SOG_separationConstant); // 4.0f * sensedRange * S(0,0);
501 
502  CPoint3D dir;
503 
504  // Create the SOG:
505  size_t modeIdx;
506  if (clearPreviousContentsOutPDF)
507  {
508  // Overwrite modes:
509  modeIdx = 0;
510  outPDF.resize(B * B);
511  }
512  else
513  {
514  // Append modes:
515  modeIdx = outPDF.size(); // Start here
516  outPDF.resize(outPDF.size() + B * B);
517  }
518 
519  size_t idxEl, idxTh; // idxEl:[0,B/2+1]
520 
521  for (idxEl = 0; idxEl <= (1 + B / 2); idxEl++)
522  {
523  el = minEl + idxEl * A_ang;
524  if (el > (maxEl + 0.5 * A_ang)) continue;
525 
526  size_t nThSteps = B;
527  // Except if we are in the top/bottom of the sphere:
528  if (fabs(cos(el)) < 1e-4)
529  {
530  nThSteps = 1;
531  }
532 
533  for (idxTh = 0; idxTh < nThSteps; idxTh++)
534  {
535  th = idxTh * A_ang;
536 
537  // Compute the mean of the new Gaussian:
538  dir.x((sensorPnt.x() + R * cos(th) * cos(el)));
539  dir.y((sensorPnt.y() + R * sin(th) * cos(el)));
540  dir.z((sensorPnt.z() + R * sin(el)));
541 
542  // If we are provided a radius for not creating modes out of it,
543  // check it:
544  bool reallyCreateIt = true;
545  if (maxDistanceFromCenter > 0)
546  reallyCreateIt =
547  dir.distanceTo(centerPoint) < maxDistanceFromCenter;
548 
549  if (reallyCreateIt)
550  {
551  // All have equal log-weights:
552  outPDF.get(modeIdx).log_w = 0;
553 
554  // The mean:
555  outPDF.get(modeIdx).val.mean = dir;
556 
557  // Compute the covariance:
558  dir = dir - sensorPnt;
559  // 3 perpendicular & normalized vectors.
561  dir.x(), dir.y(), dir.z());
562 
563  // out = H * S * H';
564  outPDF.get(modeIdx).val.cov = mrpt::math::multiply_HCHt(H, S);
565 
566  if (std::abs(minEl - maxEl) < 1e-6)
567  { // We are in 2D:
568  // 3rd column/row = 0
569  CMatrixDouble33& C33 = outPDF.get(modeIdx).val.cov;
570  C33(0, 2) = C33(2, 0) = C33(1, 2) = C33(2, 1) = C33(2, 2) =
571  0;
572  }
573 
574  // Add covariance for uncertainty composition?
575  if (covarianceCompositionToAdd)
576  outPDF.get(modeIdx).val.cov += *covarianceCompositionToAdd;
577 
578  // One more mode is used:
579  modeIdx++;
580 
581  } // end if reallyCreateIt
582 
583  } // end for idxTh
584  } // end for idxEl
585 
586  // resize to the number of really used modes:
587  outPDF.resize(modeIdx);
588 
589  MRPT_END
590 }
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:457
#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:287
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
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...
return_t square(const num_t x)
Inline function for the square of a number.
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
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:2502
#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:395
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...
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 2.0.4 Git: 7b5ddf9de Fri May 29 14:02:56 2020 +0200 at vie may 29 14:15:09 CEST 2020