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 
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.
GLsizei range
Definition: glext.h:5993
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
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
GLenum GLsizei n
Definition: glext.h:5136
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
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
GLdouble s
Definition: glext.h:3682
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
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
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...
GLsizei const GLchar ** string
Definition: glext.h:4116
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...
GLint mode
Definition: glext.h:5753
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...
GLuint in
Definition: glext.h:7391
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:58
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
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
GLenum GLint GLint y
Definition: glext.h:3542
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
GLsizeiptr size
Definition: glext.h:3934
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...
GLfloat GLfloat p
Definition: glext.h:6398
GLenum const GLfloat * params
Definition: glext.h:3538
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: cb8dd5fc8 Sat Dec 7 21:55:39 2019 +0100 at sáb dic 7 22:00:13 CET 2019