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-2019, 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, const TMatchingParams& params,
793  TMatchingExtraResults& extraResults) const
794 {
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 =
912  2.0f * correspondences.size() / static_cast<float>(nThis + nOther);
913 
914  MRPT_END
915 }
916 
917 /*---------------------------------------------------------------
918  saveToMATLABScript3D
919  ---------------------------------------------------------------*/
921  const string& file, const char* style, float confInterval) const
922 {
923  MRPT_UNUSED_PARAM(style);
924  MRPT_UNUSED_PARAM(confInterval);
925 
926  FILE* f = os::fopen(file.c_str(), "wt");
927  if (!f) return false;
928 
929  // Header:
930  os::fprintf(
931  f, "%%-------------------------------------------------------\n");
932  os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
933  os::fprintf(f, "%% 'CBeaconMap::saveToMATLABScript3D'\n");
934  os::fprintf(f, "%%\n");
935  os::fprintf(f, "%% ~ MRPT ~\n");
936  os::fprintf(
937  f, "%% Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
938  os::fprintf(f, "%% http://www.isa.uma.es/ \n");
939  os::fprintf(
940  f, "%%-------------------------------------------------------\n\n");
941 
942  // Main code:
943  os::fprintf(f, "hold on;\n\n");
944  std::vector<std::string> strs;
945  string s;
946 
947  for (const auto& m_beacon : m_beacons)
948  {
949  m_beacon.getAsMatlabDrawCommands(strs);
951  os::fprintf(f, "%s", s.c_str());
952  }
953 
954  os::fprintf(f, "axis equal;grid on;");
955 
956  os::fclose(f);
957  return true;
958 }
959 
961 {
962  out << "\n----------- [CBeaconMap::TLikelihoodOptions] ------------ \n\n";
963  out << mrpt::format(
964  "rangeStd = %f\n", rangeStd);
965  out << "\n";
966 }
967 
968 /*---------------------------------------------------------------
969  loadFromConfigFile
970  ---------------------------------------------------------------*/
972  const mrpt::config::CConfigFileBase& iniFile, const string& section)
973 {
974  rangeStd = iniFile.read_float(section.c_str(), "rangeStd", rangeStd);
975 }
976 
978 {
979  out << "\n----------- [CBeaconMap::TInsertionOptions] ------------ \n\n";
980 
981  out << mrpt::format(
982  "insertAsMonteCarlo = %c\n",
983  insertAsMonteCarlo ? 'Y' : 'N');
984  out << mrpt::format(
985  "minElevation_deg = %.03f\n", minElevation_deg);
986  out << mrpt::format(
987  "maxElevation_deg = %.03f\n", maxElevation_deg);
988  out << mrpt::format(
989  "MC_numSamplesPerMeter = %d\n",
990  MC_numSamplesPerMeter);
991  out << mrpt::format(
992  "MC_maxStdToGauss = %.03f\n", MC_maxStdToGauss);
993  out << mrpt::format(
994  "MC_thresholdNegligible = %.03f\n",
995  MC_thresholdNegligible);
996  out << mrpt::format(
997  "MC_performResampling = %c\n",
998  MC_performResampling ? 'Y' : 'N');
999  out << mrpt::format(
1000  "MC_afterResamplingNoise = %.03f\n",
1001  MC_afterResamplingNoise);
1002  out << mrpt::format(
1003  "SOG_thresholdNegligible = %.03f\n",
1004  SOG_thresholdNegligible);
1005  out << mrpt::format(
1006  "SOG_maxDistBetweenGaussians = %.03f\n",
1007  SOG_maxDistBetweenGaussians);
1008  out << mrpt::format(
1009  "SOG_separationConstant = %.03f\n",
1010  SOG_separationConstant);
1011 
1012  out << "\n";
1013 }
1014 
1015 /*---------------------------------------------------------------
1016  loadFromConfigFile
1017  ---------------------------------------------------------------*/
1019  const mrpt::config::CConfigFileBase& iniFile, const string& section)
1020 {
1021  MRPT_LOAD_CONFIG_VAR(insertAsMonteCarlo, bool, iniFile, section.c_str());
1022  MRPT_LOAD_CONFIG_VAR(maxElevation_deg, float, iniFile, section.c_str());
1023  MRPT_LOAD_CONFIG_VAR(minElevation_deg, float, iniFile, section.c_str());
1024  MRPT_LOAD_CONFIG_VAR(MC_numSamplesPerMeter, int, iniFile, section.c_str());
1025  MRPT_LOAD_CONFIG_VAR(MC_maxStdToGauss, float, iniFile, section.c_str());
1027  MC_thresholdNegligible, float, iniFile, section.c_str());
1028  MRPT_LOAD_CONFIG_VAR(MC_performResampling, bool, iniFile, section.c_str());
1030  MC_afterResamplingNoise, float, iniFile, section.c_str());
1032  SOG_thresholdNegligible, float, iniFile, section.c_str());
1034  SOG_maxDistBetweenGaussians, float, iniFile, section.c_str());
1036  SOG_separationConstant, float, iniFile, section.c_str());
1037 }
1038 
1039 /*---------------------------------------------------------------
1040  isEmpty
1041  ---------------------------------------------------------------*/
1042 bool CBeaconMap::isEmpty() const { return size() == 0; }
1043 /*---------------------------------------------------------------
1044  simulateBeaconReadings
1045  ---------------------------------------------------------------*/
1047  const CPose3D& in_robotPose, const CPoint3D& in_sensorLocationOnRobot,
1048  CObservationBeaconRanges& out_Observations) const
1049 {
1050  TSequenceBeacons::const_iterator it;
1052  CPoint3D point3D, beacon3D;
1053  CPointPDFGaussian beaconPDF;
1054 
1055  // Compute the 3D position of the sensor:
1056  point3D = in_robotPose + in_sensorLocationOnRobot;
1057 
1058  // Clear output data:
1059  out_Observations.sensedData.clear();
1060 
1061  // For each BEACON landmark in the map:
1062  for (it = m_beacons.begin(); it != m_beacons.end(); ++it)
1063  {
1064  it->getMean(beacon3D);
1065 
1066  float range = point3D.distanceTo(beacon3D);
1067 
1068  if (range < out_Observations.maxSensorDistance &&
1069  range > out_Observations.minSensorDistance)
1070  {
1071  // Add noise:
1073  0, out_Observations.stdError);
1074 
1075  // Fill out:
1076  newMeas.beaconID = it->m_ID;
1077  newMeas.sensorLocationOnRobot = in_sensorLocationOnRobot;
1078  newMeas.sensedDistance = range;
1079 
1080  // Insert:
1081  out_Observations.sensedData.push_back(newMeas);
1082  }
1083  } // end for it
1084  // Done!
1085 }
1086 
1087 /*---------------------------------------------------------------
1088  saveMetricMapRepresentationToFile
1089  ---------------------------------------------------------------*/
1091  const string& filNamePrefix) const
1092 {
1093  MRPT_START
1094 
1095  // Matlab:
1096  string fil1(filNamePrefix + string("_3D.m"));
1097  saveToMATLABScript3D(fil1);
1098 
1099  // 3D Scene:
1100  opengl::COpenGLScene scene;
1102  std::make_shared<opengl::CSetOfObjects>();
1103 
1104  getAs3DObject(obj3D);
1105  auto objGround = opengl::CGridPlaneXY::Create(
1106  -100.0f, 100.0f, -100.0f, 100.0f, .0f, 1.f);
1107 
1108  scene.insert(obj3D);
1109  scene.insert(objGround);
1110 
1111  string fil2(filNamePrefix + string("_3D.3Dscene"));
1112  scene.saveToFile(fil2);
1113 
1114  // Textual representation:
1115  string fil3(filNamePrefix + string("_covs.txt"));
1116  saveToTextFile(fil3);
1117 
1118  // Total number of particles / modes:
1119  string fil4(filNamePrefix + string("_population.txt"));
1120  {
1121  FILE* f = os::fopen(fil4.c_str(), "wt");
1122  if (f)
1123  {
1124  size_t nParts = 0, nGaussians = 0;
1125 
1126  for (const auto& m_beacon : m_beacons)
1127  {
1128  switch (m_beacon.m_typePDF)
1129  {
1131  nParts += m_beacon.m_locationMC.size();
1132  break;
1133  case CBeacon::pdfSOG:
1134  nGaussians += m_beacon.m_locationSOG.size();
1135  break;
1136  case CBeacon::pdfGauss:
1137  nGaussians++;
1138  break;
1139  };
1140  }
1141 
1142  fprintf(
1143  f, "%u %u", static_cast<unsigned>(nParts),
1144  static_cast<unsigned>(nGaussians));
1145  os::fclose(f);
1146  }
1147  }
1148 
1149  MRPT_END
1150 }
1151 
1152 /*---------------------------------------------------------------
1153  getAs3DObject
1154  ---------------------------------------------------------------*/
1156 {
1157  MRPT_START
1158 
1159  if (!genericMapParams.enableSaveAs3DObject) return;
1160 
1161  // ------------------------------------------------
1162  // Add the XYZ corner for the current area:
1163  // ------------------------------------------------
1164  outObj->insert(opengl::stock_objects::CornerXYZ());
1165 
1166  // Save 3D ellipsoids or whatever representation:
1167  for (const auto& m_beacon : m_beacons) m_beacon.getAs3DObject(outObj);
1168 
1169  MRPT_END
1170 }
1171 
1172 /*---------------------------------------------------------------
1173  Computes the ratio in [0,1] of correspondences between "this" and the
1174  "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
1175  * In the case of a multi-metric map, this returns the average between the
1176  maps. This method always return 0 for grid maps.
1177  * \param otherMap [IN] The other map to compute the matching
1178  with.
1179  * \param otherMapPose [IN] The 6D pose of the other map as seen
1180  from "this".
1181  * \param maxDistForCorr [IN] The minimum distance between 2
1182  non-probabilistic map elements for counting them as a correspondence.
1183  * \param maxMahaDistForCorr [IN] The minimum Mahalanobis distance
1184  between 2 probabilistic map elements for counting them as a correspondence.
1185  *
1186  * \return The matching ratio [0,1]
1187  * \sa computeMatchingWith2D
1188  ----------------------------------------------------------------*/
1190  const mrpt::maps::CMetricMap* otherMap2,
1191  const mrpt::poses::CPose3D& otherMapPose,
1192  const TMatchingRatioParams& params) const
1193 {
1194  MRPT_START
1195 
1196  // Compare to a similar map only:
1197  const CBeaconMap* otherMap = nullptr;
1198 
1199  if (otherMap2->GetRuntimeClass() == CLASS_ID(CBeaconMap))
1200  otherMap = dynamic_cast<const CBeaconMap*>(otherMap2);
1201 
1202  if (!otherMap) return 0;
1203 
1204  TMatchingPairList matchList;
1205  vector<bool> otherCorrespondences;
1206  float out_corrsRatio;
1207 
1208  CBeaconMap modMap;
1209 
1210  modMap.changeCoordinatesReference(otherMapPose, otherMap);
1211 
1212  computeMatchingWith3DLandmarks(
1213  &modMap, matchList, out_corrsRatio, otherCorrespondences);
1214 
1215  return out_corrsRatio;
1216 
1217  MRPT_END
1218 }
1219 
1220 /*---------------------------------------------------------------
1221  getBeaconByID
1222  ---------------------------------------------------------------*/
1224 {
1225  for (const auto& m_beacon : m_beacons)
1226  if (m_beacon.m_ID == id) return &m_beacon;
1227  return nullptr;
1228 }
1229 
1230 /*---------------------------------------------------------------
1231  getBeaconByID
1232  ---------------------------------------------------------------*/
1234 {
1235  for (auto& m_beacon : m_beacons)
1236  if (m_beacon.m_ID == id) return &m_beacon;
1237  return nullptr;
1238 }
1239 
1240 /*---------------------------------------------------------------
1241  saveToTextFile
1242 - VX VY VZ: Variances of each dimension (C11, C22, C33)
1243 - DET2D DET3D: Determinant of the 2D and 3D covariance matrixes.
1244 - C12, C13, C23: Cross covariances
1245  ---------------------------------------------------------------*/
1246 void CBeaconMap::saveToTextFile(const string& fil) const
1247 {
1248  MRPT_START
1249  FILE* f = os::fopen(fil.c_str(), "wt");
1250  ASSERT_(f != nullptr);
1251 
1252  for (const auto& m_beacon : m_beacons)
1253  {
1254  const auto [C, p] = m_beacon.getCovarianceAndMean();
1255 
1256  float D3 = C.det();
1257  float D2 = C(0, 0) * C(1, 1) - square(C(0, 1));
1258  os::fprintf(
1259  f, "%i %f %f %f %e %e %e %e %e %e %e %e\n",
1260  static_cast<int>(m_beacon.m_ID), p.x(), p.y(), p.z(), C(0, 0),
1261  C(1, 1), C(2, 2), D2, D3, C(0, 1), C(1, 2), C(1, 2));
1262  }
1263 
1264  os::fclose(f);
1265  MRPT_END
1266 }
A namespace of pseudo-random numbers generators of diferent distributions.
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
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:458
#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
double drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
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:960
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
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
T square(const T x)
Inline function for the square of a number.
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:971
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
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:410
float sensedDistance
The sensed range itself (in meters).
#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:977
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:58
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:920
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).
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
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: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020