MRPT  2.0.4
CMetricMapBuilderRBPF.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/core/lock_helper.h>
13 #include <mrpt/core/round.h>
19 
20 using namespace mrpt;
21 using namespace mrpt::slam;
22 using namespace mrpt::math;
23 using namespace mrpt::maps;
24 using namespace mrpt::obs;
25 using namespace mrpt::poses;
26 using namespace mrpt::bayes;
27 using namespace mrpt::img;
28 
29 /*---------------------------------------------------------------
30  Constructor
31  ---------------------------------------------------------------*/
33  const TConstructionOptions& initializationOptions)
34  : mapPDF(
35  initializationOptions.PF_options,
36  initializationOptions.mapsInitializers,
37  initializationOptions.predictionOptions),
38  m_PF_options(initializationOptions.PF_options),
39  insertionLinDistance(initializationOptions.insertionLinDistance),
40  insertionAngDistance(initializationOptions.insertionAngDistance),
41  localizeLinDistance(initializationOptions.localizeLinDistance),
42  localizeAngDistance(initializationOptions.localizeAngDistance),
43  odoIncrementSinceLastLocalization(),
44  odoIncrementSinceLastMapUpdate()
45 {
46  setLoggerName("CMetricMapBuilderRBPF");
47  setVerbosityLevel(initializationOptions.verbosity_level);
48  // Reset:
49  clear();
50 }
51 
53 {
54  this->setLoggerName("CMetricMapBuilderRBPF");
55  MRPT_LOG_WARN("Empty constructor invoked!\n");
56 }
57 
59  const CMetricMapBuilderRBPF& src)
60 {
61  if (this == &src)
62  {
63  return *this;
64  }
65  mapPDF = src.mapPDF;
74  return *this;
75 }
76 
77 /*---------------------------------------------------------------
78  Destructor
79  ---------------------------------------------------------------*/
81 /*---------------------------------------------------------------
82  clear
83  ---------------------------------------------------------------*/
85 {
87 
88  MRPT_LOG_DEBUG("CMetricMapBuilderRBPF::clear() called.");
89  static CPose2D nullPose(0, 0, 0);
90 
91  // Reset traveled distances counters:
93 
95 
96  // Clear maps for each particle:
97  mapPDF.clear(nullPose);
98 }
99 
100 /*---------------------------------------------------------------
101  processActionObservation
102  ---------------------------------------------------------------*/
104  CActionCollection& action, CSensoryFrame& observations)
105 {
106  MRPT_START
108 
109  // Update the traveled distance estimations:
110  {
115  if (act3D)
116  {
118  "processActionObservation(): Input action is "
119  "CActionRobotMovement3D="
120  << act3D->poseChange.getMeanVal().asString());
121  odoIncrementSinceLastMapUpdate += act3D->poseChange.getMeanVal();
122  odoIncrementSinceLastLocalization += act3D->poseChange;
123  }
124  else if (act2D)
125  {
127  "processActionObservation(): Input action is "
128  "CActionRobotMovement2D="
129  << act2D->poseChange->getMeanVal().asString());
131  mrpt::poses::CPose3D(act2D->poseChange->getMeanVal());
133  mrpt::poses::CPose3D(act2D->poseChange->getMeanVal());
134  }
135  else
136  {
137  MRPT_LOG_WARN("Action contains no odometry.\n");
138  }
139  }
140 
141  // Execute particle filter:
142  // But only if the traveled distance since the last update is long enough,
143  // or this is the first observation, etc...
144  // ----------------------------------------------------------------------------
145  bool do_localization =
146  (!mapPDF.SFs.size() || // This is the first observation!
151 
152  bool do_map_update =
153  (!mapPDF.SFs.size() || // This is the first observation!
157 
158  // Used any "options.alwaysInsertByClass" ??
159  for (auto itCl = options.alwaysInsertByClass.data.begin();
160  !do_map_update && itCl != options.alwaysInsertByClass.data.end();
161  ++itCl)
162  for (auto& o : observations)
163  if (o->GetRuntimeClass() == *itCl)
164  {
165  do_map_update = true;
166  do_localization = true;
167  break;
168  }
169 
170  if (do_map_update) do_localization = true;
171 
173  "do_map_update=%s do_localization=%s", do_map_update ? "YES" : "NO",
174  do_localization ? "YES" : "NO"));
175 
176  if (do_localization)
177  {
178  // Create an artificial action object, since
179  // we've been collecting them until a threshold:
180  // ------------------------------------------------
181  CActionCollection fakeActs;
182  {
185  if (act3D)
186  {
187  CActionRobotMovement3D newAct;
188  newAct.estimationMethod = act3D->estimationMethod;
190  newAct.timestamp = act3D->timestamp;
191  fakeActs.insert(newAct);
192  }
193  else
194  {
195  // It must be 2D odometry:
198  ASSERT_(act2D);
199  CActionRobotMovement2D newAct;
200  newAct.computeFromOdometry(
202  act2D->motionModelConfiguration);
203  newAct.timestamp = act2D->timestamp;
204  fakeActs.insert(newAct);
205  }
206  }
207 
209  "odoIncrementSinceLastLocalization before resetting = "
211  // Reset distance counters:
214 
215  CParticleFilter pf;
216  pf.m_options = m_PF_options;
218 
219  pf.executeOn(mapPDF, &fakeActs, &observations);
220 
222  {
223  // Get current pose estimation:
224  CPose3DPDFParticles poseEstimation;
225  CPose3D meanPose;
226  mapPDF.getEstimatedPosePDF(poseEstimation);
227  poseEstimation.getMean(meanPose);
228 
229  const auto [cov, estPos] = poseEstimation.getCovarianceAndMean();
230 
232  "New pose=" << estPos << std::endl
233  << "New ESS:" << mapPDF.ESS() << std::endl);
235  " STDs: x=%2.3f y=%2.3f z=%.03f yaw=%2.3fdeg\n",
236  sqrt(cov(0, 0)), sqrt(cov(1, 1)), sqrt(cov(2, 2)),
237  RAD2DEG(sqrt(cov(3, 3)))));
238  }
239  }
240 
241  if (do_map_update)
242  {
244 
245  // Update the particles' maps:
246  // -------------------------------------------------
247  MRPT_LOG_INFO("New observation inserted into the map.");
248 
249  // Add current observation to the map:
250  const bool anymap_update = mapPDF.insertObservation(observations);
251  if (!anymap_update)
253  "**No map was updated** after inserting a CSensoryFrame with "
254  << observations.size());
255 
257  }
258  else
259  {
261  }
262 
263  // Added 29/JUN/2007 JLBC: Tell all maps that they can now free aux.
264  // variables
265  // (if any) since one PF cycle is over:
266  for (auto& m_particle : mapPDF.m_particles)
267  m_particle.d->mapTillNow.auxParticleFilterCleanUp();
268 
269  MRPT_END
270 }
271 
272 /*---------------------------------------------------------------
273  initialize
274  ---------------------------------------------------------------*/
276  const CSimpleMap& initialMap, const CPosePDF* x0)
277 {
279  "[initialize] Called with " << initialMap.size()
280  << " nodes in fixed map");
281 
282  this->clear();
283 
285 
286  mrpt::poses::CPose3D curPose;
287  if (x0)
288  {
289  curPose = mrpt::poses::CPose3D(x0->getMeanVal());
290  }
291  else if (!initialMap.empty())
292  {
293  // get pose of last keyframe:
294  curPose = initialMap.rbegin()->first->getMeanVal();
295  }
296  MRPT_LOG_INFO_STREAM("[initialize] Initial pose: " << curPose);
297 
298  // Clear maps for each particle & set pose:
299  mapPDF.clear(initialMap, curPose);
300 }
301 
302 /*---------------------------------------------------------------
303  getCurrentPoseEstimation
304  ---------------------------------------------------------------*/
306 {
307  CPose3DPDFParticles::Ptr posePDF = std::make_shared<CPose3DPDFParticles>();
308  mapPDF.getEstimatedPosePDF(*posePDF);
309 
310  // Adds additional increment from accumulated odometry since last
311  // localization update:
312  for (auto& p : posePDF->m_particles)
313  {
314  p.d.composePose(
316  }
317  return posePDF;
318 }
319 
320 /*---------------------------------------------------------------
321  getCurrentMostLikelyPath
322  ---------------------------------------------------------------*/
324  std::deque<TPose3D>& outPath) const
325 {
326  double maxW = -1, w;
327  size_t mostLik = 0;
328  for (size_t i = 0; i < mapPDF.particlesCount(); i++)
329  {
330  w = mapPDF.getW(i);
331  if (w > maxW)
332  {
333  maxW = w;
334  mostLik = i;
335  }
336  }
337 
338  mapPDF.getPath(mostLik, outPath);
339 }
340 
341 /*---------------------------------------------------------------
342  getCurrentlyBuiltMap
343  ---------------------------------------------------------------*/
345 {
346  const_cast<CMetricMapBuilderRBPF*>(this)
348  out_map = mapPDF.SFs;
349 }
350 
352 {
354 }
355 
356 /*---------------------------------------------------------------
357  getCurrentlyBuiltMapSize
358  ---------------------------------------------------------------*/
360 {
361  return mapPDF.SFs.size();
362 }
363 
365 {
366  using mrpt::round;
367 
368  unsigned int i, M = mapPDF.particlesCount();
369  std::deque<TPose3D> path;
370  unsigned int imgHeight = 0;
371 
372  MRPT_START
373 
374  const auto* curMap = mapPDF.getCurrentMostLikelyMetricMap();
375 
376  ASSERT_(curMap->countMapsByClass<COccupancyGridMap2D>() > 0);
377 
378  // Find which is the most likely path index:
379  unsigned int bestPath = 0;
380  double bestPathLik = -1;
381  for (i = 0; i < M; i++)
382  {
383  if (mapPDF.getW(i) > bestPathLik)
384  {
385  bestPathLik = mapPDF.getW(i);
386  bestPath = i;
387  }
388  }
389 
390  // Compute the length of the paths:
391  mapPDF.getPath(0, path);
392 
393  // Adapt the canvas size:
394  bool alreadyCopiedImage = false;
395  auto g = curMap->mapByClass<COccupancyGridMap2D>(0);
396  {
397  auto* obj = dynamic_cast<CImage*>(img);
398  if (obj) obj->resize(g->getSizeX(), g->getSizeY(), mrpt::img::CH_GRAY);
399  }
400  if (!alreadyCopiedImage)
401  {
402  CImage imgGrid;
403 
404  // grid map as bitmap:
405  // ----------------------------------
406  g->getAsImage(imgGrid);
407 
408  img->drawImage(0, 0, imgGrid);
409  imgHeight = imgGrid.getHeight();
410  }
411 
412  int x1 = 0, x2 = 0, y1 = 0, y2 = 0;
413  float x_min = g->getXMin();
414  float y_min = g->getYMin();
415  float resolution = g->getResolution();
416 
417  // Paths hypothesis:
418  // ----------------------------------
419  /***/
420  for (i = 0; i <= M; i++)
421  {
422  if (i != bestPath || i == M)
423  {
424  mapPDF.getPath(i == M ? bestPath : i, path);
425 
426  size_t nPoses = path.size();
427 
428  // First point: (0,0)
429  x2 = round((path[0].x - x_min) / resolution);
430  y2 = round((path[0].y - y_min) / resolution);
431 
432  // Draw path in the bitmap:
433  for (size_t j = 0; j < nPoses; j++)
434  {
435  // For next segment
436  x1 = x2;
437  y1 = y2;
438 
439  // Coordinates -> pixels
440  x2 = round((path[j].x - x_min) / resolution);
441  y2 = round((path[j].y - y_min) / resolution);
442 
443  // Draw line:
444  img->line(
445  x1, round((imgHeight - 1) - y1), x2,
446  round((imgHeight - 1) - y2),
447  i == M ? TColor(0, 0, 0)
448  : TColor(0x50, 0x50, 0x50), // Color, gray levels,
449  i == M ? 3 : 1 // Line width
450  );
451  }
452  }
453  }
454 
455  MRPT_END
456 }
457 
458 /*---------------------------------------------------------------
459  saveCurrentEstimationToImage
460  ---------------------------------------------------------------*/
462  const std::string& file, bool formatEMF_BMP)
463 {
464  MRPT_START
465 
466  if (formatEMF_BMP)
467  {
468  // Draw paths (using vectorial plots!) over the EMF file:
469  // --------------------------------------------------------
470  CEnhancedMetaFile EMF(file, 100 /* Scale */);
472  }
473  else
474  {
475  CImage img(1, 1, CH_GRAY);
477  img.saveToFile(file);
478  }
479 
480  MRPT_END
481 }
482 
483 /*---------------------------------------------------------------
484  getCurrentJointEntropy
485  ---------------------------------------------------------------*/
487 {
489 }
490 
491 /*---------------------------------------------------------------
492  saveCurrentPathEstimationToTextFile
493  ---------------------------------------------------------------*/
495  const std::string& fil)
496 {
498 }
499 
500 /*---------------------------------------------------------------
501  TConstructionOptions
502  ---------------------------------------------------------------*/
504  : insertionAngDistance(30.0_deg),
505 
506  localizeAngDistance(10.0_deg),
507  PF_options(),
508  mapsInitializers(),
509  predictionOptions()
510 
511 {
512 }
513 
514 /*---------------------------------------------------------------
515  dumpToTextStream
516  ---------------------------------------------------------------*/
518  std::ostream& out) const
519 {
520  out << "\n----------- [CMetricMapBuilderRBPF::TConstructionOptions] "
521  "------------ \n\n";
522 
523  out << mrpt::format(
524  "insertionLinDistance = %f m\n",
526  out << mrpt::format(
527  "insertionAngDistance = %f deg\n",
529  out << mrpt::format(
530  "localizeLinDistance = %f m\n",
532  out << mrpt::format(
533  "localizeAngDistance = %f deg\n",
535  out << mrpt::format(
536  "verbosity_level = %s\n",
538  verbosity_level)
539  .c_str());
540 
541  PF_options.dumpToTextStream(out);
542 
543  out << " Now showing 'mapsInitializers' and 'predictionOptions':\n";
544  out << "\n";
545 
546  mapsInitializers.dumpToTextStream(out);
547  predictionOptions.dumpToTextStream(out);
548 }
549 
550 /*---------------------------------------------------------------
551  loadFromConfigFile
552  ---------------------------------------------------------------*/
554  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
555 {
556  MRPT_START
557 
558  PF_options.loadFromConfigFile(iniFile, section);
559 
562  insertionAngDistance_deg, double, insertionAngDistance, iniFile,
563  section);
564 
567  localizeAngDistance_deg, double, localizeAngDistance, iniFile, section);
568  verbosity_level = iniFile.read_enum<mrpt::system::VerbosityLevel>(
569  section, "verbosity_level", verbosity_level);
570 
571  mapsInitializers.loadFromConfigFile(iniFile, section);
572  predictionOptions.loadFromConfigFile(iniFile, section);
573 
574  MRPT_END
575 }
mrpt::img::CImage::resize
void resize(std::size_t width, std::size_t height, TImageChannels nChannels, PixelDepth depth=PixelDepth::D8U)
Changes the size of the image, erasing previous contents (does NOT scale its current content,...
Definition: CImage.cpp:250
mrpt::maps::CMultiMetricMapPDF::insertObservation
bool insertObservation(mrpt::obs::CSensoryFrame &sf)
Insert an observation to the map, at each particle's pose and to each particle's metric map.
Definition: CMultiMetricMapPDF.cpp:401
mrpt::math::cov
CMatrixDouble cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample,...
Definition: ops_matrices.h:149
mrpt::maps::CSimpleMap::empty
bool empty() const
Returns size()!=0.
Definition: CSimpleMap.cpp:54
mrpt::system::COutputLogger::isLoggingLevelVisible
bool isLoggingLevelVisible(VerbosityLevel level) const
Definition: system/COutputLogger.h:202
COccupancyGridMap2D.h
mrpt::slam::CMetricMapBuilder::options
TOptions options
Definition: CMetricMapBuilder.h:141
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:40
mrpt::slam::CMetricMapBuilderRBPF::odoIncrementSinceLastMapUpdate
mrpt::poses::CPose3D odoIncrementSinceLastMapUpdate
Traveled distance since last map update.
Definition: CMetricMapBuilderRBPF.h:76
mrpt::bayes::CParticleFilter::executeOn
void executeOn(CParticleFilterCapable &obj, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, TParticleFilterStats *stats=nullptr)
Executes a complete prediction + update step of the selected particle filtering algorithm.
Definition: CParticleFilter.cpp:45
mrpt::obs::CActionCollection::getActionByClass
T::Ptr getActionByClass(size_t ith=0) const
Access to the i'th action of a given class, or a nullptr smart pointer if there is no action of that ...
Definition: CActionCollection.h:129
mrpt::maps::CMultiMetricMapPDF::saveCurrentPathEstimationToTextFile
void saveCurrentPathEstimationToTextFile(const std::string &fil)
A logging utility: saves the current path estimation for each particle in a text file (a row per part...
Definition: CMultiMetricMapPDF.cpp:570
mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions::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: CMetricMapBuilderRBPF.cpp:553
mrpt::slam::CMetricMapBuilderRBPF::m_PF_options
bayes::CParticleFilter::TParticleFilterOptions m_PF_options
The configuration of the particle filter.
Definition: CMetricMapBuilderRBPF.h:63
mrpt::slam::CMetricMapBuilderRBPF::getCurrentMostLikelyPath
void getCurrentMostLikelyPath(std::deque< mrpt::math::TPose3D > &outPath) const
Returns the current most-likely path estimation (the path associated to the most likely particle).
Definition: CMetricMapBuilderRBPF.cpp:323
MRPT_LOG_INFO
#define MRPT_LOG_INFO(_STRING)
Definition: system/COutputLogger.h:429
mrpt::slam::CMetricMapBuilderRBPF::TStats::observationsInserted
bool observationsInserted
Whether the SF has been inserted in the metric maps.
Definition: CMetricMapBuilderRBPF.h:198
mrpt::system::COutputLogger::setVerbosityLevel
void setVerbosityLevel(const VerbosityLevel level)
alias of setMinLoggingLevel()
Definition: COutputLogger.cpp:149
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:306
MRPT_LOG_DEBUG
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
Definition: system/COutputLogger.h:427
CMetricMapBuilderRBPF.h
mrpt::bayes::CParticleFilter::m_options
CParticleFilter::TParticleFilterOptions m_options
The options to be used in the PF, must be set before executing any step of the particle filter.
Definition: CParticleFilter.h:209
mrpt::bayes::CParticleFilterDataImpl::getW
double getW(size_t i) const override
Access to i'th particle (logarithm) weight, where first one is index 0.
Definition: CParticleFilterData.h:41
mrpt::maps::CMultiMetricMapPDF::clear
void clear(const mrpt::poses::CPose2D &initialPose)
Clear all elements of the maps, and restore all paths to a single starting pose.
Definition: CMultiMetricMapPDF.cpp:71
mrpt::poses::CPose3D::setFromValues
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:265
mrpt::bayes::CParticleFilter
This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilt...
Definition: CParticleFilter.h:52
mrpt::maps::CMultiMetricMapPDF::getPath
void getPath(size_t i, std::deque< math::TPose3D > &out_path) const
Return the path (in absolute coordinate poses) for the i'th particle.
Definition: CMultiMetricMapPDF.cpp:433
mrpt::maps::CMultiMetricMapPDF::getCurrentMostLikelyMetricMap
const CMultiMetricMap * getCurrentMostLikelyMetricMap() const
Returns a pointer to the current most likely map (associated to the most likely particle)
Definition: CMultiMetricMapPDF.cpp:524
mrpt::obs::CActionRobotMovement3D
Represents a probabilistic 3D (6D) movement.
Definition: CActionRobotMovement3D.h:27
CEnhancedMetaFile.h
mrpt::slam::CMetricMapBuilder::TOptions::debugForceInsertion
bool debugForceInsertion
Always insert into map.
Definition: CMetricMapBuilder.h:127
mrpt::slam::CMetricMapBuilderRBPF
This class implements a Rao-Blackwelized Particle Filter (RBPF) approach to map building (SLAM).
Definition: CMetricMapBuilderRBPF.h:56
mrpt::maps::CSimpleMap::rbegin
const_reverse_iterator rbegin() const
Definition: CSimpleMap.h:189
mrpt::poses::CPose3DPDFParticles::Ptr
std::shared_ptr< mrpt::poses ::CPose3DPDFParticles > Ptr
Definition: CPose3DPDFParticles.h:39
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::poses::CPoseOrPoint::norm
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:256
mrpt::slam::CMetricMapBuilderRBPF::~CMetricMapBuilderRBPF
~CMetricMapBuilderRBPF() override
Destructor.
mrpt::obs::CActionCollection
Declares a class for storing a collection of robot actions.
Definition: CActionCollection.h:27
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:16
mrpt::obs::CActionRobotMovement3D::Ptr
std::shared_ptr< mrpt::obs ::CActionRobotMovement3D > Ptr
Definition: CActionRobotMovement3D.h:28
mrpt::img::CEnhancedMetaFile
This class represents a Windows Enhanced Meta File (EMF) for generating and saving graphics.
Definition: CEnhancedMetaFile.h:24
mrpt::system::LVL_INFO
@ LVL_INFO
Definition: system/COutputLogger.h:31
mrpt::obs::CActionRobotMovement2D
Represents a probabilistic 2D movement of the robot mobile base.
Definition: CActionRobotMovement2D.h:31
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
MRPT_LOG_WARN_STREAM
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
Definition: system/COutputLogger.h:475
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:23
mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions::verbosity_level
mrpt::system::VerbosityLevel verbosity_level
Definition: CMetricMapBuilderRBPF.h:103
mrpt::slam::CMetricMapBuilderRBPF::saveCurrentPathEstimationToTextFile
void saveCurrentPathEstimationToTextFile(const std::string &fil)
A logging utility: saves the current path estimation for each particle in a text file (a row per part...
Definition: CMetricMapBuilderRBPF.cpp:494
mrpt::obs::CActionRobotMovement2D::Ptr
std::shared_ptr< mrpt::obs ::CActionRobotMovement2D > Ptr
Definition: CActionRobotMovement2D.h:32
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:18
mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions::TConstructionOptions
TConstructionOptions()
Constructor.
Definition: CMetricMapBuilderRBPF.cpp:503
mrpt::slam::CMetricMapBuilderRBPF::CMetricMapBuilderRBPF
CMetricMapBuilderRBPF()
This second constructor is created for the situation where a class member needs to be of type CMetric...
Definition: CMetricMapBuilderRBPF.cpp:52
CObservationStereoImages.h
mrpt::maps::CMultiMetricMapPDF::SFs
mrpt::maps::CSimpleMap SFs
The SFs and their corresponding pose estimations.
Definition: CMultiMetricMapPDF.h:100
mrpt::slam::CMetricMapBuilderRBPF::m_statsLastIteration
TStats m_statsLastIteration
This structure will hold stats after each execution of processActionObservation.
Definition: CMetricMapBuilderRBPF.h:203
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:52
mrpt::img
Definition: CCanvas.h:17
mrpt::img::CCanvas
This virtual class defines the interface of any object accepting drawing primitives on it.
Definition: CCanvas.h:42
mrpt::system::VerbosityLevel
VerbosityLevel
Enumeration of available verbosity levels.
Definition: system/COutputLogger.h:29
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24
mrpt::bayes::CParticleFilterDataImpl::particlesCount
size_t particlesCount() const override
Get the m_particles count.
Definition: CParticleFilterData.h:55
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::slam::CMetricMapBuilderRBPF::odoIncrementSinceLastLocalization
mrpt::poses::CPose3DPDFGaussian odoIncrementSinceLastLocalization
Traveled distance since last localization update.
Definition: CMetricMapBuilderRBPF.h:74
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:45
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:41
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: CMultiMetricMap.h:121
mrpt::RAD2DEG
constexpr double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:56
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:87
mrpt::poses::CPose3D::asTPose
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:759
mrpt::img::CH_GRAY
@ CH_GRAY
Definition: img/CImage.h:61
CActionRobotMovement3D.h
iniFile
string iniFile(myDataDir+string("benchmark-options.ini"))
mrpt::maps::CMultiMetricMapPDF::getCurrentJointEntropy
double getCurrentJointEntropy()
Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Explora...
Definition: CMultiMetricMapPDF.cpp:473
mrpt::typemeta::TEnumType
A helper class that can convert an enum value into its textual representation, and viceversa.
Definition: config/CConfigFileBase.h:24
mrpt::maps::CSimpleMap::size
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:53
mrpt::img::TColor
A RGB color - 8bit.
Definition: TColor.h:26
round.h
mrpt::maps::CMultiMetricMapPDF::getEstimatedPosePDF
void getEstimatedPosePDF(mrpt::poses::CPose3DPDFParticles &out_estimation) const
Returns the current estimate of the robot pose, as a particles PDF.
Definition: CMultiMetricMapPDF.cpp:167
mrpt::poses::CPose3DPDFParticles::getCovarianceAndMean
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once.
Definition: CPose3DPDFParticles.cpp:70
mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CMetricMapBuilderRBPF.cpp:517
mrpt::slam::CMetricMapBuilderRBPF::getCurrentJointEntropy
double getCurrentJointEntropy()
Definition: CMetricMapBuilderRBPF.cpp:486
mrpt::obs::CActionRobotMovement3D::estimationMethod
TEstimationMethod estimationMethod
This fields indicates the way this estimation was obtained.
Definition: CActionRobotMovement3D.h:53
MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT
#define MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
Definition: config/CConfigFileBase.h:382
mrpt::poses::CPose3DPDFParticles
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
Definition: CPose3DPDFParticles.h:38
mrpt::math::CProbabilityDensityFunction::getMeanVal
type_value getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
Definition: CProbabilityDensityFunction.h:77
mrpt::slam::CMetricMapBuilderRBPF::getCurrentlyBuiltMapSize
unsigned int getCurrentlyBuiltMapSize() override
Returns just how many sensory-frames are stored in the currently build map.
Definition: CMetricMapBuilderRBPF.cpp:359
lock_helper.h
mrpt::slam::CMetricMapBuilderRBPF::localizeLinDistance
float localizeLinDistance
Distances (linear and angular) for updating the robot pose estimate (and particles weighs,...
Definition: CMetricMapBuilderRBPF.h:71
MRPT_LOG_WARN
#define MRPT_LOG_WARN(_STRING)
Definition: system/COutputLogger.h:431
mrpt::img::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:149
mrpt::img::CImage::saveToFile
bool saveToFile(const std::string &fileName, int jpeg_quality=95) const
Save the image to a file, whose format is determined from the extension (internally uses OpenCV).
Definition: CImage.cpp:329
mrpt::rtti::CListOfClasses::data
TSet data
Definition: CListOfClasses.h:26
mrpt::slam::CMetricMapBuilderRBPF::insertionLinDistance
float insertionLinDistance
Distances (linear and angular) for inserting a new observation into the map.
Definition: CMetricMapBuilderRBPF.h:67
mrpt::poses::CPosePDF
Declares a class that represents a probability density function (pdf) of a 2D pose (x,...
Definition: CPosePDF.h:40
mrpt::obs::CActionRobotMovement3D::poseChange
mrpt::poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
Definition: CActionRobotMovement3D.h:46
mrpt::img::CCanvas::drawImage
virtual void drawImage(int x, int y, const mrpt::img::CImage &img)
Draws an image as a bitmap at a given position.
Definition: CCanvas.cpp:255
mrpt::slam::CMetricMapBuilderRBPF::getCurrentlyBuiltMetricMap
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const override
Returns the map built so far.
Definition: CMetricMapBuilderRBPF.cpp:351
MRPT_LOG_DEBUG_STREAM
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
Definition: system/COutputLogger.h:471
mrpt::bayes::CParticleFilterData::m_particles
CParticleList m_particles
The array of particles.
Definition: CParticleFilterData.h:196
mrpt::slam::CMetricMapBuilderRBPF::drawCurrentEstimationToImage
void drawCurrentEstimationToImage(mrpt::img::CCanvas *img)
A useful method for debugging: draws the current map and path hypotheses to a CCanvas
Definition: CMetricMapBuilderRBPF.cpp:364
mrpt::slam::CMetricMapBuilderRBPF::saveCurrentEstimationToImage
void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP=true) override
A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.
Definition: CMetricMapBuilderRBPF.cpp:461
mrpt::slam::CMetricMapBuilderRBPF::initialize
void initialize(const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), const mrpt::poses::CPosePDF *x0=nullptr) override
Initialize the method, starting with a known location PDF "x0"(if supplied, set to nullptr to left un...
Definition: CMetricMapBuilderRBPF.cpp:275
mrpt::slam::CMetricMapBuilderRBPF::getCurrentPoseEstimation
mrpt::poses::CPose3DPDF::Ptr getCurrentPoseEstimation() const override
Returns a copy of the current best pose estimation as a pose PDF.
Definition: CMetricMapBuilderRBPF.cpp:305
mrpt::img::CImage::getHeight
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:855
mrpt::lockHelper
LockHelper< T > lockHelper(T &t)
Syntactic sugar to easily create a locker to any kind of std::mutex.
Definition: lock_helper.h:50
slam-precomp.h
mrpt::bayes
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
Definition: CKalmanFilterCapable.h:31
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
mrpt::math::MatrixVectorBase::setZero
void setZero()
Definition: MatrixVectorBase.h:112
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::obs::CActionCollection::insert
void insert(CAction &action)
Add a new object to the list.
Definition: CActionCollection.cpp:85
mrpt::slam::CMetricMapBuilder::critZoneChangingMap
std::mutex critZoneChangingMap
Critical zones.
Definition: CMetricMapBuilder.h:36
mrpt::maps::CSimpleMap
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:33
mrpt::slam::CMetricMapBuilderRBPF::localizeAngDistance
float localizeAngDistance
Definition: CMetricMapBuilderRBPF.h:71
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:12
mrpt::slam::CMetricMapBuilderRBPF::processActionObservation
void processActionObservation(mrpt::obs::CActionCollection &action, mrpt::obs::CSensoryFrame &observations) override
Appends a new action and observations to update this map: See the description of the class at the top...
Definition: CMetricMapBuilderRBPF.cpp:103
mrpt::poses::CPose3DPDF::Ptr
std::shared_ptr< CPose3DPDF > Ptr
Definition: CPose3DPDF.h:42
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: COccupancyGridMap2D.h:56
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:82
mrpt::poses::CPose3DPDFParticles::getMean
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF),...
Definition: CPose3DPDFParticles.cpp:52
MRPT_LOG_INFO_STREAM
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
Definition: system/COutputLogger.h:473
mrpt::slam::CMetricMapBuilderRBPF::clear
void clear()
Clear all elements of the maps.
Definition: CMetricMapBuilderRBPF.cpp:84
mrpt::system::COutputLogger::getMinLoggingLevel
VerbosityLevel getMinLoggingLevel() const
Definition: system/COutputLogger.h:201
mrpt::maps
Definition: CBeacon.h:22
mrpt::obs::CActionRobotMovement2D::computeFromOdometry
void computeFromOdometry(const mrpt::poses::CPose2D &odometryIncrement, const TMotionModelOptions &options)
Computes the PDF of the pose increment from an odometry reading and according to the given motion mod...
Definition: CActionRobotMovement2D.cpp:387
mrpt::maps::CMultiMetricMapPDF::updateSensoryFrameSequence
void updateSensoryFrameSequence()
Update the poses estimation of the member "SFs" according to the current path belief.
Definition: CMultiMetricMapPDF.cpp:545
mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions
Options for building a CMetricMapBuilderRBPF object, passed to the constructor.
Definition: CMetricMapBuilderRBPF.h:83
mrpt::system::COutputLogger::setLoggerName
void setLoggerName(const std::string &name)
Set the name of the COutputLogger instance.
Definition: COutputLogger.cpp:138
mrpt::obs::CAction::timestamp
mrpt::system::TTimeStamp timestamp
The associated time-stamp.
Definition: CAction.h:33
mrpt::slam
Definition: CMultiMetricMapPDF.h:27
mrpt::slam::CMetricMapBuilderRBPF::insertionAngDistance
float insertionAngDistance
Definition: CMetricMapBuilderRBPF.h:67
mrpt::img::CCanvas::line
virtual void line(int x0, int y0, int x1, int y1, const mrpt::img::TColor color, unsigned int width=1, TPenStyle penStyle=psSolid)
Draws a line.
Definition: CCanvas.cpp:125
mrpt::slam::CMetricMapBuilderRBPF::mapPDF
mrpt::maps::CMultiMetricMapPDF mapPDF
The map PDF: It includes a path and associated map for each particle.
Definition: CMetricMapBuilderRBPF.h:59
mrpt::poses::CPose3DPDFGaussian::mean
CPose3D mean
The mean value.
Definition: CPose3DPDFGaussian.h:78
mrpt::bayes::CParticleFilterDataImpl::ESS
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
Definition: CParticleFilterData.h:85
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::slam::CMetricMapBuilderRBPF::operator=
CMetricMapBuilderRBPF & operator=(const CMetricMapBuilderRBPF &src)
Copy Operator.
Definition: CMetricMapBuilderRBPF.cpp:58
mrpt::slam::CMetricMapBuilder::TOptions::alwaysInsertByClass
mrpt::rtti::CListOfClasses alwaysInsertByClass
A list of observation classes (derived from mrpt::obs::CObservation) which will be always inserted in...
Definition: CMetricMapBuilder.h:138
mrpt::slam::CMetricMapBuilderRBPF::getCurrentlyBuiltMap
void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const override
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
Definition: CMetricMapBuilderRBPF.cpp:344



Page generated by Doxygen 1.8.18 for MRPT 2.0.4 at Thu Sep 24 07:14:18 UTC 2020