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