MRPT  2.0.2
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 }
bool isLoggingLevelVisible(VerbosityLevel level) const
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:759
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.
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
#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 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].
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:849
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...
#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.
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.
LockHelper< T > lockHelper(T &t)
Syntactic sugar to easily create a locker to any kind of std::mutex.
Definition: lock_helper.h:50
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:247
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...
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.
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)...
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.
#define MRPT_LOG_WARN(_STRING)
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
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:330
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:25
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.
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:148
#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 2.0.2 Git: 9efc2a654 Mon Apr 6 11:24:47 2020 +0200 at lun abr 6 11:30:12 CEST 2020