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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019