Main MRPT website > C++ reference for MRPT 1.9.9
CMultiMetricMapPDF.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 "slam-precomp.h" // Precompiled headers
11 
12 #include <mrpt/random.h>
13 #include <mrpt/system/CTicTac.h>
14 #include <mrpt/io/CFileStream.h>
15 #include <mrpt/system/os.h>
16 
26 
28 
29 using namespace mrpt;
30 using namespace mrpt::math;
31 using namespace mrpt::slam;
32 using namespace mrpt::obs;
33 using namespace mrpt::maps;
34 using namespace mrpt::poses;
35 using namespace mrpt::random;
36 using namespace mrpt::system;
37 using namespace std;
38 
41 
42 /*---------------------------------------------------------------
43  Constructor
44  ---------------------------------------------------------------*/
46  const bayes::CParticleFilter::TParticleFilterOptions& opts,
47  const mrpt::maps::TSetOfMetricMapInitializers* mapsInitializers,
48  const TPredictionParams* predictionOptions)
49  : averageMap(mapsInitializers),
50  averageMapIsUpdated(false),
51  SFs(),
52  SF2robotPath(),
53  options(),
54  newInfoIndex(0)
55 {
56  m_particles.resize(opts.sampleSize);
57  for (CParticleList::iterator it = m_particles.begin();
58  it != m_particles.end(); ++it)
59  {
60  it->log_w = 0;
61  it->d.reset(new CRBPFParticleData(mapsInitializers));
62  }
63 
64  // Initialize:
65  const CPose3D nullPose(0, 0, 0);
66  clear(nullPose);
67 
68  // If provided, copy the whole set of params now:
69  if (predictionOptions != nullptr) options = *predictionOptions;
70 }
71 
72 /*---------------------------------------------------------------
73  clear
74  ---------------------------------------------------------------*/
75 void CMultiMetricMapPDF::clear(const CPose2D& initialPose)
76 {
77  CPose3D p(initialPose);
78  clear(p);
79 }
80 
81 /*---------------------------------------------------------------
82  clear
83  ---------------------------------------------------------------*/
84 void CMultiMetricMapPDF::clear(const CPose3D& initialPose)
85 {
86  const size_t M = m_particles.size();
87  for (size_t i = 0; i < M; i++)
88  {
89  m_particles[i].log_w = 0;
90 
91  m_particles[i].d->mapTillNow.clear();
92 
93  m_particles[i].d->robotPath.resize(1);
94  m_particles[i].d->robotPath[0] = initialPose.asTPose();
95  }
96 
97  SFs.clear();
98  SF2robotPath.clear();
99 
100  averageMapIsUpdated = false;
101 }
102 
104  const mrpt::maps::CSimpleMap& prevMap,
105  const mrpt::poses::CPose3D& currentPose)
106 {
107  const size_t nParts = m_particles.size(), nOldKeyframes = prevMap.size();
108  if (nOldKeyframes == 0)
109  {
110  // prevMap is empty, so reset the map
111  clear(currentPose);
112  return;
113  }
114  for (size_t idxPart = 0; idxPart < nParts; idxPart++)
115  {
116  auto& p = m_particles[idxPart];
117  p.log_w = 0;
118 
119  p.d->mapTillNow.clear();
120 
121  p.d->robotPath.resize(nOldKeyframes);
122  for (size_t i = 0; i < nOldKeyframes; i++)
123  {
124  CPose3DPDF::Ptr keyframe_pose;
125  CSensoryFrame::Ptr sfkeyframe_sf;
126  prevMap.get(i, keyframe_pose, sfkeyframe_sf);
127 
128  // as pose, use: if the PDF is also a PF with the same number of
129  // samples, use those particles;
130  // otherwise, simply use the mean for all particles as an
131  // approximation (with loss of uncertainty).
132  mrpt::poses::CPose3D kf_pose;
133  bool kf_pose_set = false;
134  if (IS_CLASS(keyframe_pose, CPose3DPDFParticles))
135  {
136  const auto pdf_parts = dynamic_cast<const CPose3DPDFParticles*>(
137  keyframe_pose.get());
138  ASSERT_(pdf_parts);
139  if (pdf_parts->particlesCount() == nParts)
140  {
141  kf_pose = CPose3D(pdf_parts->m_particles[idxPart].d);
142  kf_pose_set = true;
143  }
144  }
145  if (!kf_pose_set)
146  {
147  kf_pose = keyframe_pose->getMeanVal();
148  }
149  p.d->robotPath[i] = kf_pose.asTPose();
150  for (const auto& obs : *sfkeyframe_sf)
151  {
152  p.d->mapTillNow.insertObservation(&(*obs), &kf_pose);
153  }
154  }
155  }
156 
157  SFs = prevMap; // copy
158  SF2robotPath.clear();
159  SF2robotPath.reserve(nOldKeyframes);
160  for (size_t i = 0; i < nOldKeyframes; i++) SF2robotPath.push_back(i);
161 
162  averageMapIsUpdated = false;
163 }
164 
165 /*---------------------------------------------------------------
166  getEstimatedPosePDF
167  Returns an estimate of the pose, (the mean, or mathematical expectation of the
168  PDF), computed
169  as a weighted average over all m_particles.
170  ---------------------------------------------------------------*/
171 void CMultiMetricMapPDF::getEstimatedPosePDF(
172  CPose3DPDFParticles& out_estimation) const
173 {
174  ASSERT_(m_particles[0].d->robotPath.size() > 0);
175  getEstimatedPosePDFAtTime(
176  m_particles[0].d->robotPath.size() - 1, out_estimation);
177 }
178 
179 /*---------------------------------------------------------------
180  getEstimatedPosePDFAtTime
181  ---------------------------------------------------------------*/
182 void CMultiMetricMapPDF::getEstimatedPosePDFAtTime(
183  size_t timeStep, CPose3DPDFParticles& out_estimation) const
184 {
185  // CPose3D p;
186  size_t i, n = m_particles.size();
187 
188  // Delete current content of "out_estimation":
189  out_estimation.clearParticles();
190 
191  // Create new m_particles:
192  out_estimation.m_particles.resize(n);
193  for (i = 0; i < n; i++)
194  {
195  out_estimation.m_particles[i].d = m_particles[i].d->robotPath[timeStep];
196  out_estimation.m_particles[i].log_w = m_particles[i].log_w;
197  }
198 }
199 
200 uint8_t CRBPFParticleData::serializeGetVersion() const { return 0; }
201 void CRBPFParticleData::serializeTo(mrpt::serialization::CArchive&) const
202 {
203  THROW_EXCEPTION("Shouldn't arrive here");
204 }
205 void CRBPFParticleData::serializeFrom(mrpt::serialization::CArchive&, uint8_t)
206 {
207  THROW_EXCEPTION("Shouldn't arrive here");
208 }
209 
210 uint8_t CMultiMetricMapPDF::serializeGetVersion() const { return 0; }
211 void CMultiMetricMapPDF::serializeTo(mrpt::serialization::CArchive& out) const
212 {
213  out.WriteAs<uint32_t>(m_particles.size());
214  for (const auto& part : m_particles)
215  {
216  out << part.log_w << part.d->mapTillNow;
217  out.WriteAs<uint32_t>(part.d->robotPath.size());
218  for (const auto& p : part.d->robotPath) out << p;
219  }
220  out << SFs << SF2robotPath;
221 }
222 
223 void CMultiMetricMapPDF::serializeFrom(
225 {
226  switch (version)
227  {
228  case 0:
229  {
230  uint32_t i, n, j, m;
231 
232  // Delete current contents:
233  // --------------------------
234  clearParticles();
235  SFs.clear();
236  SF2robotPath.clear();
237 
238  averageMapIsUpdated = false;
239 
240  // Load the new data:
241  // --------------------
242  in >> n;
243 
244  m_particles.resize(n);
245  for (i = 0; i < n; i++)
246  {
247  m_particles[i].d.reset(new CRBPFParticleData());
248 
249  // Load
250  in >> m_particles[i].log_w >> m_particles[i].d->mapTillNow;
251 
252  in >> m;
253  m_particles[i].d->robotPath.resize(m);
254  for (j = 0; j < m; j++) in >> m_particles[i].d->robotPath[j];
255  }
256 
257  in >> SFs >> SF2robotPath;
258  }
259  break;
260  default:
262  };
263 }
264 
265 TPose3D CMultiMetricMapPDF::getLastPose(
266  const size_t i, bool& is_valid_pose) const
267 {
268  if (i >= m_particles.size())
269  THROW_EXCEPTION("Particle index out of bounds!");
270 
271  if (m_particles[i].d->robotPath.empty())
272  {
273  is_valid_pose = false;
274  return TPose3D(0, 0, 0, 0, 0, 0);
275  }
276  else
277  {
278  return *m_particles[i].d->robotPath.rbegin();
279  }
280 }
281 
282 const CMultiMetricMap* CMultiMetricMapPDF::getAveragedMetricMapEstimation()
283 {
284  rebuildAverageMap();
285  return &averageMap;
286 }
287 
288 /*---------------------------------------------------------------
289  getWeightedAveragedMap
290  ---------------------------------------------------------------*/
291 void CMultiMetricMapPDF::rebuildAverageMap()
292 {
293  float min_x = 1e6, max_x = -1e6, min_y = 1e6, max_y = -1e6;
295 
296  if (averageMapIsUpdated) return;
297 
298  // ---------------------------------------------------------
299  // GRID
300  // ---------------------------------------------------------
301  for (part = m_particles.begin(); part != m_particles.end(); ++part)
302  {
303  ASSERT_(part->d->mapTillNow.m_gridMaps.size() > 0);
304 
305  min_x = min(min_x, part->d->mapTillNow.m_gridMaps[0]->getXMin());
306  max_x = max(max_x, part->d->mapTillNow.m_gridMaps[0]->getXMax());
307  min_y = min(min_y, part->d->mapTillNow.m_gridMaps[0]->getYMin());
308  max_y = max(max_y, part->d->mapTillNow.m_gridMaps[0]->getYMax());
309  }
310 
311  // Asure all maps have the same dimensions:
312  for (part = m_particles.begin(); part != m_particles.end(); ++part)
313  part->d->mapTillNow.m_gridMaps[0]->resizeGrid(
314  min_x, max_x, min_y, max_y, 0.5f, false);
315 
316  for (part = m_particles.begin(); part != m_particles.end(); ++part)
317  {
318  min_x = min(min_x, part->d->mapTillNow.m_gridMaps[0]->getXMin());
319  max_x = max(max_x, part->d->mapTillNow.m_gridMaps[0]->getXMax());
320  min_y = min(min_y, part->d->mapTillNow.m_gridMaps[0]->getYMin());
321  max_y = max(max_y, part->d->mapTillNow.m_gridMaps[0]->getYMax());
322  }
323 
324  // Prepare target map:
325  ASSERT_(averageMap.m_gridMaps.size() > 0);
326  averageMap.m_gridMaps[0]->setSize(
327  min_x, max_x, min_y, max_y,
328  m_particles[0].d->mapTillNow.m_gridMaps[0]->getResolution(), 0);
329 
330  // Compute the sum of weights:
331  double sumLinearWeights = 0;
332  for (part = m_particles.begin(); part != m_particles.end(); ++part)
333  sumLinearWeights += exp(part->log_w);
334 
335  // CHECK:
336  for (part = m_particles.begin(); part != m_particles.end(); ++part)
337  {
338  ASSERT_(
339  part->d->mapTillNow.m_gridMaps[0]->getSizeX() ==
340  averageMap.m_gridMaps[0]->getSizeX());
341  ASSERT_(
342  part->d->mapTillNow.m_gridMaps[0]->getSizeY() ==
343  averageMap.m_gridMaps[0]->getSizeY());
344  }
345 
346  {
347  // ******************************************************
348  // Implementation WITHOUT the SSE Instructions Set
349  // ******************************************************
350  MRPT_START
351 
352  // Reserve a float grid-map, add weight all maps
353  // -------------------------------------------------------------------------------------------
354  std::vector<float> floatMap;
355  floatMap.resize(averageMap.m_gridMaps[0]->map.size(), 0);
356 
357  // For each particle in the RBPF:
358  double sumW = 0;
359  for (part = m_particles.begin(); part != m_particles.end(); ++part)
360  sumW += exp(part->log_w);
361 
362  if (sumW == 0) sumW = 1;
363 
364  for (part = m_particles.begin(); part != m_particles.end(); ++part)
365  {
366  // Variables:
369  part->d->mapTillNow.m_gridMaps[0]->map.begin();
371  part->d->mapTillNow.m_gridMaps[0]->map.end();
373 
374  // The weight of particle:
375  float w = exp(part->log_w) / sumW;
376 
377  ASSERT_(
378  part->d->mapTillNow.m_gridMaps[0]->map.size() ==
379  floatMap.size());
380 
381  // For each cell in individual maps:
382  for (srcCell = firstSrcCell, destCell = floatMap.begin();
383  srcCell != lastSrcCell; srcCell++, destCell++)
384  (*destCell) += w * (*srcCell);
385  }
386 
387  // Copy to fixed point map:
390  averageMap.m_gridMaps[0]->map.begin();
391 
392  ASSERT_(averageMap.m_gridMaps[0]->map.size() == floatMap.size());
393 
394  for (srcCell = floatMap.begin(); srcCell != floatMap.end();
395  srcCell++, destCell++)
396  *destCell = static_cast<COccupancyGridMap2D::cellType>(*srcCell);
397 
398  MRPT_END
399  } // End of SSE not supported
400 
401  // Don't calculate again until really necesary.
402  averageMapIsUpdated = true;
403 }
404 
405 /*---------------------------------------------------------------
406  insertObservation
407  ---------------------------------------------------------------*/
408 bool CMultiMetricMapPDF::insertObservation(CSensoryFrame& sf)
409 {
410  const size_t M = particlesCount();
411 
412  // Insert into SFs:
413  CPose3DPDFParticles::Ptr posePDF =
414  mrpt::make_aligned_shared<CPose3DPDFParticles>();
415  getEstimatedPosePDF(*posePDF);
416 
417  // Insert it into the SFs and the SF2robotPath list:
418  const uint32_t new_sf_id = SFs.size();
419  SFs.insert(posePDF, CSensoryFrame::Ptr(new CSensoryFrame(sf)));
420  SF2robotPath.resize(new_sf_id + 1);
421  SF2robotPath[new_sf_id] = m_particles[0].d->robotPath.size() - 1;
422 
423  bool anymap = false;
424  for (size_t i = 0; i < M; i++)
425  {
426  bool pose_is_valid;
427  const CPose3D robotPose = CPose3D(getLastPose(i, pose_is_valid));
428  // ASSERT_(pose_is_valid); // if not, use the default (0,0,0)
429  const bool map_modified = sf.insertObservationsInto(
430  &m_particles[i].d->mapTillNow, &robotPose);
431  anymap = anymap || map_modified;
432  }
433 
434  averageMapIsUpdated = false;
435  return anymap;
436 }
437 
438 /*---------------------------------------------------------------
439  getPath
440  ---------------------------------------------------------------*/
441 void CMultiMetricMapPDF::getPath(
442  size_t i, std::deque<math::TPose3D>& out_path) const
443 {
444  if (i >= m_particles.size()) THROW_EXCEPTION("Index out of bounds");
445  out_path = m_particles[i].d->robotPath;
446 }
447 
448 /*---------------------------------------------------------------
449  getCurrentEntropyOfPaths
450  ---------------------------------------------------------------*/
451 double CMultiMetricMapPDF::getCurrentEntropyOfPaths()
452 {
453  size_t i;
454  size_t N =
455  m_particles[0].d->robotPath.size(); // The poses count along the paths
456 
457  // Compute paths entropy:
458  // ---------------------------
459  double H_paths = 0;
460 
461  if (N)
462  {
463  // For each pose along the path:
464  for (i = 0; i < N; i++)
465  {
466  // Get pose est. as m_particles:
467  CPose3DPDFParticles posePDFParts;
468  getEstimatedPosePDFAtTime(i, posePDFParts);
469 
470  // Approximate to gaussian and compute entropy of covariance:
471  H_paths += posePDFParts.getCovarianceEntropy();
472  }
473  H_paths /= N;
474  }
475  return H_paths;
476 }
477 
478 /*---------------------------------------------------------------
479  getCurrentJointEntropy
480  ---------------------------------------------------------------*/
481 double CMultiMetricMapPDF::getCurrentJointEntropy()
482 {
483  double H_joint, H_paths, H_maps;
484  size_t i, M = m_particles.size();
486 
487  // Entropy of the paths:
488  H_paths = getCurrentEntropyOfPaths();
489 
490  float min_x = 1e6, max_x = -1e6, min_y = 1e6, max_y = -1e6;
492 
493  // ---------------------------------------------------------
494  // ASSURE ALL THE GRIDS ARE THE SAME SIZE!
495  // ---------------------------------------------------------
496  for (part = m_particles.begin(); part != m_particles.end(); ++part)
497  {
498  ASSERT_(part->d->mapTillNow.m_gridMaps.size() > 0);
499 
500  min_x = min(min_x, part->d->mapTillNow.m_gridMaps[0]->getXMin());
501  max_x = max(max_x, part->d->mapTillNow.m_gridMaps[0]->getXMax());
502  min_y = min(min_y, part->d->mapTillNow.m_gridMaps[0]->getYMin());
503  max_y = max(max_y, part->d->mapTillNow.m_gridMaps[0]->getYMax());
504  }
505 
506  // Asure all maps have the same dimensions:
507  for (part = m_particles.begin(); part != m_particles.end(); ++part)
508  part->d->mapTillNow.m_gridMaps[0]->resizeGrid(
509  min_x, max_x, min_y, max_y, 0.5f, false);
510 
511  // Sum of linear weights:
512  double sumLinearWeights = 0;
513  for (i = 0; i < M; i++) sumLinearWeights += exp(m_particles[i].log_w);
514 
515  // Compute weighted maps entropy:
516  // --------------------------------
517  H_maps = 0;
518  for (i = 0; i < M; i++)
519  {
520  ASSERT_(m_particles[i].d->mapTillNow.m_gridMaps.size() > 0);
521 
522  m_particles[i].d->mapTillNow.m_gridMaps[0]->computeEntropy(entropy);
523  H_maps += exp(m_particles[i].log_w) * entropy.H / sumLinearWeights;
524  }
525 
526  printf("H_paths=%e\n", H_paths);
527  printf("H_maps=%e\n", H_maps);
528 
529  H_joint = H_paths + H_maps;
530  return H_joint;
531 }
532 
533 const CMultiMetricMap* CMultiMetricMapPDF::getCurrentMostLikelyMetricMap() const
534 {
535  size_t i, max_i = 0, n = m_particles.size();
536  double max_w = m_particles[0].log_w;
537 
538  for (i = 0; i < n; i++)
539  {
540  if (m_particles[i].log_w > max_w)
541  {
542  max_w = m_particles[i].log_w;
543  max_i = i;
544  }
545  }
546 
547  // Return its map:
548  return &m_particles[max_i].d->mapTillNow;
549 }
550 
551 /*---------------------------------------------------------------
552  updateSensoryFrameSequence
553  ---------------------------------------------------------------*/
554 void CMultiMetricMapPDF::updateSensoryFrameSequence()
555 {
556  MRPT_START
557  CPose3DPDFParticles posePartsPDF;
558  CPose3DPDF::Ptr previousPosePDF;
559  CSensoryFrame::Ptr dummy;
560 
561  for (size_t i = 0; i < SFs.size(); i++)
562  {
563  // Get last estimation:
564  SFs.get(i, previousPosePDF, dummy);
565 
566  // Compute the new one:
567  getEstimatedPosePDFAtTime(SF2robotPath[i], posePartsPDF);
568 
569  // Copy into SFs:
570  previousPosePDF->copyFrom(posePartsPDF);
571  }
572 
573  MRPT_END
574 }
575 
576 /*---------------------------------------------------------------
577  saveCurrentPathEstimationToTextFile
578  ---------------------------------------------------------------*/
579 void CMultiMetricMapPDF::saveCurrentPathEstimationToTextFile(
580  const std::string& fil)
581 {
582  FILE* f = os::fopen(fil.c_str(), "wt");
583  if (!f) return;
584 
585  for (CParticleList::iterator it = m_particles.begin();
586  it != m_particles.end(); ++it)
587  {
588  for (size_t i = 0; i < it->d->robotPath.size(); i++)
589  {
590  const mrpt::math::TPose3D& p = it->d->robotPath[i];
591 
592  os::fprintf(
593  f, "%.04f %.04f %.04f %.04f %.04f %.04f ", p.x, p.y, p.z, p.yaw,
594  p.pitch, p.roll);
595  }
596  os::fprintf(f, " %e\n", it->log_w);
597  }
598 
599  os::fclose(f);
600 }
601 
602 /*---------------------------------------------------------------
603  TPredictionParams
604  ---------------------------------------------------------------*/
605 CMultiMetricMapPDF::TPredictionParams::TPredictionParams()
606  : pfOptimalProposal_mapSelection(0),
607  ICPGlobalAlign_MinQuality(0.70f),
608  update_gridMapLikelihoodOptions(),
609  KLD_params(),
610  icp_params()
611 {
612 }
613 
614 /*---------------------------------------------------------------
615  dumpToTextStream
616  ---------------------------------------------------------------*/
618  std::ostream& out) const
619 {
620  out << mrpt::format(
621  "\n----------- [CMultiMetricMapPDF::TPredictionParams] ------------ "
622  "\n\n");
623 
624  out << mrpt::format(
625  "pfOptimalProposal_mapSelection = %i\n",
626  pfOptimalProposal_mapSelection);
627  out << mrpt::format(
628  "ICPGlobalAlign_MinQuality = %f\n",
629  ICPGlobalAlign_MinQuality);
630 
631  KLD_params.dumpToTextStream(out);
632  icp_params.dumpToTextStream(out);
633  out << mrpt::format("\n");
634 }
635 
636 /*---------------------------------------------------------------
637  loadFromConfigFile
638  ---------------------------------------------------------------*/
640  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
641 {
642  pfOptimalProposal_mapSelection = iniFile.read_int(
643  section, "pfOptimalProposal_mapSelection",
644  pfOptimalProposal_mapSelection, true);
645 
646  MRPT_LOAD_CONFIG_VAR(ICPGlobalAlign_MinQuality, float, iniFile, section);
647 
648  KLD_params.loadFromConfigFile(iniFile, section);
649  icp_params.loadFromConfigFile(iniFile, section);
650 }
A namespace of pseudo-random numbers generators of diferent distributions.
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:1046
Scalar * iterator
Definition: eigen_plugins.h:26
void clearParticles()
Free the memory of all the particles and reset the array "m_particles" to length zero.
#define MRPT_START
Definition: exceptions.h:262
#define min(a, b)
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:35
CParticleList m_particles
The array of particles.
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
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.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
GLenum GLsizei n
Definition: glext.h:5074
STL namespace.
void WriteAs(const TYPE_FROM_ACTUAL &value)
Definition: CArchive.h:152
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
double H
The target variable for absolute entropy, computed as: H(map)=Sumx,y{ -p(x,y)*ln(p(x,y)) -(1-p(x,y))*ln(1-p(x,y)) }
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(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.
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
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
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:53
void get(size_t index, mrpt::poses::CPose3DPDF::Ptr &out_posePDF, mrpt::obs::CSensoryFrame::Ptr &out_SF) const
Access to the i&#39;th pair, first one is index &#39;0&#39;.
Definition: CSimpleMap.cpp:56
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Auxiliary class used in mrpt::maps::CMultiMetricMapPDF.
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:406
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:103
#define MRPT_END
Definition: exceptions.h:266
The struct for passing extra simulation parameters to the prediction/update stage when running a part...
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
GLuint in
Definition: glext.h:7274
double getCovarianceEntropy() const
Compute the entropy of the estimated covariance matrix.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:255
This class stores any customizable set of metric maps.
unsigned __int32 uint32_t
Definition: rptypes.h:47
bool insertObservationsInto(mrpt::maps::CMetricMap *theMap, const mrpt::poses::CPose3D *robotPose=nullptr) const
Insert all the observations in this SF into a metric map or any kind (see mrpt::maps::CMetricMap).
Declares a class that represents a Rao-Blackwellized set of particles for solving the SLAM problem (T...
Used for returning entropy related information.
GLfloat GLfloat p
Definition: glext.h:6305
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:188
void clear()
Remove all stored pairs.
Definition: CSimpleMap.cpp:55
Declares a class that represents a Probability Density function (PDF) of a 3D pose.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019