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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020