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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019