MRPT  2.0.4
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 }
os.h
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:1087
mrpt::maps::CBeacon::pdfSOG
@ pdfSOG
Definition: CBeacon.h:50
mrpt::obs::CObservationBeaconRanges::TMeasurement
Each one of the measurements.
Definition: CObservationBeaconRanges.h:38
mrpt::maps::CBeacon::m_typePDF
TTypePDF m_typePDF
Which one of the different 3D point PDF is currently used in this object: montecarlo,...
Definition: CBeacon.h:57
mrpt::obs::CObservationBeaconRanges::TMeasurement::sensedDistance
float sensedDistance
The sensed range itself (in meters).
Definition: CObservationBeaconRanges.h:50
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,...
Definition: CBeaconMap.cpp:1186
mrpt::maps::CBeaconMap::TMapDefinition::TMapDefinition
TMapDefinition()
mrpt::containers::clear
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
Eigen::Matrix
Definition: math_frwds.h:32
mrpt::system::os::fclose
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:286
CBeaconMap.h
mrpt::poses::CPointPDFGaussian::cov
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Definition: CPointPDFGaussian.h:44
mrpt::poses::CPointPDFParticles::setSize
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
Definition: CPointPDFParticles.cpp:37
mrpt::maps::CBeaconMap::serializeFrom
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
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
Definition: CSetOfObjects.h:28
geometry.h
mrpt::obs::CObservationBeaconRanges::minSensorDistance
float minSensorDistance
Info about sensor.
Definition: CObservationBeaconRanges.h:33
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:177
mrpt::math::CVectorDynamic< double >::iterator
typename vec_t::iterator iterator
Definition: CVectorDynamic.h:59
string_utils.h
mrpt::max3
const T max3(const T &A, const T &B, const T &C)
Definition: core/include/mrpt/core/bits_math.h:129
mrpt::math::CVectorDynamic::begin
iterator begin()
Definition: CVectorDynamic.h:61
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: config/CConfigFileBase.h:306
mrpt::poses::CPointPDFSOG::getCovarianceAndMean
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Definition: CPointPDFSOG.cpp:80
mrpt::poses::CPointPDFSOG::normalizeWeights
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
Definition: CPointPDFSOG.cpp:405
mrpt::poses::CPoseOrPoint::x_incr
void x_incr(const double v)
Definition: CPoseOrPoint.h:170
mrpt::math::CMatrixDynamic::setSize
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
Definition: CMatrixDynamic.h:352
mrpt::tfest::TMatchingPair::other_z
float other_z
Definition: TMatchingPair.h:53
CSetOfObjects.h
mrpt::poses::CPointPDFGaussian
A gaussian distribution for 3D points.
Definition: CPointPDFGaussian.h:22
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
mrpt::opengl::COpenGLScene
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
Definition: COpenGLScene.h:56
CFileOutputStream.h
mrpt::maps::CBeaconMap::m_beacons
TSequenceBeacons m_beacons
The individual beacons.
Definition: CBeaconMap.h:54
mrpt::maps::CBeaconMap::TLikelihoodOptions::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CBeaconMap.cpp:957
mrpt::tfest::TMatchingPair::other_idx
uint32_t other_idx
Definition: TMatchingPair.h:51
MAP_DEFINITION_REGISTER
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
Definition: TMetricMapTypesRegistry.h:91
mrpt::maps::CBeaconMap::internal_insertObservation
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
Definition: CBeaconMap.cpp:355
mrpt::obs::CObservationBeaconRanges::maxSensorDistance
float maxSensorDistance
Definition: CObservationBeaconRanges.h:33
data_utils.h
mrpt::poses::CPoseOrPoint::y_incr
void y_incr(const double v)
Definition: CPoseOrPoint.h:174
mrpt::maps::TMatchingRatioParams
Parameters for CMetricMap::compute3DMatchingRatio()
Definition: metric_map_types.h:64
mrpt::maps::CBeaconMap::getAs3DObject
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CBeaconMap.cpp:1152
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::poses::CPointPDFParticles::getCovarianceAndMean
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Definition: CPointPDFParticles.cpp:83
mrpt::tfest
Functions for estimating the optimal transformation between two frames of references given measuremen...
Definition: indiv-compat-decls.h:14
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
CParticleFilter.h
mrpt::maps::TMetricMapInitializer
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Definition: TMetricMapInitializer.h:32
mrpt::maps::CBeaconMap::internal_clear
void internal_clear() override
Internal method called by clear()
Definition: CBeaconMap.cpp:95
mrpt::tfest::TMatchingPair
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
mrpt::maps::CBeaconMap::TInsertionOptions::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CBeaconMap.cpp:974
R
const float R
Definition: CKinematicChain.cpp:137
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mrpt::maps::CBeaconMap::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CBeaconMap.cpp:105
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
stock_objects.h
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::poses::CPoseOrPoint::distanceTo
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:214
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:821
mrpt::poses::CPointPDFSOG::end
iterator end()
Definition: CPointPDFSOG.h:101
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
MRPT_CHECK_NORMAL_NUMBER
#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
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
random.h
mrpt::obs::CObservationBeaconRanges::sensedData
std::deque< TMeasurement > sensedData
The list of observed ranges.
Definition: CObservationBeaconRanges.h:56
mrpt::math::CMatrixDouble
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
Definition: CMatrixDynamic.h:589
CConfigFileBase.h
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
mrpt::obs::CObservation::GetRuntimeClass
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
mrpt::random::CRandomGenerator::drawUniform
return_t drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm,...
Definition: RandomGenerators.h:142
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
mrpt::obs::CObservationBeaconRanges
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
Definition: CObservationBeaconRanges.h:24
mrpt::tfest::TMatchingPair::this_x
float this_x
Definition: TMatchingPair.h:52
mrpt::math::MatrixBase::det
Scalar det() const
Determinant of matrix.
Definition: MatrixBase_impl.h:76
mrpt::tfest::TMatchingPair::this_idx
uint32_t this_idx
Definition: TMatchingPair.h:50
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24
mrpt::opengl::CGridPlaneXY::Create
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:31
mrpt::maps::CBeaconMap::TInsertionOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
Initilization of default parameters.
Definition: CBeaconMap.cpp:1015
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
COpenGLScene.h
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::obs::CObservationBeaconRanges::stdError
float stdError
The "sigma" of the sensor, assuming a zero-mean Gaussian noise model.
Definition: CObservationBeaconRanges.h:35
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:65
mrpt::maps::CBeaconMap::TMapDefinition
Definition: CBeaconMap.h:308
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::tfest::TMatchingPair::other_y
float other_y
Definition: TMatchingPair.h:53
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::obs::CObservationBeaconRanges::TMeasurement::beaconID
int32_t beaconID
The ID of the sensed beacon (or INVALID_BEACON_ID if unknown)
Definition: CObservationBeaconRanges.h:52
iniFile
string iniFile(myDataDir+string("benchmark-options.ini"))
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
mrpt::maps::CBeacon::generateRingSOG
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
round.h
mrpt::poses::CPointPDFParticles::clear
void clear()
Clear all the particles (free memory)
Definition: CPointPDFParticles.cpp:33
mrpt::math::size
size_t size(const MATRIXLIKE &m, const int dim)
Definition: math/include/mrpt/math/bits_math.h:21
mrpt::maps::CBeaconMap::TMapDefinition::insertionOpts
mrpt::maps::CBeaconMap::TInsertionOptions insertionOpts
Observations insertion options.
Definition: CBeaconMap.h:310
mrpt::d2f
float d2f(const double d)
shortcut for static_cast<float>(double)
Definition: core/include/mrpt/core/bits_math.h:189
mrpt::maps::CBeaconMap::TMapDefinition::likelihoodOpts
mrpt::maps::CBeaconMap::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
Definition: CBeaconMap.h:312
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:68
mrpt::square
return_t square(const num_t x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:23
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:146
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:257
CParticleFilterCapable.h
mrpt::maps::CBeaconMap::internal_CreateFromMapDefinition
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
Definition: CBeaconMap.cpp:71
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
params
mrpt::vision::TStereoCalibParams params
Definition: chessboard_stereo_camera_calib_unittest.cpp:24
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Definition: CSerializable.h:166
mrpt::math::MatrixVectorBase::transpose
auto transpose()
Definition: MatrixVectorBase.h:159
mrpt::maps::TMatchingParams
Parameters for the determination of matchings between point clouds, etc.
Definition: metric_map_types.h:20
mrpt::tfest::TMatchingPair::this_y
float this_y
Definition: TMatchingPair.h:52
mrpt::math::CVectorDynamic< double >
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:1243
mrpt::poses::CPointPDFSOG::begin
iterator begin()
Definition: CPointPDFSOG.h:100
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:1220
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:91
mrpt::math::CVectorDynamic::size
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
Definition: CVectorDynamic.h:141
mrpt::poses::CPointPDFSOG::const_iterator
std::deque< TGaussianMode >::const_iterator const_iterator
Definition: CPointPDFSOG.h:51
mrpt::opengl::COpenGLScene::saveToFile
bool saveToFile(const std::string &fil) const
Saves the scene to a 3Dscene file, loadable by the application SceneViewer3D.
Definition: COpenGLScene.cpp:293
mrpt::bayes::CParticleFilterData::m_particles
CParticleList m_particles
The array of particles.
Definition: CParticleFilterData.h:196
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:419
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
CObservationBeaconRanges.h
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:136
mrpt::maps::CBeacon::m_locationMC
mrpt::poses::CPointPDFParticles m_locationMC
The individual PDF, if m_typePDF=pdfMonteCarlo (publicly accesible for ease of use,...
Definition: CBeacon.h:62
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:110
mrpt::math::CMatrixFixed::asEigen
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object
Definition: CMatrixFixed.h:251
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
mrpt::maps::CBeaconMap::TLikelihoodOptions::loadFromConfigFile
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
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:43
mrpt::bayes
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
Definition: CKalmanFilterCapable.h:30
maps-precomp.h
mrpt::math::multiply_HCHt_scalar
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
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::maps::CBeaconMap
A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian,...
Definition: CBeaconMap.h:43
mrpt::maps::CMetricMap::GetRuntimeClass
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
mrpt::maps::CBeaconMap::TMapDefinition::dumpToTextStream_map_specific
void dumpToTextStream_map_specific(std::ostream &out) const override
Definition: CBeaconMap.cpp:62
mrpt::tfest::TMatchingPair::this_z
float this_z
Definition: TMatchingPair.h:52
mrpt::maps::CBeacon::pdfMonteCarlo
@ pdfMonteCarlo
Definition: CBeacon.h:48
mrpt::maps::CBeacon
The class for storing individual "beacon landmarks" under a variety of 3D position PDF distributions.
Definition: CBeacon.h:35
mrpt::maps::CBeaconMap::determineMatching2D
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
mrpt::random::CRandomGenerator::drawGaussian1D
return_t drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
Definition: RandomGenerators.h:194
mrpt::obs::CObservation
Declares a class that represents any robot's observation.
Definition: CObservation.h:43
mrpt::poses::CPointPDFSOG::erase
iterator erase(iterator i)
Definition: CPointPDFSOG.h:104
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::maps::CBeaconMap::resize
void resize(const size_t N)
Resize the number of SOG modes.
Definition: CBeaconMap.cpp:103
mrpt::tfest::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:70
mrpt::system::os::fopen
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:268
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:1043
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:18
mrpt::maps
Definition: CBeacon.h:21
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:919
mrpt::maps::TMatchingExtraResults
Additional results from the determination of matchings between point clouds, etc.,...
Definition: metric_map_types.h:51
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:243
CArchive.h
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:31
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::maps::CBeacon::pdfGauss
@ pdfGauss
Definition: CBeacon.h:49
mrpt::poses::CPointPDFGaussian::mean
CPoint3D mean
The mean value.
Definition: CPointPDFGaussian.h:42
mrpt::maps::CBeacon::TBeaconID
int64_t TBeaconID
The type for the IDs of landmarks.
Definition: CBeacon.h:42
CGridPlaneXY.h
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:293
mrpt::system::stringListAsString
void stringListAsString(const std::vector< std::string > &lst, std::string &out, const std::string &newline="\r\n")
Convert a string list to one single string with new-lines.
Definition: string_utils.cpp:351
mrpt::obs::CObservationBeaconRanges::TMeasurement::sensorLocationOnRobot
mrpt::poses::CPoint3D sensorLocationOnRobot
Position of the sensor on the robot.
Definition: CObservationBeaconRanges.h:48
mrpt::maps::CBeaconMap::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CBeaconMap.cpp:104
mrpt::maps::CBeaconMap::size
size_t size() const
Returns the stored landmarks count.
Definition: CBeaconMap.cpp:99
mrpt::maps::CBeaconMap::computeMatchingWith3DLandmarks
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,...
Definition: CBeaconMap.cpp:843
mrpt::maps::CBeaconMap::TMapDefinition::loadFromConfigFile_map_specific
void loadFromConfigFile_map_specific(const mrpt::config::CConfigFileBase &source, const std::string &sectionNamePrefix) override
Load all map-specific params.
Definition: CBeaconMap.cpp:47
mrpt::bayes::CParticleFilterDataImpl::ESS
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
Definition: CParticleFilterData.h:85
mrpt::math::CMatrixDynamic< double >
mrpt::tfest::TMatchingPair::other_x
float other_x
Definition: TMatchingPair.h:53
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::poses::CPointPDFSOG::size
size_t size() const
Return the number of Gaussian modes.
Definition: CPointPDFSOG.h:108
mrpt::system
Definition: backtrace.h:14
mrpt::maps::CBeaconMap::isEmpty
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
Definition: CBeaconMap.cpp:1039



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sat Jun 27 14:00:59 UTC 2020