MRPT  1.9.9
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-2019, 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/round.h>
18 
19 using namespace mrpt;
20 using namespace mrpt::slam;
21 using namespace mrpt::math;
22 using namespace mrpt::maps;
23 using namespace mrpt::obs;
24 using namespace mrpt::poses;
25 using namespace mrpt::bayes;
26 using namespace mrpt::img;
27 
28 /*---------------------------------------------------------------
29  Constructor
30  ---------------------------------------------------------------*/
32  const TConstructionOptions& initializationOptions)
33  : mapPDF(
34  initializationOptions.PF_options,
35  initializationOptions.mapsInitializers,
36  initializationOptions.predictionOptions),
37  m_PF_options(initializationOptions.PF_options),
38  insertionLinDistance(initializationOptions.insertionLinDistance),
39  insertionAngDistance(initializationOptions.insertionAngDistance),
40  localizeLinDistance(initializationOptions.localizeLinDistance),
41  localizeAngDistance(initializationOptions.localizeAngDistance),
42  odoIncrementSinceLastLocalization(),
43  odoIncrementSinceLastMapUpdate()
44 {
45  setLoggerName("CMetricMapBuilderRBPF");
46  setVerbosityLevel(initializationOptions.verbosity_level);
47  // Reset:
48  clear();
49 }
50 
52 {
53  this->setLoggerName("CMetricMapBuilderRBPF");
54  MRPT_LOG_WARN("Empty constructor invoked!\n");
55 }
56 
59 {
60  if (this == &src)
61  {
62  return *this;
63  }
64  mapPDF = src.mapPDF;
65  m_PF_options = src.m_PF_options;
66  insertionLinDistance = src.insertionLinDistance;
67  insertionAngDistance = src.insertionAngDistance;
68  localizeLinDistance = src.localizeLinDistance;
69  localizeAngDistance = src.localizeAngDistance;
70  odoIncrementSinceLastLocalization = src.odoIncrementSinceLastLocalization;
71  odoIncrementSinceLastMapUpdate = src.odoIncrementSinceLastMapUpdate;
72  m_statsLastIteration = src.m_statsLastIteration;
73  return *this;
74 }
75 
76 /*---------------------------------------------------------------
77  Destructor
78  ---------------------------------------------------------------*/
80 /*---------------------------------------------------------------
81  clear
82  ---------------------------------------------------------------*/
84 {
85  std::lock_guard<std::mutex> csl(
86  critZoneChangingMap); // Enter critical section (updating map)
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
107  std::lock_guard<std::mutex> csl(
108  critZoneChangingMap); // Enter critical section (updating map)
109 
110  // Update the traveled distance estimations:
111  {
116  if (act3D)
117  {
119  "processActionObservation(): Input action is "
120  "CActionRobotMovement3D="
121  << act3D->poseChange.getMeanVal().asString());
122  odoIncrementSinceLastMapUpdate += act3D->poseChange.getMeanVal();
123  odoIncrementSinceLastLocalization += act3D->poseChange;
124  }
125  else if (act2D)
126  {
128  "processActionObservation(): Input action is "
129  "CActionRobotMovement2D="
130  << act2D->poseChange->getMeanVal().asString());
132  mrpt::poses::CPose3D(act2D->poseChange->getMeanVal());
134  mrpt::poses::CPose3D(act2D->poseChange->getMeanVal());
135  }
136  else
137  {
138  MRPT_LOG_WARN("Action contains no odometry.\n");
139  }
140  }
141 
142  // Execute particle filter:
143  // But only if the traveled distance since the last update is long enough,
144  // or this is the first observation, etc...
145  // ----------------------------------------------------------------------------
146  bool do_localization =
147  (!mapPDF.SFs.size() || // This is the first observation!
152 
153  bool do_map_update =
154  (!mapPDF.SFs.size() || // This is the first observation!
158 
159  // Used any "options.alwaysInsertByClass" ??
160  for (auto itCl = options.alwaysInsertByClass.data.begin();
161  !do_map_update && itCl != options.alwaysInsertByClass.data.end();
162  ++itCl)
163  for (auto& o : observations)
164  if (o->GetRuntimeClass() == *itCl)
165  {
166  do_map_update = true;
167  do_localization = true;
168  break;
169  }
170 
171  if (do_map_update) do_localization = true;
172 
174  "do_map_update=%s do_localization=%s", do_map_update ? "YES" : "NO",
175  do_localization ? "YES" : "NO"));
176 
177  if (do_localization)
178  {
179  // Create an artificial action object, since
180  // we've been collecting them until a threshold:
181  // ------------------------------------------------
182  CActionCollection fakeActs;
183  {
186  if (act3D)
187  {
188  CActionRobotMovement3D newAct;
189  newAct.estimationMethod = act3D->estimationMethod;
191  newAct.timestamp = act3D->timestamp;
192  fakeActs.insert(newAct);
193  }
194  else
195  {
196  // It must be 2D odometry:
199  ASSERT_(act2D);
200  CActionRobotMovement2D newAct;
201  newAct.computeFromOdometry(
203  act2D->motionModelConfiguration);
204  newAct.timestamp = act2D->timestamp;
205  fakeActs.insert(newAct);
206  }
207  }
208 
210  "odoIncrementSinceLastLocalization before resetting = "
212  // Reset distance counters:
215 
216  CParticleFilter pf;
217  pf.m_options = m_PF_options;
219 
220  pf.executeOn(mapPDF, &fakeActs, &observations);
221 
223  {
224  // Get current pose estimation:
225  CPose3DPDFParticles poseEstimation;
226  CPose3D meanPose;
227  mapPDF.getEstimatedPosePDF(poseEstimation);
228  poseEstimation.getMean(meanPose);
229 
230  const auto [cov, estPos] = poseEstimation.getCovarianceAndMean();
231 
233  "New pose=" << estPos << std::endl
234  << "New ESS:" << mapPDF.ESS() << std::endl);
236  " STDs: x=%2.3f y=%2.3f z=%.03f yaw=%2.3fdeg\n",
237  sqrt(cov(0, 0)), sqrt(cov(1, 1)), sqrt(cov(2, 2)),
238  RAD2DEG(sqrt(cov(3, 3)))));
239  }
240  }
241 
242  if (do_map_update)
243  {
245 
246  // Update the particles' maps:
247  // -------------------------------------------------
248  MRPT_LOG_INFO("New observation inserted into the map.");
249 
250  // Add current observation to the map:
251  const bool anymap_update = mapPDF.insertObservation(observations);
252  if (!anymap_update)
254  "**No map was updated** after inserting a CSensoryFrame with "
255  << observations.size());
256 
258  }
259  else
260  {
262  }
263 
264  // Added 29/JUN/2007 JLBC: Tell all maps that they can now free aux.
265  // variables
266  // (if any) since one PF cycle is over:
267  for (auto& m_particle : mapPDF.m_particles)
268  m_particle.d->mapTillNow.auxParticleFilterCleanUp();
269 
270  MRPT_END;
271 }
272 
273 MRPT_TODO(
274  "Split initialize() in two, one generic without pose, one with "
275  "particles-based PDF");
276 
277 /*---------------------------------------------------------------
278  initialize
279  ---------------------------------------------------------------*/
281  const CSimpleMap& initialMap, const CPosePDF* x0)
282 {
284  "[initialize] Called with " << initialMap.size()
285  << " nodes in fixed map");
286 
287  this->clear();
288 
289  std::lock_guard<std::mutex> csl(
290  critZoneChangingMap); // Enter critical section (updating map)
291 
292  mrpt::poses::CPose3D curPose;
293  if (x0)
294  {
295  curPose = mrpt::poses::CPose3D(x0->getMeanVal());
296  }
297  else if (!initialMap.empty())
298  {
299  // get pose of last keyframe:
300  curPose = initialMap.rbegin()->first->getMeanVal();
301  }
302  MRPT_LOG_INFO_STREAM("[initialize] Initial pose: " << curPose);
303 
304  // Clear maps for each particle & set pose:
305  mapPDF.clear(initialMap, curPose);
306 }
307 
308 /*---------------------------------------------------------------
309  getCurrentPoseEstimation
310  ---------------------------------------------------------------*/
312 {
313  CPose3DPDFParticles::Ptr posePDF = std::make_shared<CPose3DPDFParticles>();
314  mapPDF.getEstimatedPosePDF(*posePDF);
315 
316  // Adds additional increment from accumulated odometry since last
317  // localization update:
318  for (auto& p : posePDF->m_particles)
319  {
320  p.d.composePose(
322  }
323  return posePDF;
324 }
325 
326 /*---------------------------------------------------------------
327  getCurrentMostLikelyPath
328  ---------------------------------------------------------------*/
330  std::deque<TPose3D>& outPath) const
331 {
332  double maxW = -1, w;
333  size_t mostLik = 0;
334  for (size_t i = 0; i < mapPDF.particlesCount(); i++)
335  {
336  w = mapPDF.getW(i);
337  if (w > maxW)
338  {
339  maxW = w;
340  mostLik = i;
341  }
342  }
343 
344  mapPDF.getPath(mostLik, outPath);
345 }
346 
347 /*---------------------------------------------------------------
348  getCurrentlyBuiltMap
349  ---------------------------------------------------------------*/
351 {
352  const_cast<CMetricMapBuilderRBPF*>(this)
354  out_map = mapPDF.SFs;
355 }
356 
358 {
360 }
361 
362 /*---------------------------------------------------------------
363  getCurrentlyBuiltMapSize
364  ---------------------------------------------------------------*/
366 {
367  return mapPDF.SFs.size();
368 }
369 
371 {
372  using mrpt::round;
373 
374  unsigned int i, M = mapPDF.particlesCount();
375  std::deque<TPose3D> path;
376  unsigned int imgHeight = 0;
377 
378  MRPT_START
379 
380  const auto* curMap = mapPDF.getCurrentMostLikelyMetricMap();
381 
382  ASSERT_(curMap->countMapsByClass<COccupancyGridMap2D>() > 0);
383 
384  // Find which is the most likely path index:
385  unsigned int bestPath = 0;
386  double bestPathLik = -1;
387  for (i = 0; i < M; i++)
388  {
389  if (mapPDF.getW(i) > bestPathLik)
390  {
391  bestPathLik = mapPDF.getW(i);
392  bestPath = i;
393  }
394  }
395 
396  // Compute the length of the paths:
397  mapPDF.getPath(0, path);
398 
399  // Adapt the canvas size:
400  bool alreadyCopiedImage = false;
401  auto g = curMap->mapByClass<COccupancyGridMap2D>(0);
402  {
403  auto* obj = dynamic_cast<CImage*>(img);
404  if (obj) obj->resize(g->getSizeX(), g->getSizeY(), mrpt::img::CH_GRAY);
405  }
406  if (!alreadyCopiedImage)
407  {
408  CImage imgGrid;
409 
410  // grid map as bitmap:
411  // ----------------------------------
412  g->getAsImage(imgGrid);
413 
414  img->drawImage(0, 0, imgGrid);
415  imgHeight = imgGrid.getHeight();
416  }
417 
418  int x1 = 0, x2 = 0, y1 = 0, y2 = 0;
419  float x_min = g->getXMin();
420  float y_min = g->getYMin();
421  float resolution = g->getResolution();
422 
423  // Paths hypothesis:
424  // ----------------------------------
425  /***/
426  for (i = 0; i <= M; i++)
427  {
428  if (i != bestPath || i == M)
429  {
430  mapPDF.getPath(i == M ? bestPath : i, path);
431 
432  size_t nPoses = path.size();
433 
434  // First point: (0,0)
435  x2 = round((path[0].x - x_min) / resolution);
436  y2 = round((path[0].y - y_min) / resolution);
437 
438  // Draw path in the bitmap:
439  for (size_t j = 0; j < nPoses; j++)
440  {
441  // For next segment
442  x1 = x2;
443  y1 = y2;
444 
445  // Coordinates -> pixels
446  x2 = round((path[j].x - x_min) / resolution);
447  y2 = round((path[j].y - y_min) / resolution);
448 
449  // Draw line:
450  img->line(
451  x1, round((imgHeight - 1) - y1), x2,
452  round((imgHeight - 1) - y2),
453  i == M ? TColor(0, 0, 0)
454  : TColor(0x50, 0x50, 0x50), // Color, gray levels,
455  i == M ? 3 : 1 // Line width
456  );
457  }
458  }
459  }
460 
461  MRPT_END
462 }
463 
464 /*---------------------------------------------------------------
465  saveCurrentEstimationToImage
466  ---------------------------------------------------------------*/
468  const std::string& file, bool formatEMF_BMP)
469 {
470  MRPT_START
471 
472  if (formatEMF_BMP)
473  {
474  // Draw paths (using vectorial plots!) over the EMF file:
475  // --------------------------------------------------------
476  CEnhancedMetaFile EMF(file, 100 /* Scale */);
478  }
479  else
480  {
481  CImage img(1, 1, CH_GRAY);
483  img.saveToFile(file);
484  }
485 
486  MRPT_END
487 }
488 
489 /*---------------------------------------------------------------
490  getCurrentJointEntropy
491  ---------------------------------------------------------------*/
493 {
495 }
496 
497 /*---------------------------------------------------------------
498  saveCurrentPathEstimationToTextFile
499  ---------------------------------------------------------------*/
501  const std::string& fil)
502 {
504 }
505 
506 /*---------------------------------------------------------------
507  TConstructionOptions
508  ---------------------------------------------------------------*/
510  : insertionAngDistance(30.0_deg),
511 
512  localizeAngDistance(10.0_deg),
513  PF_options(),
514  mapsInitializers(),
515  predictionOptions()
516 
517 {
518 }
519 
520 /*---------------------------------------------------------------
521  dumpToTextStream
522  ---------------------------------------------------------------*/
524  std::ostream& out) const
525 {
526  out << "\n----------- [CMetricMapBuilderRBPF::TConstructionOptions] "
527  "------------ \n\n";
528 
529  out << mrpt::format(
530  "insertionLinDistance = %f m\n",
532  out << mrpt::format(
533  "insertionAngDistance = %f deg\n",
535  out << mrpt::format(
536  "localizeLinDistance = %f m\n",
538  out << mrpt::format(
539  "localizeAngDistance = %f deg\n",
541  out << mrpt::format(
542  "verbosity_level = %s\n",
544  verbosity_level)
545  .c_str());
546 
547  PF_options.dumpToTextStream(out);
548 
549  out << " Now showing 'mapsInitializers' and 'predictionOptions':\n";
550  out << "\n";
551 
552  mapsInitializers.dumpToTextStream(out);
553  predictionOptions.dumpToTextStream(out);
554 }
555 
556 /*---------------------------------------------------------------
557  loadFromConfigFile
558  ---------------------------------------------------------------*/
560  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
561 {
562  MRPT_START
563 
564  PF_options.loadFromConfigFile(iniFile, section);
565 
568  insertionAngDistance_deg, double, insertionAngDistance, iniFile,
569  section);
570 
573  localizeAngDistance_deg, double, localizeAngDistance, iniFile, section);
574  verbosity_level = iniFile.read_enum<mrpt::system::VerbosityLevel>(
575  section, "verbosity_level", verbosity_level);
576 
577  mapsInitializers.loadFromConfigFile(iniFile, section);
578  predictionOptions.loadFromConfigFile(iniFile, section);
579 
580  MRPT_END
581 }
bool isLoggingLevelVisible(VerbosityLevel level) const
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:775
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...
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:241
#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:41
VerbosityLevel
Enumeration of available verbosity levels.
bool observationsInserted
Whether the SF has been inserted in the metric maps.
CMetricMapBuilderRBPF & operator=(const CMetricMapBuilderRBPF &src)
Copy Operator.
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const override
Returns the map built so far.
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.
#define MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
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.
float insertionLinDistance
Distances (linear and angular) for inserting a new observation into the map.
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 drawImage(int x, int y, const mrpt::img::CImage &img) override
Draws an image as a bitmap at a given position.
Definition: CImage.cpp:1171
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].
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:878
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
const_reverse_iterator rbegin() const
Definition: CSimpleMap.h:189
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...
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...
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...
GLuint src
Definition: glext.h:7397
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
#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:4199
size_t particlesCount() const override
Get the m_particles count.
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...
void clear()
Clear all elements of the maps.
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:120
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.
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 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:254
mrpt::maps::CMultiMetricMapPDF mapPDF
The map PDF: It includes a path and associated map for each particle.
A helper class that can convert an enum value into its textual representation, and viceversa...
GLint GLvoid * img
Definition: glext.h:3769
void setLoggerName(const std::string &name)
Set the name of the COutputLogger instance.
~CMetricMapBuilderRBPF() override
Destructor.
CMetricMapBuilderRBPF()
This second constructor is created for the situation where a class member needs to be of type CMetric...
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.
GLubyte g
Definition: glext.h:6372
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
mrpt::poses::CPose3DPDF::Ptr getCurrentPoseEstimation() const override
Returns a copy of the current best pose estimation as a pose PDF.
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);
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
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:256
type_value getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
GLsizei const GLchar ** string
Definition: glext.h:4116
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...
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:38
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.
T::Ptr getActionByClass(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 ...
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()
A class for storing an occupancy grid map.
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:39
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:85
mrpt::vision::TStereoCalibResults out
double getCurrentJointEntropy()
Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Explora...
constexpr double RAD2DEG(const double x)
Radians to degrees.
#define MRPT_END
Definition: exceptions.h:245
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
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:3542
#define MRPT_LOG_WARN(_STRING)
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:3542
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...
unsigned int getCurrentlyBuiltMapSize() override
Returns just how many sensory-frames are stored in the currently build map.
GLfloat GLfloat p
Definition: glext.h:6398
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:33
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:147
#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:24



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce444d842 Fri Dec 6 19:35:10 2019 +0100 at vie dic 6 19:45:12 CET 2019