MRPT  1.9.9
CMetricMapBuilderRBPF.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 
16 #include <mrpt/math/utils.h>
17 
18 using namespace mrpt;
19 using namespace mrpt::slam;
20 using namespace mrpt::math;
21 using namespace mrpt::maps;
22 using namespace mrpt::obs;
23 using namespace mrpt::poses;
24 using namespace mrpt::bayes;
25 using namespace mrpt::img;
26 
27 /*---------------------------------------------------------------
28  Constructor
29  ---------------------------------------------------------------*/
31  const TConstructionOptions& initializationOptions)
32  : mapPDF(
33  initializationOptions.PF_options,
34  &initializationOptions.mapsInitializers,
35  &initializationOptions.predictionOptions),
36  m_PF_options(initializationOptions.PF_options),
37  insertionLinDistance(initializationOptions.insertionLinDistance),
38  insertionAngDistance(initializationOptions.insertionAngDistance),
39  localizeLinDistance(initializationOptions.localizeLinDistance),
40  localizeAngDistance(initializationOptions.localizeAngDistance),
41  odoIncrementSinceLastLocalization(),
42  odoIncrementSinceLastMapUpdate()
43 {
44  setLoggerName("CMetricMapBuilderRBPF");
45  setVerbosityLevel(initializationOptions.verbosity_level);
46  // Reset:
47  clear();
48 }
49 
51 {
52  this->setLoggerName("CMetricMapBuilderRBPF");
53  MRPT_LOG_WARN("Empty constructor invoked!\n");
54 }
55 
56 /*---------------------------------------------------------------
57  Copy operator
58  ---------------------------------------------------------------*/
61 {
62  if (this == &src)
63  {
64  return *this;
65  }
66  mapPDF = src.mapPDF;
67  m_PF_options = src.m_PF_options;
68  insertionLinDistance = src.insertionLinDistance;
69  insertionAngDistance = src.insertionAngDistance;
70  localizeLinDistance = src.localizeLinDistance;
71  localizeAngDistance = src.localizeAngDistance;
72  odoIncrementSinceLastLocalization = src.odoIncrementSinceLastLocalization;
73  odoIncrementSinceLastMapUpdate = src.odoIncrementSinceLastMapUpdate;
74  m_statsLastIteration = src.m_statsLastIteration;
75  return *this;
76 }
77 
78 /*---------------------------------------------------------------
79  Destructor
80  ---------------------------------------------------------------*/
82 /*---------------------------------------------------------------
83  clear
84  ---------------------------------------------------------------*/
86 {
87  std::lock_guard<std::mutex> csl(
88  critZoneChangingMap); // Enter critical section (updating map)
89 
90  MRPT_LOG_DEBUG("CMetricMapBuilderRBPF::clear() called.");
91  static CPose2D nullPose(0, 0, 0);
92 
93  // Reset traveled distances counters:
95 
97 
98  // Clear maps for each particle:
99  mapPDF.clear(nullPose);
100 }
101 
102 /*---------------------------------------------------------------
103  processActionObservation
104  ---------------------------------------------------------------*/
106  CActionCollection& action, CSensoryFrame& observations)
107 {
108  MRPT_START
109  std::lock_guard<std::mutex> csl(
110  critZoneChangingMap); // Enter critical section (updating map)
111 
112  // Update the traveled distance estimations:
113  {
118  if (act3D)
119  {
121  "processActionObservation(): Input action is "
122  "CActionRobotMovement3D="
123  << act3D->poseChange.getMeanVal().asString());
124  odoIncrementSinceLastMapUpdate += act3D->poseChange.getMeanVal();
125  odoIncrementSinceLastLocalization += act3D->poseChange;
126  }
127  else if (act2D)
128  {
130  "processActionObservation(): Input action is "
131  "CActionRobotMovement2D="
132  << act2D->poseChange->getMeanVal().asString());
134  mrpt::poses::CPose3D(act2D->poseChange->getMeanVal());
136  mrpt::poses::CPose3D(act2D->poseChange->getMeanVal());
137  }
138  else
139  {
140  MRPT_LOG_WARN("Action contains no odometry.\n");
141  }
142  }
143 
144  // Execute particle filter:
145  // But only if the traveled distance since the last update is long enough,
146  // or this is the first observation, etc...
147  // ----------------------------------------------------------------------------
148  bool do_localization =
149  (!mapPDF.SFs.size() || // This is the first observation!
154 
155  bool do_map_update =
156  (!mapPDF.SFs.size() || // This is the first observation!
160 
161  // Used any "options.alwaysInsertByClass" ??
162  for (auto itCl = options.alwaysInsertByClass.data.begin();
163  !do_map_update && itCl != options.alwaysInsertByClass.data.end();
164  ++itCl)
165  for (auto& o : observations)
166  if (o->GetRuntimeClass() == *itCl)
167  {
168  do_map_update = true;
169  do_localization = true;
170  break;
171  }
172 
173  if (do_map_update) do_localization = true;
174 
176  mrpt::format(
177  "do_map_update=%s do_localization=%s", do_map_update ? "YES" : "NO",
178  do_localization ? "YES" : "NO"));
179 
180  if (do_localization)
181  {
182  // Create an artificial action object, since
183  // we've been collecting them until a threshold:
184  // ------------------------------------------------
185  CActionCollection fakeActs;
186  {
189  if (act3D)
190  {
191  CActionRobotMovement3D newAct;
192  newAct.estimationMethod = act3D->estimationMethod;
194  newAct.timestamp = act3D->timestamp;
195  fakeActs.insert(newAct);
196  }
197  else
198  {
199  // It must be 2D odometry:
202  ASSERT_(act2D);
203  CActionRobotMovement2D newAct;
204  newAct.computeFromOdometry(
206  act2D->motionModelConfiguration);
207  newAct.timestamp = act2D->timestamp;
208  fakeActs.insert(newAct);
209  }
210  }
211 
213  "odoIncrementSinceLastLocalization before resetting = "
215  // Reset distance counters:
218 
219  CParticleFilter pf;
220  pf.m_options = m_PF_options;
222 
223  pf.executeOn(mapPDF, &fakeActs, &observations);
224 
226  {
227  // Get current pose estimation:
228  CPose3DPDFParticles poseEstimation;
229  CPose3D meanPose;
230  mapPDF.getEstimatedPosePDF(poseEstimation);
231  poseEstimation.getMean(meanPose);
232 
233  CPose3D estPos;
235  poseEstimation.getCovarianceAndMean(cov, estPos);
236 
238  "New pose=" << estPos << std::endl
239  << "New ESS:" << mapPDF.ESS() << std::endl);
241  format(
242  " STDs: x=%2.3f y=%2.3f z=%.03f yaw=%2.3fdeg\n",
243  sqrt(cov(0, 0)), sqrt(cov(1, 1)), sqrt(cov(2, 2)),
244  RAD2DEG(sqrt(cov(3, 3)))));
245  }
246  }
247 
248  if (do_map_update)
249  {
251 
252  // Update the particles' maps:
253  // -------------------------------------------------
254  MRPT_LOG_INFO("New observation inserted into the map.");
255 
256  // Add current observation to the map:
257  const bool anymap_update = mapPDF.insertObservation(observations);
258  if (!anymap_update)
260  "**No map was updated** after inserting a CSensoryFrame with "
261  << observations.size());
262 
264  }
265  else
266  {
268  }
269 
270  // Added 29/JUN/2007 JLBC: Tell all maps that they can now free aux.
271  // variables
272  // (if any) since one PF cycle is over:
274  mapPDF.m_particles.begin();
275  it != mapPDF.m_particles.end(); ++it)
276  it->d->mapTillNow.auxParticleFilterCleanUp();
277 
278  MRPT_END;
279 }
280 
281 MRPT_TODO(
282  "Split initialize() in two, one generic without pose, one with "
283  "particles-based PDF");
284 
285 /*---------------------------------------------------------------
286  initialize
287  ---------------------------------------------------------------*/
289  const CSimpleMap& initialMap, const CPosePDF* x0)
290 {
292  "[initialize] Called with " << initialMap.size()
293  << " nodes in fixed map");
294 
295  this->clear();
296 
297  std::lock_guard<std::mutex> csl(
298  critZoneChangingMap); // Enter critical section (updating map)
299 
300  mrpt::poses::CPose3D curPose;
301  if (x0)
302  {
303  curPose = mrpt::poses::CPose3D(x0->getMeanVal());
304  }
305  else if (!initialMap.empty())
306  {
307  // get pose of last keyframe:
308  curPose = initialMap.rbegin()->first->getMeanVal();
309  }
310  MRPT_LOG_INFO_STREAM("[initialize] Initial pose: " << curPose);
311 
312  // Clear maps for each particle & set pose:
313  mapPDF.clear(initialMap, curPose);
314 }
315 
316 /*---------------------------------------------------------------
317  getCurrentPoseEstimation
318  ---------------------------------------------------------------*/
320 {
321  CPose3DPDFParticles::Ptr posePDF =
322  mrpt::make_aligned_shared<CPose3DPDFParticles>();
323  mapPDF.getEstimatedPosePDF(*posePDF);
324 
325  // Adds additional increment from accumulated odometry since last
326  // localization update:
327  for (auto& p : posePDF->m_particles)
328  {
329  p.d.composePose(
331  }
332  return posePDF;
333 }
334 
335 /*---------------------------------------------------------------
336  getCurrentMostLikelyPath
337  ---------------------------------------------------------------*/
339  std::deque<TPose3D>& outPath) const
340 {
341  double maxW = -1, w;
342  size_t mostLik = 0;
343  for (size_t i = 0; i < mapPDF.particlesCount(); i++)
344  {
345  w = mapPDF.getW(i);
346  if (w > maxW)
347  {
348  maxW = w;
349  mostLik = i;
350  }
351  }
352 
353  mapPDF.getPath(mostLik, outPath);
354 }
355 
356 /*---------------------------------------------------------------
357  getCurrentlyBuiltMap
358  ---------------------------------------------------------------*/
360 {
361  const_cast<CMetricMapBuilderRBPF*>(this)
363  out_map = mapPDF.SFs;
364 }
365 
367 {
369 }
370 
371 /*---------------------------------------------------------------
372  getCurrentlyBuiltMapSize
373  ---------------------------------------------------------------*/
375 {
376  return mapPDF.SFs.size();
377 }
378 
380 {
381  using mrpt::round;
382 
383  unsigned int i, M = mapPDF.particlesCount();
384  std::deque<TPose3D> path;
385  unsigned int imgHeight = 0;
386 
387  MRPT_START
388 
389  const mrpt::maps::CMultiMetricMap* currentMetricMapEstimation =
391 
392  ASSERT_(currentMetricMapEstimation->m_gridMaps.size() > 0);
393 
394  // Find which is the most likely path index:
395  unsigned int bestPath = 0;
396  double bestPathLik = -1;
397  for (i = 0; i < M; i++)
398  {
399  if (mapPDF.getW(i) > bestPathLik)
400  {
401  bestPathLik = mapPDF.getW(i);
402  bestPath = i;
403  }
404  }
405 
406  // Compute the length of the paths:
407  mapPDF.getPath(0, path);
408 
409  // Adapt the canvas size:
410  bool alreadyCopiedImage = false;
411  {
412  CImage* obj = dynamic_cast<CImage*>(img);
413  if (obj)
414  obj->resize(
415  currentMetricMapEstimation->m_gridMaps[0]->getSizeX(),
416  currentMetricMapEstimation->m_gridMaps[0]->getSizeY(), 1, true);
417  }
418  if (!alreadyCopiedImage)
419  {
420  CImage imgGrid;
421 
422  // grid map as bitmap:
423  // ----------------------------------
424  currentMetricMapEstimation->m_gridMaps[0]->getAsImage(imgGrid);
425 
426  img->drawImage(0, 0, imgGrid);
427  imgHeight = imgGrid.getHeight();
428  }
429 
430  int x1 = 0, x2 = 0, y1 = 0, y2 = 0;
431  float x_min = currentMetricMapEstimation->m_gridMaps[0]->getXMin();
432  float y_min = currentMetricMapEstimation->m_gridMaps[0]->getYMin();
433  float resolution =
434  currentMetricMapEstimation->m_gridMaps[0]->getResolution();
435 
436  // Paths hypothesis:
437  // ----------------------------------
438  /***/
439  for (i = 0; i <= M; i++)
440  {
441  if (i != bestPath || i == M)
442  {
443  mapPDF.getPath(i == M ? bestPath : i, path);
444 
445  size_t nPoses = path.size();
446 
447  // First point: (0,0)
448  x2 = round((path[0].x - x_min) / resolution);
449  y2 = round((path[0].y - y_min) / resolution);
450 
451  // Draw path in the bitmap:
452  for (size_t j = 0; j < nPoses; j++)
453  {
454  // For next segment
455  x1 = x2;
456  y1 = y2;
457 
458  // Coordinates -> pixels
459  x2 = round((path[j].x - x_min) / resolution);
460  y2 = round((path[j].y - y_min) / resolution);
461 
462  // Draw line:
463  img->line(
464  x1, round((imgHeight - 1) - y1), x2,
465  round((imgHeight - 1) - y2),
466  i == M ? TColor(0, 0, 0)
467  : TColor(0x50, 0x50, 0x50), // Color, gray levels,
468  i == M ? 3 : 1 // Line width
469  );
470  }
471  }
472  }
473 
474  MRPT_END
475 }
476 
477 /*---------------------------------------------------------------
478  saveCurrentEstimationToImage
479  ---------------------------------------------------------------*/
481  const std::string& file, bool formatEMF_BMP)
482 {
483  MRPT_START
484 
485  if (formatEMF_BMP)
486  {
487  // Draw paths (using vectorial plots!) over the EMF file:
488  // --------------------------------------------------------
489  CEnhancedMetaFile EMF(file, 100 /* Scale */);
491  }
492  else
493  {
494  CImage img(1, 1, CH_GRAY);
496  img.saveToFile(file);
497  }
498 
499  MRPT_END
500 }
501 
502 /*---------------------------------------------------------------
503  getCurrentJointEntropy
504  ---------------------------------------------------------------*/
506 {
508 }
509 
510 /*---------------------------------------------------------------
511  saveCurrentPathEstimationToTextFile
512  ---------------------------------------------------------------*/
514  const std::string& fil)
515 {
517 }
518 
519 /*---------------------------------------------------------------
520  TConstructionOptions
521  ---------------------------------------------------------------*/
523  : insertionLinDistance(1.0f),
524  insertionAngDistance(DEG2RAD(30)),
525  localizeLinDistance(0.4f),
526  localizeAngDistance(DEG2RAD(10)),
527  PF_options(),
528  mapsInitializers(),
529  predictionOptions(),
530  verbosity_level(mrpt::system::LVL_INFO)
531 {
532 }
533 
534 /*---------------------------------------------------------------
535  dumpToTextStream
536  ---------------------------------------------------------------*/
538  std::ostream& out) const
539 {
540  out << mrpt::format(
541  "\n----------- [CMetricMapBuilderRBPF::TConstructionOptions] "
542  "------------ \n\n");
543 
544  out << mrpt::format(
545  "insertionLinDistance = %f m\n",
547  out << mrpt::format(
548  "insertionAngDistance = %f deg\n",
550  out << mrpt::format(
551  "localizeLinDistance = %f m\n",
553  out << mrpt::format(
554  "localizeAngDistance = %f deg\n",
556  out << mrpt::format(
557  "verbosity_level = %s\n",
559  verbosity_level)
560  .c_str());
561 
562  PF_options.dumpToTextStream(out);
563 
564  out << mrpt::format(
565  " Now showing 'mapsInitializers' and 'predictionOptions':\n");
566  out << mrpt::format("\n");
567 
568  mapsInitializers.dumpToTextStream(out);
569  predictionOptions.dumpToTextStream(out);
570 }
571 
572 /*---------------------------------------------------------------
573  loadFromConfigFile
574  ---------------------------------------------------------------*/
576  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
577 {
578  MRPT_START
579 
580  PF_options.loadFromConfigFile(iniFile, section);
581 
584  insertionAngDistance_deg, double, insertionAngDistance, iniFile,
585  section);
586 
589  localizeAngDistance_deg, double, localizeAngDistance, iniFile, section);
590  verbosity_level = iniFile.read_enum<mrpt::system::VerbosityLevel>(
591  section, "verbosity_level", verbosity_level);
592 
593  mapsInitializers.loadFromConfigFile(iniFile, section);
594  predictionOptions.loadFromConfigFile(iniFile, section);
595 
596  MRPT_END
597 }
bool isLoggingLevelVisible(VerbosityLevel level) const
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:1043
void clear(const mrpt::poses::CPose2D &initialPose)
Clear all elements of the maps, and restore all paths to a single starting pose.
This class represents a Windows Enhanced Meta File (EMF) for generating and saving graphics...
unsigned int getCurrentlyBuiltMapSize()
Returns just how many sensory-frames are stored in the currently build map.
Scalar * iterator
Definition: eigen_plugins.h:26
mrpt::poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
bool empty() const
Returns size()!=0.
Definition: CSimpleMap.cpp:54
#define MRPT_START
Definition: exceptions.h:262
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
CPose3D mean
The mean value.
This virtual class defines the interface of any object accepting drawing primitives on it...
Definition: CCanvas.h:40
void getCovarianceAndMean(mrpt::math::CMatrixDouble66 &cov, CPose3D &mean_point) const override
Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once...
VerbosityLevel
Enumeration of available verbosity levels.
double RAD2DEG(const double x)
Radians to degrees.
bool observationsInserted
Whether the SF has been inserted in the metric maps.
CMetricMapBuilderRBPF & operator=(const CMetricMapBuilderRBPF &src)
Copy Operator.
const CMultiMetricMap * getCurrentMostLikelyMetricMap() const
Returns a pointer to the current most likely map (associated to the most likely particle) ...
mrpt::poses::CPose3D odoIncrementSinceLastMapUpdate
Traveled distance since last map update.
void resize(unsigned int width, unsigned int height, TImageChannels nChannels, bool originTopLeft)
Changes the size of the image, erasing previous contents (does NOT scale its current content...
Definition: img/CImage.h:261
#define MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:33
CParticleList m_particles
The array of particles.
float insertionLinDistance
Distances (linear and angular) for inserting a new observation into the map.
double DEG2RAD(const double x)
Degrees to radians.
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
float localizeLinDistance
Distances (linear and angular) for updating the robot pose estimate (and particles weighs...
mrpt::rtti::CListOfClasses alwaysInsertByClass
A list of observation classes (derived from mrpt::obs::CObservation) which will be always inserted in...
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...
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
MRPT_TODO("Split initialize() in two, one generic without pose, one with " "particles-based PDF")
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:892
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:526
const_reverse_iterator rbegin() const
Definition: CSimpleMap.h:190
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...
TDATA getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
GLuint src
Definition: glext.h:7278
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
Declares a class for storing a collection of robot actions.
void getEstimatedPosePDF(mrpt::poses::CPose3DPDFParticles &out_estimation) const
Returns the current estimate of the robot pose, as a particles PDF.
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
size_t particlesCount() const override
Get the m_particles count.
void clear()
Clear all elements of the maps.
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const
Returns the map built so far.
TEstimationMethod estimationMethod
This fields indicates the way this estimation was obtained.
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...
VerbosityLevel getMinLoggingLevel() const
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
Represents a probabilistic 3D (6D) movement.
Represents a probabilistic 2D movement of the robot mobile base.
A numeric matrix of compile-time fixed size.
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 saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP=true)
A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file...
mrpt::maps::CMultiMetricMapPDF mapPDF
The map PDF: It includes a path and associated map for each particle.
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:253
A helper class that can convert an enum value into its textual representation, and viceversa...
GLint GLvoid * img
Definition: glext.h:3763
mrpt::poses::CPose3DPDF::Ptr getCurrentPoseEstimation() const
Returns a copy of the current best pose estimation as a pose PDF.
void setLoggerName(const std::string &name)
Set the name of the COutputLogger instance.
CMetricMapBuilderRBPF()
This second constructor is created for the situation where a class member needs to be of type CMetric...
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:52
void initialize(const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), const mrpt::poses::CPosePDF *x0=nullptr)
Initialize the method, starting with a known location PDF "x0"(if supplied, set to nullptr to left un...
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:53
This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilt...
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
GLsizei const GLchar ** string
Definition: glext.h:4101
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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.
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
#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...
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
void setVerbosityLevel(const VerbosityLevel level)
alias of setMinLoggingLevel()
void updateSensoryFrameSequence()
Update the poses estimation of the member "SFs" according to the current path belief.
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 is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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.
double getW(size_t i) const override
Access to i&#39;th particle (logarithm) weight, where first one is index 0.
mrpt::maps::CSimpleMap SFs
The SFs and their corresponding pose estimations.
TStats m_statsLastIteration
This structure will hold stats after each execution of processActionObservation.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
This class implements a Rao-Blackwelized Particle Filter (RBPF) approach to map building (SLAM)...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
double getCurrentJointEntropy()
Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Explora...
#define MRPT_END
Definition: exceptions.h:266
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:239
void getCurrentMostLikelyPath(std::deque< mrpt::math::TPose3D > &outPath) const
Returns the current most-likely path estimation (the path associated to the most likely particle)...
bool debugForceInsertion
Always insert into map.
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
mrpt::poses::CPose3DPDFGaussian odoIncrementSinceLastLocalization
Traveled distance since last localization update.
GLenum GLint GLint y
Definition: glext.h:3538
#define CH_GRAY
Definition: img/CImage.h:44
#define MRPT_LOG_WARN(_STRING)
void processActionObservation(mrpt::obs::CActionCollection &action, mrpt::obs::CSensoryFrame &observations)
Appends a new action and observations to update this map: See the description of the class at the top...
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > 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:148
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:254
T::Ptr getActionByClass(const size_t &ith=0) const
Access to the i&#39;th action of a given class, or a nullptr smart pointer if there is no action of that ...
bool insertObservation(mrpt::obs::CSensoryFrame &sf)
Insert an observation to the map, at each particle&#39;s pose and to each particle&#39;s metric map...
A RGB color - 8bit.
Definition: TColor.h:20
GLenum GLint x
Definition: glext.h:3538
This class stores any customizable set of metric maps.
CParticleFilter::TParticleFilterOptions m_options
The options to be used in the PF, must be set before executing any step of the particle filter...
GLfloat GLfloat p
Definition: glext.h:6305
Options for building a CMetricMapBuilderRBPF object, passed to the constructor.
std::mutex critZoneChangingMap
Critical zones.
bayes::CParticleFilter::TParticleFilterOptions m_PF_options
The configuration of the particle filter.
void drawCurrentEstimationToImage(mrpt::img::CCanvas *img)
A useful method for debugging: draws the current map and path hypotheses to a CCanvas.
mrpt::system::TTimeStamp timestamp
The associated time-stamp.
Definition: CAction.h:36
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:130
#define MRPT_LOG_INFO(_STRING)
void getPath(size_t i, std::deque< math::TPose3D > &out_path) const
Return the path (in absolute coordinate poses) for the i&#39;th particle.
void insert(CAction &action)
Add a new object to the list.
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020