Main MRPT website > C++ reference for MRPT 1.9.9
CBeaconMap.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/CBeaconMap.h>
13 #include <mrpt/obs/CObservationBeaconRanges.h>
14 #include <mrpt/random.h>
15 #include <mrpt/utils/CFileOutputStream.h>
16 #include <mrpt/utils/CConfigFileBase.h>
17 #include <mrpt/utils/round.h> // round()
18 #include <mrpt/math/geometry.h>
19 #include <mrpt/bayes/CParticleFilterCapable.h>
20 #include <mrpt/bayes/CParticleFilter.h>
21 #include <mrpt/math/data_utils.h> // averageLogLikelihood()
22 #include <mrpt/system/os.h>
23 #include <mrpt/utils/CStream.h>
24 
25 #include <mrpt/opengl/COpenGLScene.h>
26 #include <mrpt/opengl/CSetOfObjects.h>
27 #include <mrpt/opengl/CGridPlaneXY.h>
28 #include <mrpt/opengl/stock_objects.h>
29 
30 using namespace mrpt;
31 using namespace mrpt::maps;
32 using namespace mrpt::math;
33 using namespace mrpt::obs;
34 using namespace mrpt::utils;
35 using namespace mrpt::random;
36 using namespace mrpt::poses;
37 using namespace mrpt::bayes;
38 using namespace mrpt::system;
39 using namespace std;
40 
41 // =========== Begin of Map definition ============
42 MAP_DEFINITION_REGISTER("CBeaconMap,beaconMap", mrpt::maps::CBeaconMap)
43 
44 CBeaconMap::TMapDefinition::TMapDefinition() {}
45 void CBeaconMap::TMapDefinition::loadFromConfigFile_map_specific(
46  const mrpt::utils::CConfigFileBase& source,
47  const std::string& sectionNamePrefix)
48 {
49  // [<sectionNamePrefix>+"_creationOpts"]
50  // const std::string sSectCreation =
51  // sectionNamePrefix+string("_creationOpts");
52  // MRPT_LOAD_CONFIG_VAR(resolution, float, source,sSectCreation);
53 
54  insertionOpts.loadFromConfigFile(
55  source, sectionNamePrefix + string("_insertOpts"));
56  likelihoodOpts.loadFromConfigFile(
57  source, sectionNamePrefix + string("_likelihoodOpts"));
58 }
59 
60 void CBeaconMap::TMapDefinition::dumpToTextStream_map_specific(
61  mrpt::utils::CStream& out) const
62 {
63  // LOADABLEOPTS_DUMP_VAR(resolution , float);
64 
65  this->insertionOpts.dumpToTextStream(out);
66  this->likelihoodOpts.dumpToTextStream(out);
67 }
68 
69 mrpt::maps::CMetricMap* CBeaconMap::internal_CreateFromMapDefinition(
70  const mrpt::maps::TMetricMapInitializer& _def)
71 {
72  const CBeaconMap::TMapDefinition& def =
73  *dynamic_cast<const CBeaconMap::TMapDefinition*>(&_def);
74  CBeaconMap* obj = new CBeaconMap();
75  obj->insertionOptions = def.insertionOpts;
76  obj->likelihoodOptions = def.likelihoodOpts;
77  return obj;
78 }
79 // =========== End of Map definition Block =========
80 
81 IMPLEMENTS_SERIALIZABLE(CBeaconMap, CMetricMap, mrpt::maps)
82 
83 /*---------------------------------------------------------------
84  Constructor
85  ---------------------------------------------------------------*/
86 CBeaconMap::CBeaconMap() : m_beacons(), likelihoodOptions(), insertionOptions()
87 {
88 }
89 
90 /*---------------------------------------------------------------
91  clear
92  ---------------------------------------------------------------*/
93 void CBeaconMap::internal_clear() { m_beacons.clear(); }
94 /*---------------------------------------------------------------
95  getLandmarksCount
96  ---------------------------------------------------------------*/
97 size_t CBeaconMap::size() const { return m_beacons.size(); }
98 /*---------------------------------------------------------------
99  Resize
100  ---------------------------------------------------------------*/
101 void CBeaconMap::resize(const size_t N) { m_beacons.resize(N); }
102 /*---------------------------------------------------------------
103  writeToStream
104  Implements the writing to a CStream capability of
105  CSerializable objects
106  ---------------------------------------------------------------*/
107 void CBeaconMap::writeToStream(mrpt::utils::CStream& out, int* version) const
108 {
109  if (version)
110  *version = 1;
111  else
112  {
113  out << genericMapParams; // v1
114 
115  // First, write the number of landmarks:
116  const uint32_t n = m_beacons.size();
117  out << n;
118  // Write all landmarks:
119  for (const_iterator it = begin(); it != end(); ++it) out << (*it);
120  }
121 }
122 
123 /*---------------------------------------------------------------
124  readFromStream
125  Implements the reading from a CStream capability of
126  CSerializable objects
127  ---------------------------------------------------------------*/
128 void CBeaconMap::readFromStream(mrpt::utils::CStream& in, int version)
129 {
130  switch (version)
131  {
132  case 0:
133  case 1:
134  {
135  if (version >= 1) in >> genericMapParams; // v1
136 
137  uint32_t n, i;
138 
139  // Delete previous content of map:
140  clear();
141 
142  // Load from stream:
143  // Read all landmarks:
144  in >> n;
145  m_beacons.resize(n);
146  for (i = 0; i < n; i++) in >> m_beacons[i];
147  }
148  break;
149  default:
150  MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
151  };
152 }
153 
154 /*---------------------------------------------------------------
155  computeObservationLikelihood
156  ---------------------------------------------------------------*/
157 double CBeaconMap::internal_computeObservationLikelihood(
158  const CObservation* obs, const CPose3D& robotPose3D)
159 {
160  MRPT_START
161 
162  /* ===============================================================================================================
163  Refer to the papers:
164  - IROS 2008, "Efficient Probabilistic Range-Only SLAM",
165  http://www.mrpt.org/paper-ro-slam-with-sog/
166 
167  - ICRA 2008, "A Pure Probabilistic Approach to Range-Only SLAM",
168  http://www.mrpt.org/tutorials/slam-algorithms/rangeonly_slam/
169  ===============================================================================================================
170  */
171 
172  if (CLASS_ID(CObservationBeaconRanges) == obs->GetRuntimeClass())
173  {
174  /********************************************************************
175 
176  OBSERVATION TYPE: CObservationBeaconRanges
177 
178  Lik. between "this" and "auxMap";
179 
180  ********************************************************************/
181  double ret = 0;
182  const CObservationBeaconRanges* o =
183  static_cast<const CObservationBeaconRanges*>(obs);
184  deque<CObservationBeaconRanges::TMeasurement>::const_iterator it_obs;
185  const CBeacon* beac;
186  CPoint3D sensor3D;
187 
188  for (it_obs = o->sensedData.begin(); it_obs != o->sensedData.end();
189  ++it_obs)
190  {
191  // Look for the beacon in this map:
192  beac = getBeaconByID(it_obs->beaconID);
193 
194  if (beac != nullptr && it_obs->sensedDistance > 0 &&
195  !std::isnan(it_obs->sensedDistance))
196  {
197  float sensedRange = it_obs->sensedDistance;
198 
199  // FOUND: Compute the likelihood function:
200  // -----------------------------------------------------
201  // Compute the 3D position of the sensor:
202  sensor3D = robotPose3D + it_obs->sensorLocationOnRobot;
203 
204  // Depending on the PDF type of the beacon in the map:
205  switch (beac->m_typePDF)
206  {
207  // ------------------------------
208  // PDF is MonteCarlo
209  // ------------------------------
210  case CBeacon::pdfMonteCarlo:
211  {
212  CPointPDFParticles::CParticleList::const_iterator it;
213  CVectorDouble logWeights(
214  beac->m_locationMC.m_particles.size());
215  CVectorDouble logLiks(
216  beac->m_locationMC.m_particles.size());
217  CVectorDouble::iterator itLW, itLL;
218 
219  for (it = beac->m_locationMC.m_particles.begin(),
220  itLW = logWeights.begin(), itLL = logLiks.begin();
221  it != beac->m_locationMC.m_particles.end();
222  ++it, ++itLW, ++itLL)
223  {
224  float expectedRange = sensor3D.distance3DTo(
225  it->d->x, it->d->y, it->d->z);
226  // expectedRange +=
227  // float(0.1*(1-exp(-0.16*expectedRange)));
228 
229  *itLW = it->log_w; // Linear weight of this
230  // likelihood component
231  *itLL = -0.5 * square(
232  (sensedRange - expectedRange) /
233  likelihoodOptions.rangeStd);
234  // ret+= exp(
235  // -0.5*square((sensedRange-expectedRange)/likelihoodOptions.rangeStd)
236  // );
237  } // end for it
238 
239  if (logWeights.size())
240  ret += math::averageLogLikelihood(
241  logWeights, logLiks); // A numerically-stable
242  // method to average the
243  // likelihoods
244  }
245  break;
246  // ------------------------------
247  // PDF is Gaussian
248  // ------------------------------
249  case CBeacon::pdfGauss:
250  {
251  // Compute the Jacobian H and varZ
252  CMatrixFixedNumeric<double, 1, 3> H;
253  float varZ, varR = square(likelihoodOptions.rangeStd);
254  float Ax =
255  beac->m_locationGauss.mean.x() - sensor3D.x();
256  float Ay =
257  beac->m_locationGauss.mean.y() - sensor3D.y();
258  float Az =
259  beac->m_locationGauss.mean.z() - sensor3D.z();
260  H(0, 0) = Ax;
261  H(0, 1) = Ay;
262  H(0, 2) = Az;
263  float expectedRange =
264  sensor3D.distanceTo(beac->m_locationGauss.mean);
265  H *= 1.0 / expectedRange; // sqrt(Ax*Ax+Ay*Ay+Az*Az);
266 
267  varZ =
268  H.multiply_HCHt_scalar(beac->m_locationGauss.cov);
269 
270  varZ += varR;
271 
272  // Compute the mean expected range (add bias!):
273  // expectedRange +=
274  // float(0.1*(1-exp(-0.16*expectedRange)));
275 
276  // Compute the likelihood:
277  // lik \propto exp( -0.5* ( ^z - z )^2 / varZ );
278  // log_lik = -0.5* ( ^z - z )^2 / varZ
279  ret +=
280  -0.5 * square(sensedRange - expectedRange) / varZ;
281  }
282  break;
283  // ------------------------------
284  // PDF is SOG
285  // ------------------------------
286  case CBeacon::pdfSOG:
287  {
288  CMatrixDouble13 H;
289  CVectorDouble logWeights(beac->m_locationSOG.size());
290  CVectorDouble logLiks(beac->m_locationSOG.size());
291  CVectorDouble::iterator itLW, itLL;
292  CPointPDFSOG::const_iterator it;
293  // For each Gaussian mode:
294  for (it = beac->m_locationSOG.begin(),
295  itLW = logWeights.begin(), itLL = logLiks.begin();
296  it != beac->m_locationSOG.end();
297  ++it, ++itLW, ++itLL)
298  {
299  // Compute the Jacobian H and varZ
300  double varZ,
301  varR = square(likelihoodOptions.rangeStd);
302  double Ax = it->val.mean.x() - sensor3D.x();
303  double Ay = it->val.mean.y() - sensor3D.y();
304  double Az = it->val.mean.z() - sensor3D.z();
305  H(0, 0) = Ax;
306  H(0, 1) = Ay;
307  H(0, 2) = Az;
308  double expectedRange =
309  sensor3D.distanceTo(it->val.mean);
310  H *= 1.0 /
311  expectedRange; // sqrt(Ax*Ax+Ay*Ay+Az*Az);
312 
313  varZ = H.multiply_HCHt_scalar(it->val.cov);
314  varZ += varR;
315 
316  // Compute the mean expected range (add bias!):
317  // expectedRange +=
318  // float(0.1*(1-exp(-0.16*expectedRange)));
319 
320  // Compute the likelihood:
321  *itLW = it->log_w; // log-weight of this likelihood
322  // component
323  *itLL = -0.5 * square(sensedRange - expectedRange) /
324  varZ;
325  } // end for each mode
326 
327  // Accumulate to the overall (log) likelihood value:
328  if (logWeights.size())
329  ret += math::averageLogLikelihood(
330  logWeights,
331  logLiks); // log( linear_lik / sumW );
332  }
333  break;
334 
335  default:
336  THROW_EXCEPTION("Invalid beac->m_typePDF!!!");
337  };
338  }
339  else
340  {
341  // If not found, a uniform distribution:
342  if (o->maxSensorDistance != o->minSensorDistance)
343  ret += log(
344  1.0 / (o->maxSensorDistance - o->minSensorDistance));
345  }
346  } // for each sensed beacon "it"
347 
348  // printf("ret: %e\n",ret);
349  MRPT_CHECK_NORMAL_NUMBER(ret);
350  return ret;
351 
352  } // end of likelihood of CObservationBeaconRanges
353  else
354  {
355  /********************************************************************
356  OBSERVATION TYPE: Unknown
357  ********************************************************************/
358  return 0;
359  }
360  MRPT_END
361 }
362 
363 /*---------------------------------------------------------------
364  insertObservation
365  ---------------------------------------------------------------*/
366 bool CBeaconMap::internal_insertObservation(
367  const mrpt::obs::CObservation* obs, const CPose3D* robotPose)
368 {
369  MRPT_START
370 
371  CPose2D robotPose2D;
372  CPose3D robotPose3D;
373 
374  if (robotPose)
375  {
376  robotPose2D = CPose2D(*robotPose);
377  robotPose3D = (*robotPose);
378  }
379  else
380  {
381  // Default values are (0,0,0)
382  }
383 
384  if (CLASS_ID(CObservationBeaconRanges) == obs->GetRuntimeClass())
385  {
386  /********************************************************************
387  OBSERVATION TYPE: CObservationBeaconRanges
388  ********************************************************************/
389 
390  /* ===============================================================================================================
391  Refer to the papers:
392  - IROS 2008, "Efficient Probabilistic Range-Only SLAM",
393  http://www.mrpt.org/paper-ro-slam-with-sog/
394 
395  - ICRA 2008, "A Pure Probabilistic Approach to Range-Only SLAM",
396  http://www.mrpt.org/tutorials/slam-algorithms/rangeonly_slam/
397  ===============================================================================================================
398  */
399 
400  // Here we fuse OR create the beacon position PDF:
401  // --------------------------------------------------------
402  const CObservationBeaconRanges* o =
403  static_cast<const CObservationBeaconRanges*>(obs);
404 
405  for (deque<CObservationBeaconRanges::TMeasurement>::const_iterator it =
406  o->sensedData.begin();
407  it != o->sensedData.end(); ++it)
408  {
409  CPoint3D sensorPnt(robotPose3D + it->sensorLocationOnRobot);
410  float sensedRange = it->sensedDistance;
411  unsigned int sensedID = it->beaconID;
412 
413  CBeacon* beac = getBeaconByID(sensedID);
414 
415  if (sensedRange > 0) // Only sensible range values!
416  {
417  if (!beac)
418  {
419  // ======================================
420  // INSERT
421  // ======================================
422  CBeacon newBeac;
423  newBeac.m_ID = sensedID;
424 
425  if (insertionOptions.insertAsMonteCarlo)
426  {
427  // Insert as a new set of samples:
428  // ------------------------------------------------
429 
430  newBeac.m_typePDF = CBeacon::pdfMonteCarlo;
431 
432  size_t numParts = round(
433  insertionOptions.MC_numSamplesPerMeter *
434  sensedRange);
435  ASSERT_(
436  insertionOptions.minElevation_deg <=
437  insertionOptions.maxElevation_deg)
438  double minA =
439  DEG2RAD(insertionOptions.minElevation_deg);
440  double maxA =
441  DEG2RAD(insertionOptions.maxElevation_deg);
442  newBeac.m_locationMC.setSize(numParts);
443  for (CPointPDFParticles::CParticleList::iterator itP =
444  newBeac.m_locationMC.m_particles.begin();
445  itP != newBeac.m_locationMC.m_particles.end();
446  ++itP)
447  {
448  double th =
449  getRandomGenerator().drawUniform(-M_PI, M_PI);
450  double el = getRandomGenerator().drawUniform(minA, maxA);
451  double R = getRandomGenerator().drawGaussian1D(
452  sensedRange, likelihoodOptions.rangeStd);
453  itP->d->x = sensorPnt.x() + R * cos(th) * cos(el);
454  itP->d->y = sensorPnt.y() + R * sin(th) * cos(el);
455  itP->d->z = sensorPnt.z() + R * sin(el);
456  } // end for itP
457  }
458  else
459  {
460  // Insert as a Sum of Gaussians:
461  // ------------------------------------------------
462  newBeac.m_typePDF = CBeacon::pdfSOG;
463  CBeacon::generateRingSOG(
464  sensedRange, // Sensed range
465  newBeac.m_locationSOG, // Output SOG
466  this, // My CBeaconMap, for options.
467  sensorPnt // Sensor point
468  );
469  }
470 
471  // and insert it:
472  m_beacons.push_back(newBeac);
473 
474  } // end insert
475  else
476  {
477  // ======================================
478  // FUSE
479  // ======================================
480  switch (beac->m_typePDF)
481  {
482  // ------------------------------
483  // FUSE: PDF is MonteCarlo
484  // ------------------------------
485  case CBeacon::pdfMonteCarlo:
486  {
487  double maxW = -1e308, sumW = 0;
488  // Update weights:
489  // --------------------
490  for (CPointPDFParticles::CParticleList::iterator
491  it =
492  beac->m_locationMC.m_particles.begin();
493  it != beac->m_locationMC.m_particles.end();
494  ++it)
495  {
496  float expectedRange = sensorPnt.distance3DTo(
497  it->d->x, it->d->y, it->d->z);
498  // Add bias:
499  // expectedRange +=
500  // float(0.1*(1-exp(-0.16*expectedRange)));
501  it->log_w +=
502  -0.5 * square(
503  (sensedRange - expectedRange) /
504  likelihoodOptions.rangeStd);
505  maxW = max(it->log_w, maxW);
506  sumW += exp(it->log_w);
507  } // end for it
508 
509  // Perform resampling (SIR filter) or not (simply
510  // accumulate weights)??
511  // ------------------------------------------------------------------------
512  if (insertionOptions.MC_performResampling)
513  {
514  // Yes, perform an auxiliary PF SIR here:
515  // ---------------------------------------------
516  if (beac->m_locationMC.ESS() < 0.5)
517  {
518  // We must resample:
519  // Make a list with the log weights:
520  vector<double> log_ws;
521  vector<size_t> indxs;
522  beac->m_locationMC.getWeights(log_ws);
523 
524  // And compute the resampled indexes:
525  CParticleFilterCapable::computeResampling(
526  CParticleFilter::prSystematic, log_ws,
527  indxs);
528 
529  // Replace with the new samples:
530  beac->m_locationMC.performSubstitution(
531  indxs);
532 
533  // Determine if this is a 2D beacon map:
534  bool is2D =
535  (insertionOptions.minElevation_deg ==
536  insertionOptions.maxElevation_deg);
537  float noiseStd =
538  insertionOptions
539  .MC_afterResamplingNoise;
540 
541  // AND, add a small noise:
542  CPointPDFParticles::CParticleList::iterator
543  itSample;
544  for (itSample = beac->m_locationMC
545  .m_particles.begin();
546  itSample !=
547  beac->m_locationMC.m_particles.end();
548  ++itSample)
549  {
550  itSample->d->x +=
551  getRandomGenerator().drawGaussian1D(
552  0, noiseStd);
553  itSample->d->y +=
554  getRandomGenerator().drawGaussian1D(
555  0, noiseStd);
556  if (!is2D)
557  itSample->d->z +=
558  getRandomGenerator().drawGaussian1D(
559  0, noiseStd);
560  }
561  }
562  } // end "do resample"
563  else
564  {
565  // Do not resample:
566  // ---------------------------------------------
567 
568  // Remove very very very unlikely particles:
569  // -------------------------------------------
570  for (CPointPDFParticles::CParticleList::iterator
571  it = beac->m_locationMC.m_particles
572  .begin();
573  it !=
574  beac->m_locationMC.m_particles.end();)
575  {
576  if (it->log_w <
577  (maxW -
578  insertionOptions
579  .MC_thresholdNegligible))
580  {
581  it->d.reset();
582  it = beac->m_locationMC.m_particles
583  .erase(it);
584  }
585  else
586  ++it;
587  }
588  } // end "do not resample"
589 
590  // Normalize weights:
591  // log_w = log( exp(log_w)/sumW ) ->
592  // log_w -= log(sumW);
593  // -----------------------------------------
594  sumW = log(sumW);
595  for (CPointPDFParticles::CParticleList::iterator
596  it =
597  beac->m_locationMC.m_particles.begin();
598  it != beac->m_locationMC.m_particles.end();
599  ++it)
600  it->log_w -= sumW;
601 
602  // Is the moment to turn into a Gaussian??
603  // -------------------------------------------
604  CPoint3D MEAN;
605  CMatrixDouble33 COV;
606  beac->m_locationMC.getCovarianceAndMean(COV, MEAN);
607 
608  double D1 = sqrt(COV(0, 0));
609  double D2 = sqrt(COV(1, 1));
610  double D3 = sqrt(COV(2, 2));
611 
612  double mxVar = max3(D1, D2, D3);
613 
614  if (mxVar < insertionOptions.MC_maxStdToGauss)
615  {
616  // Collapse into Gaussian:
617  beac->m_locationMC
618  .clear(); // Erase prev. samples
619 
620  // Assure a non-null covariance!
621  CMatrixDouble COV2 = CMatrixDouble(COV);
622  COV2.setSize(2, 2);
623  if (COV2.det() == 0)
624  {
625  COV.setIdentity();
626  COV *= square(0.01f);
627  if (insertionOptions.minElevation_deg ==
628  insertionOptions.maxElevation_deg)
629  COV(2, 2) = 0; // We are in a 2D map:
630  }
631 
632  beac->m_typePDF =
633  CBeacon::pdfGauss; // Pass to gaussian.
634  beac->m_locationGauss.mean = MEAN;
635  beac->m_locationGauss.cov = COV;
636  }
637  }
638  break;
639 
640  // ------------------------------
641  // FUSE: PDF is Gaussian:
642  // ------------------------------
643  case CBeacon::pdfGauss:
644  {
645  // Compute the mean expected range:
646  float expectedRange = sensorPnt.distanceTo(
647  beac->m_locationGauss.mean);
648  float varR = square(likelihoodOptions.rangeStd);
649  // bool useEKF_or_KF = true;
650 
651  // if (useEKF_or_KF)
652  {
653  // EKF method:
654  // ---------------------
655  // Add bias:
656  // expectedRange +=
657  // float(0.1*(1-exp(-0.16*expectedRange)));
658 
659  // An EKF for updating the Gaussian:
660  float y = sensedRange - expectedRange;
661 
662  // Compute the Jacobian H and varZ
663  CMatrixDouble13 H;
664  double varZ;
665  double Ax =
666  (beac->m_locationGauss.mean.x() -
667  sensorPnt.x());
668  double Ay =
669  (beac->m_locationGauss.mean.y() -
670  sensorPnt.y());
671  double Az =
672  (beac->m_locationGauss.mean.z() -
673  sensorPnt.z());
674  H(0, 0) = Ax;
675  H(0, 1) = Ay;
676  H(0, 2) = Az;
677  H *= 1.0 /
678  expectedRange; // sqrt(Ax*Ax+Ay*Ay+Az*Az);
679  varZ = H.multiply_HCHt_scalar(
680  beac->m_locationGauss.cov);
681  varZ += varR;
682 
683  CMatrixDouble31 K;
684  K.multiply_ABt(beac->m_locationGauss.cov, H);
685  K *= 1.0 / varZ;
686 
687  // Update stage of the EKF:
688  beac->m_locationGauss.mean.x_incr(K(0, 0) * y);
689  beac->m_locationGauss.mean.y_incr(K(1, 0) * y);
690  beac->m_locationGauss.mean.z_incr(K(2, 0) * y);
691 
692  beac->m_locationGauss.cov =
693  (Eigen::Matrix<double, 3, 3>::Identity() -
694  K * H) *
695  beac->m_locationGauss.cov;
696  // beac->m_locationGauss.cov.force_symmetry();
697  }
698  }
699  break;
700  // ------------------------------
701  // FUSE: PDF is SOG
702  // ------------------------------
703  case CBeacon::pdfSOG:
704  {
705  // Compute the mean expected range for this mode:
706  float varR = square(likelihoodOptions.rangeStd);
707 
708  // For each Gaussian mode:
709  // 1) Update its weight (using the likelihood of
710  // the observation linearized at the mean)
711  // 2) Update its mean/cov (as in the simple EKF)
712  CPointPDFSOG::iterator it;
713  double max_w = -1e9;
714  for (it = beac->m_locationSOG.begin();
715  it != beac->m_locationSOG.end(); ++it)
716  {
717  double expectedRange =
718  sensorPnt.distanceTo(it->val.mean);
719 
720  // An EKF for updating the Gaussian:
721  double y = sensedRange - expectedRange;
722 
723  // Compute the Jacobian H and varZ
724  CMatrixDouble13 H;
725  double varZ;
726  double Ax = (it->val.mean.x() - sensorPnt.x());
727  double Ay = (it->val.mean.y() - sensorPnt.y());
728  double Az = (it->val.mean.z() - sensorPnt.z());
729  H(0, 0) = Ax;
730  H(0, 1) = Ay;
731  H(0, 2) = Az;
732  H *= 1.0 /
733  expectedRange; // sqrt(Ax*Ax+Ay*Ay+Az*Az);
734  varZ = H.multiply_HCHt_scalar(it->val.cov);
735  varZ += varR;
736  CMatrixDouble31 K;
737  K.multiply(it->val.cov, H.transpose());
738  K *= 1.0 / varZ;
739 
740  // Update stage of the EKF:
741  it->val.mean.x_incr(K(0, 0) * y);
742  it->val.mean.y_incr(K(1, 0) * y);
743  it->val.mean.z_incr(K(2, 0) * y);
744 
745  it->val.cov =
746  (Eigen::Matrix<double, 3, 3>::Identity() -
747  K * H) *
748  it->val.cov;
749  // it->val.cov.force_symmetry();
750 
751  // Update the weight of this mode:
752  // ----------------------------------
753  it->log_w += -0.5 * square(y) / varZ;
754 
755  max_w = max(
756  max_w,
757  it->log_w); // keep the maximum mode weight
758  } // end for each mode
759 
760  // Remove modes with negligible weights:
761  // -----------------------------------------------------------
762  for (it = beac->m_locationSOG.begin();
763  it != beac->m_locationSOG.end();)
764  {
765  if (max_w - it->log_w >
766  insertionOptions.SOG_thresholdNegligible)
767  {
768  // Remove the mode:
769  it = beac->m_locationSOG.erase(it);
770  }
771  else
772  ++it;
773  }
774 
775  // printf("ESS: %f\n",beac->m_locationSOG.ESS());
776 
777  // Normalize the weights:
778  beac->m_locationSOG.normalizeWeights();
779 
780  // Should we pass this beacon to a single Gaussian
781  // mode?
782  // -----------------------------------------------------------
783  CPoint3D curMean;
784  CMatrixDouble33 curCov;
785  beac->m_locationSOG.getCovarianceAndMean(
786  curCov, curMean);
787 
788  double D1 = sqrt(curCov(0, 0));
789  double D2 = sqrt(curCov(1, 1));
790  double D3 = sqrt(curCov(2, 2));
791  float maxDiag = max3(D1, D2, D3);
792 
793  if (maxDiag < 0.10f)
794  {
795  // Yes, transform:
796  beac->m_locationGauss.mean = curMean;
797  beac->m_locationGauss.cov = curCov;
798  beac->m_typePDF = CBeacon::pdfGauss;
799  // Done!
800  }
801  }
802  break;
803  default:
804  THROW_EXCEPTION("Invalid beac->m_typePDF!!!");
805  };
806 
807  } // end fuse
808  } // end if range makes sense
809  } // end for each observation
810 
811  // DONE!!
812  // Observation was successfully inserted into the map
813  return true;
814  }
815  else
816  {
817  return false;
818  }
819 
820  MRPT_END
821 }
822 
823 /*---------------------------------------------------------------
824  determineMatching2D
825  ---------------------------------------------------------------*/
826 void CBeaconMap::determineMatching2D(
827  const mrpt::maps::CMetricMap* otherMap, const CPose2D& otherMapPose,
828  TMatchingPairList& correspondences, const TMatchingParams& params,
829  TMatchingExtraResults& extraResults) const
830 {
831  MRPT_UNUSED_PARAM(params);
832  MRPT_START
833  extraResults = TMatchingExtraResults();
834 
835  CBeaconMap auxMap;
836  CPose3D otherMapPose3D(otherMapPose);
837 
838  // Check the other map class:
839  ASSERT_(otherMap->GetRuntimeClass() == CLASS_ID(CBeaconMap));
840  const CBeaconMap* otherMap2 = static_cast<const CBeaconMap*>(otherMap);
841  vector<bool> otherCorrespondences;
842 
843  // Coordinates change:
844  auxMap.changeCoordinatesReference(otherMapPose3D, otherMap2);
845 
846  // Use the 3D matching method:
847  computeMatchingWith3DLandmarks(
848  otherMap2, correspondences, extraResults.correspondencesRatio,
849  otherCorrespondences);
850 
851  MRPT_END
852 }
853 
854 /*---------------------------------------------------------------
855  changeCoordinatesReference
856  ---------------------------------------------------------------*/
857 void CBeaconMap::changeCoordinatesReference(const CPose3D& newOrg)
858 {
859  // Change the reference of each individual beacon:
860  for (iterator lm = m_beacons.begin(); lm != m_beacons.end(); ++lm)
861  lm->changeCoordinatesReference(newOrg);
862 }
863 
864 /*---------------------------------------------------------------
865  changeCoordinatesReference
866  ---------------------------------------------------------------*/
867 void CBeaconMap::changeCoordinatesReference(
868  const CPose3D& newOrg, const mrpt::maps::CBeaconMap* otherMap)
869 {
870  // In this object we cannot apply any special speed-up: Just copy and change
871  // coordinates:
872  (*this) = *otherMap;
873  changeCoordinatesReference(newOrg);
874 }
875 
876 /*---------------------------------------------------------------
877  computeMatchingWith3DLandmarks
878  ---------------------------------------------------------------*/
879 void CBeaconMap::computeMatchingWith3DLandmarks(
880  const mrpt::maps::CBeaconMap* anotherMap,
881  TMatchingPairList& correspondences, float& correspondencesRatio,
882  vector<bool>& otherCorrespondences) const
883 {
884  MRPT_START
885 
886  TSequenceBeacons::const_iterator thisIt, otherIt;
887  size_t nThis, nOther;
888  unsigned int j, k;
889  TMatchingPair match;
890  CPointPDFGaussian pointPDF_k, pointPDF_j;
891  vector<bool> thisLandmarkAssigned;
892 
893  // Get the number of landmarks:
894  nThis = m_beacons.size();
895  nOther = anotherMap->m_beacons.size();
896 
897  // Initially no LM has a correspondence:
898  thisLandmarkAssigned.resize(nThis, false);
899 
900  // Initially, set all landmarks without correspondences:
901  correspondences.clear();
902  otherCorrespondences.clear();
903  otherCorrespondences.resize(nOther, false);
904  correspondencesRatio = 0;
905 
906  for (k = 0, otherIt = anotherMap->m_beacons.begin();
907  otherIt != anotherMap->m_beacons.end(); ++otherIt, ++k)
908  {
909  for (j = 0, thisIt = m_beacons.begin(); thisIt != m_beacons.end();
910  ++thisIt, ++j)
911  {
912  // Is it a correspondence?
913  if ((otherIt)->m_ID == (thisIt)->m_ID)
914  {
915  // If a previous correspondence for this LM was found, discard
916  // this one!
917  if (!thisLandmarkAssigned[j])
918  {
919  thisLandmarkAssigned[j] = true;
920 
921  // OK: A correspondence found!!
922  otherCorrespondences[k] = true;
923 
924  match.this_idx = j;
925 
926  CPoint3D mean_j = m_beacons[j].getMeanVal();
927 
928  match.this_x = mean_j.x();
929  match.this_y = mean_j.y();
930  match.this_z = mean_j.z();
931 
932  CPoint3D mean_k = anotherMap->m_beacons[k].getMeanVal();
933  match.other_idx = k;
934  match.other_x = mean_k.x();
935  match.other_y = mean_k.y();
936  match.other_z = mean_k.z();
937 
938  correspondences.push_back(match);
939  }
940  }
941 
942  } // end of "otherIt" is SIFT
943 
944  } // end of other it., k
945 
946  // Compute the corrs ratio:
947  correspondencesRatio =
948  2.0f * correspondences.size() / static_cast<float>(nThis + nOther);
949 
950  MRPT_END
951 }
952 
953 /*---------------------------------------------------------------
954  saveToMATLABScript3D
955  ---------------------------------------------------------------*/
956 bool CBeaconMap::saveToMATLABScript3D(
957  const string& file, const char* style, float confInterval) const
958 {
959  MRPT_UNUSED_PARAM(style);
960  MRPT_UNUSED_PARAM(confInterval);
961 
962  FILE* f = os::fopen(file.c_str(), "wt");
963  if (!f) return false;
964 
965  // Header:
966  os::fprintf(
967  f, "%%-------------------------------------------------------\n");
968  os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
969  os::fprintf(f, "%% 'CBeaconMap::saveToMATLABScript3D'\n");
970  os::fprintf(f, "%%\n");
971  os::fprintf(f, "%% ~ MRPT ~\n");
972  os::fprintf(
973  f, "%% Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
974  os::fprintf(f, "%% http://www.isa.uma.es/ \n");
975  os::fprintf(
976  f, "%%-------------------------------------------------------\n\n");
977 
978  // Main code:
979  os::fprintf(f, "hold on;\n\n");
980  utils::CStringList strs;
981  string s;
982 
983  for (const_iterator it = m_beacons.begin(); it != m_beacons.end(); ++it)
984  {
985  it->getAsMatlabDrawCommands(strs);
986  strs.getText(s);
987  os::fprintf(f, "%s", s.c_str());
988  }
989 
990  os::fprintf(f, "axis equal;grid on;");
991 
992  os::fclose(f);
993  return true;
994 }
995 
996 /*---------------------------------------------------------------
997  TLikelihoodOptions
998  ---------------------------------------------------------------*/
999 CBeaconMap::TLikelihoodOptions::TLikelihoodOptions() : rangeStd(0.08f) {}
1000 /*---------------------------------------------------------------
1001  dumpToTextStream
1002  ---------------------------------------------------------------*/
1003 void CBeaconMap::TLikelihoodOptions::dumpToTextStream(
1004  mrpt::utils::CStream& out) const
1005 {
1006  out.printf(
1007  "\n----------- [CBeaconMap::TLikelihoodOptions] ------------ \n\n");
1008 
1009  out.printf("rangeStd = %f\n", rangeStd);
1010 
1011  out.printf("\n");
1012 }
1013 
1014 /*---------------------------------------------------------------
1015  loadFromConfigFile
1016  ---------------------------------------------------------------*/
1017 void CBeaconMap::TLikelihoodOptions::loadFromConfigFile(
1018  const mrpt::utils::CConfigFileBase& iniFile, const string& section)
1019 {
1020  rangeStd = iniFile.read_float(section.c_str(), "rangeStd", rangeStd);
1021 }
1022 
1023 /*---------------------------------------------------------------
1024  TInsertionOptions
1025  ---------------------------------------------------------------*/
1026 CBeaconMap::TInsertionOptions::TInsertionOptions()
1027  : insertAsMonteCarlo(true),
1028  maxElevation_deg(0),
1029  minElevation_deg(0),
1030  MC_numSamplesPerMeter(1000),
1031  MC_maxStdToGauss(0.4f),
1032  MC_thresholdNegligible(5),
1033  MC_performResampling(false),
1034  MC_afterResamplingNoise(0.01f),
1035  SOG_thresholdNegligible(20.0f),
1036  SOG_maxDistBetweenGaussians(1.0f),
1037  SOG_separationConstant(3.0f)
1038 {
1039 }
1040 
1041 /*---------------------------------------------------------------
1042  dumpToTextStream
1043  ---------------------------------------------------------------*/
1044 void CBeaconMap::TInsertionOptions::dumpToTextStream(
1045  mrpt::utils::CStream& out) const
1046 {
1047  out.printf(
1048  "\n----------- [CBeaconMap::TInsertionOptions] ------------ \n\n");
1049 
1050  out.printf(
1051  "insertAsMonteCarlo = %c\n",
1052  insertAsMonteCarlo ? 'Y' : 'N');
1053  out.printf(
1054  "minElevation_deg = %.03f\n", minElevation_deg);
1055  out.printf(
1056  "maxElevation_deg = %.03f\n", maxElevation_deg);
1057  out.printf(
1058  "MC_numSamplesPerMeter = %d\n",
1059  MC_numSamplesPerMeter);
1060  out.printf(
1061  "MC_maxStdToGauss = %.03f\n", MC_maxStdToGauss);
1062  out.printf(
1063  "MC_thresholdNegligible = %.03f\n",
1064  MC_thresholdNegligible);
1065  out.printf(
1066  "MC_performResampling = %c\n",
1067  MC_performResampling ? 'Y' : 'N');
1068  out.printf(
1069  "MC_afterResamplingNoise = %.03f\n",
1070  MC_afterResamplingNoise);
1071  out.printf(
1072  "SOG_thresholdNegligible = %.03f\n",
1073  SOG_thresholdNegligible);
1074  out.printf(
1075  "SOG_maxDistBetweenGaussians = %.03f\n",
1076  SOG_maxDistBetweenGaussians);
1077  out.printf(
1078  "SOG_separationConstant = %.03f\n",
1079  SOG_separationConstant);
1080 
1081  out.printf("\n");
1082 }
1083 
1084 /*---------------------------------------------------------------
1085  loadFromConfigFile
1086  ---------------------------------------------------------------*/
1087 void CBeaconMap::TInsertionOptions::loadFromConfigFile(
1088  const mrpt::utils::CConfigFileBase& iniFile, const string& section)
1089 {
1090  MRPT_LOAD_CONFIG_VAR(insertAsMonteCarlo, bool, iniFile, section.c_str());
1091  MRPT_LOAD_CONFIG_VAR(maxElevation_deg, float, iniFile, section.c_str());
1092  MRPT_LOAD_CONFIG_VAR(minElevation_deg, float, iniFile, section.c_str());
1093  MRPT_LOAD_CONFIG_VAR(MC_numSamplesPerMeter, int, iniFile, section.c_str());
1094  MRPT_LOAD_CONFIG_VAR(MC_maxStdToGauss, float, iniFile, section.c_str());
1095  MRPT_LOAD_CONFIG_VAR(
1096  MC_thresholdNegligible, float, iniFile, section.c_str());
1097  MRPT_LOAD_CONFIG_VAR(MC_performResampling, bool, iniFile, section.c_str());
1098  MRPT_LOAD_CONFIG_VAR(
1099  MC_afterResamplingNoise, float, iniFile, section.c_str());
1100  MRPT_LOAD_CONFIG_VAR(
1101  SOG_thresholdNegligible, float, iniFile, section.c_str());
1102  MRPT_LOAD_CONFIG_VAR(
1103  SOG_maxDistBetweenGaussians, float, iniFile, section.c_str());
1104  MRPT_LOAD_CONFIG_VAR(
1105  SOG_separationConstant, float, iniFile, section.c_str());
1106 }
1107 
1108 /*---------------------------------------------------------------
1109  isEmpty
1110  ---------------------------------------------------------------*/
1111 bool CBeaconMap::isEmpty() const { return size() == 0; }
1112 /*---------------------------------------------------------------
1113  simulateBeaconReadings
1114  ---------------------------------------------------------------*/
1115 void CBeaconMap::simulateBeaconReadings(
1116  const CPose3D& in_robotPose, const CPoint3D& in_sensorLocationOnRobot,
1117  CObservationBeaconRanges& out_Observations) const
1118 {
1119  TSequenceBeacons::const_iterator it;
1120  mrpt::obs::CObservationBeaconRanges::TMeasurement newMeas;
1121  CPoint3D point3D, beacon3D;
1122  CPointPDFGaussian beaconPDF;
1123 
1124  // Compute the 3D position of the sensor:
1125  point3D = in_robotPose + in_sensorLocationOnRobot;
1126 
1127  // Clear output data:
1128  out_Observations.sensedData.clear();
1129 
1130  // For each BEACON landmark in the map:
1131  for (it = m_beacons.begin(); it != m_beacons.end(); ++it)
1132  {
1133  it->getMean(beacon3D);
1134 
1135  float range = point3D.distanceTo(beacon3D);
1136 
1137  if (range < out_Observations.maxSensorDistance &&
1138  range > out_Observations.minSensorDistance)
1139  {
1140  // Add noise:
1141  range +=
1142  getRandomGenerator().drawGaussian1D(0, out_Observations.stdError);
1143 
1144  // Fill out:
1145  newMeas.beaconID = it->m_ID;
1146  newMeas.sensorLocationOnRobot = in_sensorLocationOnRobot;
1147  newMeas.sensedDistance = range;
1148 
1149  // Insert:
1150  out_Observations.sensedData.push_back(newMeas);
1151  }
1152  } // end for it
1153  // Done!
1154 }
1155 
1156 /*---------------------------------------------------------------
1157  saveMetricMapRepresentationToFile
1158  ---------------------------------------------------------------*/
1159 void CBeaconMap::saveMetricMapRepresentationToFile(
1160  const string& filNamePrefix) const
1161 {
1162  MRPT_START
1163 
1164  // Matlab:
1165  string fil1(filNamePrefix + string("_3D.m"));
1166  saveToMATLABScript3D(fil1);
1167 
1168  // 3D Scene:
1169  opengl::COpenGLScene scene;
1170  opengl::CSetOfObjects::Ptr obj3D =
1171  mrpt::make_aligned_shared<opengl::CSetOfObjects>();
1172 
1173  getAs3DObject(obj3D);
1174  opengl::CGridPlaneXY::Ptr objGround =
1175  mrpt::make_aligned_shared<opengl::CGridPlaneXY>(
1176  -100, 100, -100, 100, 0, 1);
1177 
1178  scene.insert(obj3D);
1179  scene.insert(objGround);
1180 
1181  string fil2(filNamePrefix + string("_3D.3Dscene"));
1182  CFileOutputStream f(fil2.c_str());
1183  f << scene;
1184 
1185  // Textual representation:
1186  string fil3(filNamePrefix + string("_covs.txt"));
1187  saveToTextFile(fil3);
1188 
1189  // Total number of particles / modes:
1190  string fil4(filNamePrefix + string("_population.txt"));
1191  {
1192  FILE* f = os::fopen(fil4.c_str(), "wt");
1193  if (f)
1194  {
1195  size_t nParts = 0, nGaussians = 0;
1196 
1197  for (TSequenceBeacons::const_iterator it = m_beacons.begin();
1198  it != m_beacons.end(); ++it)
1199  {
1200  switch (it->m_typePDF)
1201  {
1202  case CBeacon::pdfMonteCarlo:
1203  nParts += it->m_locationMC.size();
1204  break;
1205  case CBeacon::pdfSOG:
1206  nGaussians += it->m_locationSOG.size();
1207  break;
1208  case CBeacon::pdfGauss:
1209  nGaussians++;
1210  break;
1211  };
1212  }
1213 
1214  fprintf(
1215  f, "%u %u", static_cast<unsigned>(nParts),
1216  static_cast<unsigned>(nGaussians));
1217  os::fclose(f);
1218  }
1219  }
1220 
1221  MRPT_END
1222 }
1223 
1224 /*---------------------------------------------------------------
1225  getAs3DObject
1226  ---------------------------------------------------------------*/
1227 void CBeaconMap::getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const
1228 {
1229  MRPT_START
1230 
1231  if (!genericMapParams.enableSaveAs3DObject) return;
1232 
1233  // ------------------------------------------------
1234  // Add the XYZ corner for the current area:
1235  // ------------------------------------------------
1236  outObj->insert(opengl::stock_objects::CornerXYZ());
1237 
1238  // Save 3D ellipsoids or whatever representation:
1239  for (const_iterator it = m_beacons.begin(); it != m_beacons.end(); ++it)
1240  it->getAs3DObject(outObj);
1241 
1242  MRPT_END
1243 }
1244 
1245 /*---------------------------------------------------------------
1246  Computes the ratio in [0,1] of correspondences between "this" and the
1247  "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
1248  * In the case of a multi-metric map, this returns the average between the
1249  maps. This method always return 0 for grid maps.
1250  * \param otherMap [IN] The other map to compute the matching
1251  with.
1252  * \param otherMapPose [IN] The 6D pose of the other map as seen
1253  from "this".
1254  * \param maxDistForCorr [IN] The minimum distance between 2
1255  non-probabilistic map elements for counting them as a correspondence.
1256  * \param maxMahaDistForCorr [IN] The minimum Mahalanobis distance
1257  between 2 probabilistic map elements for counting them as a correspondence.
1258  *
1259  * \return The matching ratio [0,1]
1260  * \sa computeMatchingWith2D
1261  ----------------------------------------------------------------*/
1262 float CBeaconMap::compute3DMatchingRatio(
1263  const mrpt::maps::CMetricMap* otherMap2,
1264  const mrpt::poses::CPose3D& otherMapPose,
1265  const TMatchingRatioParams& params) const
1266 {
1267  MRPT_START
1268 
1269  // Compare to a similar map only:
1270  const CBeaconMap* otherMap = nullptr;
1271 
1272  if (otherMap2->GetRuntimeClass() == CLASS_ID(CBeaconMap))
1273  otherMap = static_cast<const CBeaconMap*>(otherMap2);
1274 
1275  if (!otherMap) return 0;
1276 
1277  TMatchingPairList matchList;
1278  vector<bool> otherCorrespondences;
1279  float out_corrsRatio;
1280 
1281  CBeaconMap modMap;
1282 
1283  modMap.changeCoordinatesReference(otherMapPose, otherMap);
1284 
1285  computeMatchingWith3DLandmarks(
1286  &modMap, matchList, out_corrsRatio, otherCorrespondences);
1287 
1288  return out_corrsRatio;
1289 
1290  MRPT_END
1291 }
1292 
1293 /*---------------------------------------------------------------
1294  getBeaconByID
1295  ---------------------------------------------------------------*/
1296 const CBeacon* CBeaconMap::getBeaconByID(CBeacon::TBeaconID id) const
1297 {
1298  for (const_iterator it = m_beacons.begin(); it != m_beacons.end(); ++it)
1299  if (it->m_ID == id) return &(*it);
1300  return nullptr;
1301 }
1302 
1303 /*---------------------------------------------------------------
1304  getBeaconByID
1305  ---------------------------------------------------------------*/
1306 CBeacon* CBeaconMap::getBeaconByID(CBeacon::TBeaconID id)
1307 {
1308  for (iterator it = m_beacons.begin(); it != m_beacons.end(); ++it)
1309  if (it->m_ID == id) return &(*it);
1310  return nullptr;
1311 }
1312 
1313 /*---------------------------------------------------------------
1314  saveToTextFile
1315 - VX VY VZ: Variances of each dimension (C11, C22, C33)
1316 - DET2D DET3D: Determinant of the 2D and 3D covariance matrixes.
1317 - C12, C13, C23: Cross covariances
1318  ---------------------------------------------------------------*/
1319 void CBeaconMap::saveToTextFile(const string& fil) const
1320 {
1321  MRPT_START
1322  FILE* f = os::fopen(fil.c_str(), "wt");
1323  ASSERT_(f != nullptr);
1324 
1325  CPoint3D p;
1326  CMatrixDouble33 C;
1327 
1328  for (const_iterator it = m_beacons.begin(); it != m_beacons.end(); ++it)
1329  {
1330  it->getCovarianceAndMean(C, p);
1331 
1332  float D3 = C.det();
1333  float D2 = C(0, 0) * C(1, 1) - square(C(0, 1));
1334  os::fprintf(
1335  f, "%i %f %f %f %e %e %e %e %e %e %e %e\n",
1336  static_cast<int>(it->m_ID), p.x(), p.y(), p.z(), C(0, 0), C(1, 1),
1337  C(2, 2), D2, D3, C(0, 1), C(1, 2), C(1, 2));
1338  }
1339 
1340  os::fclose(f);
1341  MRPT_END
1342 }
mrpt::random
A namespace of pseudo-random numbers genrators of diferent distributions.
Definition: RandomGenerators.h:25
mrpt::random::CRandomGenerator::drawUniform
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
Definition: RandomGenerators.h:103
mrpt::maps::CBeaconMap::isEmpty
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
Definition: CBeaconMap.cpp:1111
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:135
range
GLsizei range
Definition: glext.h:5907
mrpt::maps::TMetricMapInitializer
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Definition: TMetricMapInitializer.h:34
mrpt::maps::TMatchingRatioParams
Parameters for CMetricMap::compute3DMatchingRatio()
Definition: metric_map_types.h:75
mrpt::maps::CBeaconMap::TMapDefinition::insertionOpts
mrpt::maps::CBeaconMap::TInsertionOptions insertionOpts
Observations insertion options.
Definition: CBeaconMap.h:319
mrpt::maps::CBeacon::m_locationMC
mrpt::poses::CPointPDFParticles m_locationMC
The individual PDF, if m_typePDF=pdfMonteCarlo (publicly accesible for ease of use, but the CPointPDF interface is also implemented in CBeacon).
Definition: CBeacon.h:69
CFileOutputStream.h
mrpt::maps::CBeacon::TBeaconID
int64_t TBeaconID
The type for the IDs of landmarks.
Definition: CBeacon.h:49
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: CParticleFilter.h:223
mrpt::poses::CPoseOrPoint::distanceTo
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:206
mrpt::system
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:30
mrpt::poses::CPoseOrPoint::y_incr
void y_incr(const double v)
Definition: CPoseOrPoint.h:166
mrpt::utils::CConfigFileBase::read_float
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:106
mrpt::maps::CBeaconMap::compute3DMatchingRatio
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const override
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" In the case of a multi-metric map, this returns the average between the maps.
Definition: CBeaconMap.cpp:1262
mrpt::maps::TMapGenericParams::enableSaveAs3DObject
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects ...
Definition: metric_map_types.h:95
mrpt::maps::CBeaconMap::getAs3DObject
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CBeaconMap.cpp:1227
mrpt::system::os::fclose
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
mrpt::maps::CBeaconMap::writeToStream
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const override
Introduces a pure virtual method responsible for writing to a CStream.
Definition: CBeaconMap.cpp:107
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
Definition: CSerializable.h:169
mrpt::poses::CPointPDFGaussian::mean
CPoint3D mean
The mean value.
Definition: CPointPDFGaussian.h:44
MRPT_CHECK_NORMAL_NUMBER
#define MRPT_CHECK_NORMAL_NUMBER(val)
Definition: CColouredPointsMap.h:313
mrpt::bayes
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
Definition: CParticleFilter.h:28
mrpt::maps::CBeaconMap::computeMatchingWith3DLandmarks
void computeMatchingWith3DLandmarks(const mrpt::maps::CBeaconMap *otherMap, mrpt::utils::TMatchingPairList &correspondences, float &correspondencesRatio, std::vector< bool > &otherCorrespondences) const
Perform a search for correspondences between "this" and another lansmarks map: Firsly, the landmarks&#39; descriptor is used to find correspondences, then inconsistent ones removed by looking at their 3D poses.
Definition: CBeaconMap.cpp:879
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: CColouredPointsMap.h:112
CObservationBeaconRanges.h
n
GLenum GLsizei n
Definition: glext.h:5074
iterator
Scalar * iterator
Definition: eigen_plugins.h:26
mrpt::maps::CMetricMap::GetRuntimeClass
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
mrpt::bayes::CParticleFilterDataImpl::ESS
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
Definition: CParticleFilterData.h:87
data_utils.h
begin
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:29
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:42
mrpt::maps::CBeaconMap::TMapDefinition::loadFromConfigFile_map_specific
void loadFromConfigFile_map_specific(const mrpt::utils::CConfigFileBase &source, const std::string &sectionNamePrefix) override
Load all map-specific params.
Definition: CBeaconMap.cpp:45
mrpt::obs::CObservation::GetRuntimeClass
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
mrpt::obs::CObservationBeaconRanges
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
Definition: CObservationBeaconRanges.h:27
mrpt::obs::CObservationBeaconRanges::TMeasurement::beaconID
int32_t beaconID
The ID of the sensed beacon (or INVALID_BEACON_ID if unknown)
Definition: CObservationBeaconRanges.h:55
std
STL namespace.
mrpt::maps::CMetricMap::genericMapParams
TMapGenericParams genericMapParams
Common params to all maps.
Definition: CMetricMap.h:281
M_PI
#define M_PI
Definition: bits.h:92
mrpt::maps::CBeaconMap::iterator
std::deque< CBeacon >::iterator iterator
Definition: CBeaconMap.h:52
CGridPlaneXY.h
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:27
mrpt::maps::CBeaconMap::TInsertionOptions::TInsertionOptions
TInsertionOptions()
Initilization of default parameters.
Definition: CBeaconMap.cpp:1026
mrpt::maps::CBeaconMap::internal_clear
virtual void internal_clear() override
Internal method called by clear()
Definition: CBeaconMap.cpp:93
s
GLdouble s
Definition: glext.h:3676
obj
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
mrpt::utils::clear
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:189
mrpt::maps::CBeacon::m_locationSOG
mrpt::poses::CPointPDFSOG m_locationSOG
The individual PDF, if m_typePDF=pdfSOG (publicly accesible for ease of use, but the CPointPDF interf...
Definition: CBeacon.h:75
mrpt::random::CRandomGenerator::drawGaussian1D
double drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
Definition: RandomGenerators.h:155
mrpt::bayes::CParticleFilterData::getWeights
void getWeights(std::vector< double > &out_logWeights) const
Returns a vector with the sequence of the logaritmic weights of all the samples.
Definition: CParticleFilterData.h:256
mrpt::utils::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: CConfigFileBase.h:39
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:55
CStream.h
mrpt::maps::CBeaconMap::m_beacons
TSequenceBeacons m_beacons
The individual beacons.
Definition: CBeaconMap.h:57
mrpt::utils::CStream
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:41
mrpt::utils::CStringList
A class for storing a list of text lines.
Definition: CStringList.h:32
mrpt::bayes::CParticleFilterData::m_particles
CParticleList m_particles
The array of particles.
Definition: CParticleFilterData.h:208
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:39
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::maps::CBeaconMap::changeCoordinatesReference
void changeCoordinatesReference(const mrpt::poses::CPose3D &newOrg)
Changes the reference system of the map to a given 3D pose.
Definition: CBeaconMap.cpp:857
CSetOfObjects.h
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:85
mrpt::maps::TMatchingExtraResults
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
Definition: metric_map_types.h:62
MRPT_END
#define MRPT_END
Definition: CColouredPointsMap.h:430
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: CColouredPointsMap.h:366
mrpt::utils::CFileOutputStream
This CStream derived class allow using a file as a write-only, binary stream.
Definition: CFileOutputStream.h:26
mrpt::math::averageLogLikelihood
double averageLogLikelihood(const CVectorDouble &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
Definition: math.cpp:1777
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: CColouredPointsMap.h:182
mrpt::poses::CPointPDFGaussian::cov
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Definition: CPointPDFGaussian.h:46
mrpt::math::CMatrixDouble
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
Definition: CMatrixTemplateNumeric.h:147
mrpt::poses::CPointPDFSOG::const_iterator
std::deque< TGaussianMode >::const_iterator const_iterator
Definition: CPointPDFSOG.h:54
mrpt::utils::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:93
mrpt::obs::CObservationBeaconRanges::sensedData
std::deque< TMeasurement > sensedData
The list of observed ranges.
Definition: CObservationBeaconRanges.h:59
end
GLuint GLuint end
Definition: glext.h:3528
mrpt::poses::CPoseOrPoint::x_incr
void x_incr(const double v)
Definition: CPoseOrPoint.h:162
mrpt::poses::CPointPDFParticles::setSize
void setSize(size_t numberParticles, const CPoint3D &defaultValue=CPoint3D(0, 0, 0))
Erase all the previous particles and change the number of particles, with a given initial value...
Definition: CPointPDFParticles.cpp:38
mrpt::maps::CBeaconMap::TInsertionOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CBeaconMap.cpp:1087
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< CSetOfObjects > Ptr
Definition: CSetOfObjects.h:32
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:19
mrpt::maps::CBeaconMap
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
mrpt::poses::CPointPDFSOG::normalizeWeights
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
Definition: CPointPDFSOG.cpp:456
mrpt::maps::CBeaconMap::saveToTextFile
void saveToTextFile(const std::string &fil) const
Save a text file with a row per beacon, containing this 11 elements:
Definition: CBeaconMap.cpp:1319
mrpt::poses::CPointPDFSOG::getCovarianceAndMean
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPoint3D &mean_point) const override
Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once...
Definition: CPointPDFSOG.cpp:82
CConfigFileBase.h
DEG2RAD
#define DEG2RAD
Definition: CColouredPointsMap.h:112
mrpt::maps::CBeacon::m_typePDF
TTypePDF m_typePDF
Which one of the different 3D point PDF is currently used in this object: montecarlo, gaussian, or a sum of gaussians.
Definition: CBeacon.h:64
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:32
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::obs::CObservationBeaconRanges::stdError
float stdError
The "sigma" of the sensor, assuming a zero-mean Gaussian noise model.
Definition: CObservationBeaconRanges.h:38
mrpt::maps::CBeaconMap::TLikelihoodOptions::dumpToTextStream
void dumpToTextStream(mrpt::utils::CStream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
Definition: CBeaconMap.cpp:1003
mrpt::bayes::CParticleFilterDataImpl::performSubstitution
void performSubstitution(const std::vector< size_t > &indx) override
Replaces the old particles by copies determined by the indexes in "indx", performing an efficient cop...
Definition: CParticleFilterData.h:114
mrpt::poses::CPointPDFSOG::erase
iterator erase(iterator i)
Definition: CPointPDFSOG.h:107
mrpt::system::os::fprintf
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:405
mrpt::obs::CObservationBeaconRanges::TMeasurement::sensedDistance
float sensedDistance
The sensed range itself (in meters).
Definition: CObservationBeaconRanges.h:53
mrpt::maps::CBeaconMap::readFromStream
void readFromStream(mrpt::utils::CStream &in, int version) override
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
Definition: CBeaconMap.cpp:128
MRPT_START
#define MRPT_START
Definition: CColouredPointsMap.h:426
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:17
MAP_DEFINITION_REGISTER
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
Definition: TMetricMapTypesRegistry.h:95
mrpt::maps::CBeaconMap::TMapDefinition::dumpToTextStream_map_specific
void dumpToTextStream_map_specific(mrpt::utils::CStream &out) const override
Definition: CBeaconMap.cpp:60
CParticleFilterCapable.h
COpenGLScene.h
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:55
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
R
const float R
Definition: CKinematicChain.cpp:149
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::obs::CObservation
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:41
mrpt::maps::CBeaconMap::internal_CreateFromMapDefinition
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
Definition: CBeaconMap.cpp:69
mrpt::opengl::stock_objects::CornerXYZ
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
Definition: StockObjects.cpp:211
mrpt::poses::CPointPDFSOG::size
size_t size() const
Return the number of Gaussian modes.
Definition: CPointPDFSOG.h:111
mrpt::maps::CBeaconMap::getBeaconByID
const CBeacon * getBeaconByID(CBeacon::TBeaconID id) const
Returns a pointer to the beacon with the given ID, or nullptr if it does not exist.
Definition: CBeaconMap.cpp:1296
mrpt::maps::CBeaconMap::determineMatching2D
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::utils::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const override
Computes the matching between this and another 2D point map, which includes finding: ...
Definition: CBeaconMap.cpp:826
in
GLuint in
Definition: glext.h:7274
mrpt::opengl::COpenGLScene
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:60
source
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
ASSERT_
#define ASSERT_(f)
Definition: CColouredPointsMap.h:310
mrpt::maps::CBeaconMap::internal_insertObservation
virtual bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
Definition: CBeaconMap.cpp:366
mrpt::obs::CObservationBeaconRanges::TMeasurement::sensorLocationOnRobot
mrpt::poses::CPoint3D sensorLocationOnRobot
Position of the sensor on the robot.
Definition: CObservationBeaconRanges.h:51
CParticleFilter.h
mrpt::utils::max3
const T max3(const T &A, const T &B, const T &C)
Definition: bits.h:161
mrpt::utils::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:25
mrpt::maps::CBeaconMap::resize
void resize(const size_t N)
Resize the number of SOG modes.
Definition: CBeaconMap.cpp:101
CBeaconMap.h
mrpt::maps::CBeaconMap::TMapDefinition::likelihoodOpts
mrpt::maps::CBeaconMap::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
Definition: CBeaconMap.h:321
mrpt::maps::CBeaconMap::TInsertionOptions::dumpToTextStream
void dumpToTextStream(mrpt::utils::CStream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
Definition: CBeaconMap.cpp:1044
y
GLenum GLint GLint y
Definition: glext.h:3538
mrpt::poses::CPointPDFParticles::clear
void clear()
Clear all the particles (free memory)
Definition: CPointPDFParticles.cpp:34
mrpt::maps::CBeaconMap::internal_computeObservationLikelihood
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
Definition: CBeaconMap.cpp:157
mrpt::system::os::fopen
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:254
mrpt::maps::CBeaconMap::const_iterator
std::deque< CBeacon >::const_iterator const_iterator
Definition: CBeaconMap.h:53
mrpt::maps::CBeacon::m_ID
TBeaconID m_ID
An ID for the landmark (see details next...) This ID was introduced in the version 3 of this class (2...
Definition: CBeacon.h:98
mrpt::maps::CBeaconMap::saveToMATLABScript3D
bool saveToMATLABScript3D(const std::string &file, const char *style="b", float confInterval=0.95f) const
Save to a MATLAB script which displays 3D error ellipses for the map.
Definition: CBeaconMap.cpp:956
mrpt::maps::CBeaconMap::TLikelihoodOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CBeaconMap.cpp:1017
mrpt::maps::CBeacon::generateRingSOG
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:496
mrpt::poses::CPointPDFSOG::iterator
std::deque< TGaussianMode >::iterator iterator
Definition: CPointPDFSOG.h:55
mrpt::poses::CPointPDFParticles::getCovarianceAndMean
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPoint3D &mean_point) const override
Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once...
Definition: CPointPDFParticles.cpp:87
MRPT_LOAD_CONFIG_VAR
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
Definition: CConfigFileBase.h:279
mrpt::maps::CBeaconMap::saveMetricMapRepresentationToFile
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
Definition: CBeaconMap.cpp:1159
mrpt::maps::TMatchingParams
Parameters for the determination of matchings between point clouds, etc.
Definition: metric_map_types.h:22
uint32_t
unsigned __int32 uint32_t
Definition: rptypes.h:47
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:25
mrpt::opengl::COpenGLScene::insert
void insert(const CRenderizable::Ptr &newObject, const std::string &viewportName=std::string("main"))
Insert a new object into the scene, in the given viewport (by default, into the "main" viewport)...
Definition: COpenGLScene.cpp:189
mrpt::maps::CBeaconMap::simulateBeaconReadings
void simulateBeaconReadings(const mrpt::poses::CPose3D &in_robotPose, const mrpt::poses::CPoint3D &in_sensorLocationOnRobot, mrpt::obs::CObservationBeaconRanges &out_Observations) const
Simulates a reading toward each of the beacons in the landmarks map, if any.
Definition: CBeaconMap.cpp:1115
p
GLfloat GLfloat p
Definition: glext.h:6305
params
GLenum const GLfloat * params
Definition: glext.h:3534
mrpt::utils::CStringList::getText
void getText(std::string &outText) const
Returns the whole string list as a single string with &#39; &#39; characters for newlines.
Definition: CStringList.cpp:116
mrpt::maps::CBeacon
The class for storing individual "beacon landmarks" under a variety of 3D position PDF distributions...
Definition: CBeacon.h:42
mrpt::utils::TMatchingPair
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
mrpt::opengl::CGridPlaneXY::Ptr
std::shared_ptr< CGridPlaneXY > Ptr
Definition: CGridPlaneXY.h:34
mrpt::poses::CPointPDFGaussian
A gaussian distribution for 3D points.
Definition: CPointPDFGaussian.h:25
mrpt::utils::CStream::printf
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:597
stock_objects.h
mrpt::maps::CBeaconMap::size
size_t size() const
Returns the stored landmarks count.
Definition: CBeaconMap.cpp:97
mrpt::poses::CPoseOrPoint::distance3DTo
double distance3DTo(double ax, double ay, double az) const
Returns the 3D distance from this pose/point to a 3D point.
Definition: CPoseOrPoint.h:239
mrpt::maps::CBeaconMap::TLikelihoodOptions::TLikelihoodOptions
TLikelihoodOptions()
Initilization of default parameters.
Definition: CBeaconMap.cpp:999
mrpt::maps::CBeacon::m_locationGauss
mrpt::poses::CPointPDFGaussian m_locationGauss
The individual PDF, if m_typePDF=pdfGauss (publicly accesible for ease of use, but the CPointPDF inte...
Definition: CBeacon.h:72


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019