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


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST