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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 3316834cb Wed Aug 21 11:42:56 2019 +0200 at miƩ ago 21 11:50:11 CEST 2019