Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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::utils;
24 using namespace mrpt::poses;
25 using namespace mrpt::bayes;
26 
28 using namespace mrpt::utils::metaprogramming;
29 
30 /*---------------------------------------------------------------
31  Constructor
32  ---------------------------------------------------------------*/
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 
58 /*---------------------------------------------------------------
59  Copy operator
60  ---------------------------------------------------------------*/
62 {
63  if (this==&src) {
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 
84 }
85 
86 /*---------------------------------------------------------------
87  clear
88  ---------------------------------------------------------------*/
90 {
91  MRPT_LOG_DEBUG("CMetricMapBuilderRBPF::clear() called.");
92  static CPose2D nullPose(0,0,0);
93 
94  // Reset traveled distances counters:
96 
98 
99  // Clear maps for each particle:
100  mapPDF.clear( nullPose );
101 }
102 
103 /*---------------------------------------------------------------
104  processActionObservation
105  ---------------------------------------------------------------*/
107  CActionCollection &action,
108  CSensoryFrame &observations )
109 {
110  MRPT_START
111  mrpt::synch::CCriticalSectionLocker csl(&critZoneChangingMap); // Enter critical section (updating map)
112 
113  // Update the traveled distance estimations:
114  {
115  CActionRobotMovement3DPtr act3D = action.getActionByClass<CActionRobotMovement3D>();
116  CActionRobotMovement2DPtr act2D = action.getActionByClass<CActionRobotMovement2D>();
117  if (act3D)
118  {
119  MRPT_LOG_DEBUG("processActionObservation(): Input action is CActionRobotMovement3D");
120  odoIncrementSinceLastMapUpdate += act3D->poseChange.getMeanVal();
121  odoIncrementSinceLastLocalization += act3D->poseChange;
122  }
123  else if (act2D)
124  {
125  MRPT_LOG_DEBUG("processActionObservation(): Input action is CActionRobotMovement2D");
126  odoIncrementSinceLastMapUpdate += mrpt::poses::CPose3D(act2D->poseChange->getMeanVal());
127  odoIncrementSinceLastLocalization.mean += mrpt::poses::CPose3D(act2D->poseChange->getMeanVal());
128  }
129  else
130  {
131  MRPT_LOG_WARN("Action contains no odometry.\n");
132  }
133  }
134 
135  // Execute particle filter:
136  // But only if the traveled distance since the last update is long enough,
137  // or this is the first observation, etc...
138  // ----------------------------------------------------------------------------
139  bool do_localization = (
140  !mapPDF.SFs.size() || // This is the first observation!
144 
145  bool do_map_update = (
146  !mapPDF.SFs.size() || // This is the first observation!
150 
151  // Used any "options.alwaysInsertByClass" ??
153  for ( CSensoryFrame::iterator it=observations.begin();it!=observations.end();++it)
154  if ((*it)->GetRuntimeClass()==*itCl)
155  {
156  do_map_update = true;
157  do_localization = true;
158  break;
159  }
160 
161 
162  if (do_map_update)
163  do_localization = true;
164 
165  MRPT_LOG_DEBUG(mrpt::format("do_map_update=%s do_localization=%s",do_map_update ? "YES":"NO", do_localization ? "YES":"NO" ));
166 
167  if (do_localization)
168  {
169  // Create an artificial action object, since
170  // we've been collecting them until a threshold:
171  // ------------------------------------------------
172  CActionCollection fakeActs;
173  {
174  CActionRobotMovement3DPtr act3D = action.getActionByClass<CActionRobotMovement3D>();
175  if (act3D)
176  {
177  CActionRobotMovement3D newAct;
178  newAct.estimationMethod = act3D->estimationMethod;
180  newAct.timestamp = act3D->timestamp;
181  fakeActs.insert(newAct);
182  }
183  else
184  {
185  // It must be 2D odometry:
186  CActionRobotMovement2DPtr act2D = action.getActionByClass<CActionRobotMovement2D>();
187  ASSERT_(act2D)
188  CActionRobotMovement2D newAct;
189  newAct.computeFromOdometry( CPose2D(odoIncrementSinceLastLocalization.mean), act2D->motionModelConfiguration );
190  newAct.timestamp = act2D->timestamp;
191  fakeActs.insert(newAct);
192  }
193  }
194 
195  MRPT_LOG_DEBUG_STREAM( "odoIncrementSinceLastLocalization before resetting = " << odoIncrementSinceLastLocalization.mean);
196  // Reset distance counters:
199 
200  CParticleFilter pf;
201  pf.m_options = m_PF_options;
202  pf.setVerbosityLevel( this->getMinLoggingLevel() );
203 
204  pf.executeOn( mapPDF, &fakeActs, &observations );
205 
206  if (isLoggingLevelVisible(LVL_INFO))
207  {
208  // Get current pose estimation:
209  CPose3DPDFParticles poseEstimation;
210  CPose3D meanPose;
211  mapPDF.getEstimatedPosePDF(poseEstimation);
212  poseEstimation.getMean(meanPose);
213 
214  CPose3D estPos;
216  poseEstimation.getCovarianceAndMean(cov,estPos);
217 
218  MRPT_LOG_INFO_STREAM( "New pose=" << estPos << std::endl << "New ESS:"<< mapPDF.ESS() << std::endl);
219  MRPT_LOG_INFO( format(" STDs: x=%2.3f y=%2.3f z=%.03f yaw=%2.3fdeg\n", sqrt(cov(0,0)),sqrt(cov(1,1)),sqrt(cov(2,2)),RAD2DEG(sqrt(cov(3,3)))) );
220  }
221  }
222 
223  if (do_map_update)
224  {
226 
227  // Update the particles' maps:
228  // -------------------------------------------------
229  MRPT_LOG_INFO("New observation inserted into the map.");
230 
231  // Add current observation to the map:
232  const bool anymap_update = mapPDF.insertObservation(observations);
233  if (!anymap_update)
234  MRPT_LOG_WARN_STREAM( "**No map was updated** after inserting a CSensoryFrame with "<< observations.size());
235 
237  }
238  else
239  {
241  }
242 
243  // Added 29/JUN/2007 JLBC: Tell all maps that they can now free aux. variables
244  // (if any) since one PF cycle is over:
246  it->d->mapTillNow.auxParticleFilterCleanUp();
247 
248  MRPT_END;
249 }
250 
251 /*---------------------------------------------------------------
252  initialize
253  ---------------------------------------------------------------*/
255  const CSimpleMap &initialMap,
256  CPosePDF *x0 )
257 {
258  mrpt::synch::CCriticalSectionLocker csl(&critZoneChangingMap); // Enter critical section (updating map)
259 
260  MRPT_LOG_INFO_STREAM( "[initialize] Called with " << initialMap.size() << " nodes in fixed map");
261 
262  this->clear();
263  mrpt::poses::CPose3D curPose;
264  if (x0)
265  {
266  curPose = mrpt::poses::CPose3D(x0->getMeanVal());
267  }
268  else if (!initialMap.empty())
269  {
270  // get pose of last keyframe:
271  curPose = initialMap.rbegin()->first->getMeanVal();
272  }
273  MRPT_LOG_INFO_STREAM("[initialize] Initial pose: " << curPose);
274 
275  // Clear maps for each particle & set pose:
276  mapPDF.clear(initialMap,curPose);
277 
278 }
279 
280 
281 /*---------------------------------------------------------------
282  getCurrentPoseEstimation
283  ---------------------------------------------------------------*/
285 {
286  CPose3DPDFParticlesPtr posePDF = CPose3DPDFParticles::Create();
287  mapPDF.getEstimatedPosePDF(*posePDF);
288 
289  // Adds additional increment from accumulated odometry since last localization update:
290  for (auto &p : posePDF->m_particles)
291  {
292  (*p.d) = (*p.d) + this->odoIncrementSinceLastLocalization.mean;
293  }
294  return posePDF;
295 }
296 
297 /*---------------------------------------------------------------
298  getCurrentMostLikelyPath
299  ---------------------------------------------------------------*/
300 void CMetricMapBuilderRBPF::getCurrentMostLikelyPath( std::deque<TPose3D> &outPath ) const
301 {
302  double maxW = -1, w;
303  size_t mostLik=0;
304  for (size_t i=0;i<mapPDF.particlesCount();i++)
305  {
306  w = mapPDF.getW(i);
307  if (w>maxW)
308  {
309  maxW = w;
310  mostLik = i;
311  }
312  }
313 
314  mapPDF.getPath( mostLik, outPath );
315 }
316 
317 /*---------------------------------------------------------------
318  getCurrentlyBuiltMap
319  ---------------------------------------------------------------*/
321  CSimpleMap &out_map) const
322 {
324  out_map = mapPDF.SFs;
325 }
326 
328 {
330 }
331 
332 /*---------------------------------------------------------------
333  getCurrentlyBuiltMapSize
334  ---------------------------------------------------------------*/
336 {
337  return mapPDF.SFs.size();
338 }
339 
340 
341 /*---------------------------------------------------------------
342  drawCurrentEstimationToImage
343  ---------------------------------------------------------------*/
345 {
346  using mrpt::utils::round;
347 
348  unsigned int i, M = mapPDF.particlesCount();
349  std::deque<TPose3D> path;
350  unsigned int imgHeight=0;
351 
352  MRPT_START
353 
354  const mrpt::maps::CMultiMetricMap * currentMetricMapEstimation = mapPDF.getCurrentMostLikelyMetricMap( );
355 
356  ASSERT_( currentMetricMapEstimation->m_gridMaps.size()>0 );
357 
358 
359  // Find which is the most likely path index:
360  unsigned int bestPath = 0;
361  double bestPathLik = -1;
362  for (i=0;i<M;i++)
363  {
364  if (mapPDF.getW(i)>bestPathLik)
365  {
366  bestPathLik = mapPDF.getW(i);
367  bestPath = i;
368  }
369  }
370 
371  // Compute the length of the paths:
372  mapPDF.getPath(0, path);
373 
374  // Adapt the canvas size:
375  bool alreadyCopiedImage = false;
376  {
377  CImage *obj = dynamic_cast<CImage*>( img );
378  if (obj)
379  obj->resize(
380  currentMetricMapEstimation->m_gridMaps[0]->getSizeX(),
381  currentMetricMapEstimation->m_gridMaps[0]->getSizeY(),
382  1,
383  true);
384  }
385  if (!alreadyCopiedImage)
386  {
387  CImage imgGrid;
388 
389  // grid map as bitmap:
390  // ----------------------------------
391  currentMetricMapEstimation->m_gridMaps[0]->getAsImage( imgGrid );
392 
393  img->drawImage( 0,0, imgGrid );
394  imgHeight = imgGrid.getHeight();
395  }
396 
397  int x1=0,x2=0,y1=0,y2=0;
398  float x_min = currentMetricMapEstimation->m_gridMaps[0]->getXMin();
399  float y_min = currentMetricMapEstimation->m_gridMaps[0]->getYMin();
400  float resolution = currentMetricMapEstimation->m_gridMaps[0]->getResolution();
401 
402  // Paths hypothesis:
403  // ----------------------------------
404  /***/
405  for (i=0;i<=M;i++)
406  {
407  if (i!=bestPath || i==M)
408  {
409  mapPDF.getPath(i==M ? bestPath:i, path);
410 
411  size_t nPoses = path.size();
412 
413  // First point: (0,0)
414  x2 = round( ( path[0].x - x_min) / resolution);
415  y2 = round( ( path[0].y - y_min) / resolution );
416 
417  // Draw path in the bitmap:
418  for (size_t j=0;j<nPoses;j++)
419  {
420  // For next segment
421  x1 = x2;
422  y1 = y2;
423 
424  // Coordinates -> pixels
425  x2 = round( ( path[j].x - x_min) / resolution );
426  y2 = round( ( path[j].y - y_min) / resolution );
427 
428  // Draw line:
429  img->line(
430  x1, round( (imgHeight-1)-y1 ),
431  x2, round( (imgHeight-1)-y2 ),
432  i==M ? TColor(0,0,0) : TColor(0x50,0x50,0x50), // Color, gray levels,
433  i==M ? 3 : 1 // Line width
434  );
435  }
436  }
437  }
438 
439  MRPT_END
440 }
441 
442 /*---------------------------------------------------------------
443  saveCurrentEstimationToImage
444  ---------------------------------------------------------------*/
446 {
447  MRPT_START
448 
449  if (formatEMF_BMP)
450  {
451  // Draw paths (using vectorial plots!) over the EMF file:
452  // --------------------------------------------------------
453  CEnhancedMetaFile EMF( file, 100 /* Scale */ );
455  }
456  else
457  {
458  CImage img(1,1, CH_GRAY );
460  img.saveToFile(file);
461  }
462 
463  MRPT_END
464 }
465 
466 /*---------------------------------------------------------------
467  getCurrentJointEntropy
468  ---------------------------------------------------------------*/
470 {
472 }
473 
474 /*---------------------------------------------------------------
475  saveCurrentPathEstimationToTextFile
476  ---------------------------------------------------------------*/
478 {
480 }
481 
482 /*---------------------------------------------------------------
483  TConstructionOptions
484  ---------------------------------------------------------------*/
486  insertionLinDistance ( 1.0f ),
487  insertionAngDistance ( DEG2RAD(30) ),
488  localizeLinDistance ( 0.4f ),
489  localizeAngDistance ( DEG2RAD(10) ),
490  PF_options(),
491  mapsInitializers(),
492  predictionOptions(),
493  verbosity_level(mrpt::utils::LVL_INFO)
494 {
495 }
496 
497 /*---------------------------------------------------------------
498  dumpToTextStream
499  ---------------------------------------------------------------*/
501 {
502  out.printf("\n----------- [CMetricMapBuilderRBPF::TConstructionOptions] ------------ \n\n");
503 
504  out.printf("insertionLinDistance = %f m\n", insertionLinDistance );
505  out.printf("insertionAngDistance = %f deg\n", RAD2DEG(insertionAngDistance) );
506  out.printf("localizeLinDistance = %f m\n", localizeLinDistance );
507  out.printf("localizeAngDistance = %f deg\n", RAD2DEG(localizeAngDistance) );
508  out.printf("verbosity_level = %s\n", mrpt::utils::TEnumType<mrpt::utils::VerbosityLevel>::value2name(verbosity_level).c_str());
509 
510  PF_options.dumpToTextStream(out);
511 
512  out.printf(" Now showing 'mapsInitializers' and 'predictionOptions':\n");
513  out.printf("\n");
514 
515  mapsInitializers.dumpToTextStream(out);
516  predictionOptions.dumpToTextStream(out);
517 
518 }
519 
520 /*---------------------------------------------------------------
521  loadFromConfigFile
522  ---------------------------------------------------------------*/
524  const mrpt::utils::CConfigFileBase &iniFile,
525  const std::string &section)
526 {
527  MRPT_START
528 
529  PF_options.loadFromConfigFile(iniFile,section);
530 
531  MRPT_LOAD_CONFIG_VAR(insertionLinDistance, float, iniFile,section);
532  MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT(insertionAngDistance_deg, double, insertionAngDistance, iniFile,section);
533 
534  MRPT_LOAD_CONFIG_VAR(localizeLinDistance, float, iniFile,section);
535  MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT(localizeAngDistance_deg, double, localizeAngDistance, iniFile,section);
536  verbosity_level = iniFile.read_enum<mrpt::utils::VerbosityLevel>(section,"verbosity_level", verbosity_level );
537 
538  mapsInitializers.loadFromConfigFile(iniFile,section);
539  predictionOptions.loadFromConfigFile(iniFile,section);
540 
541  MRPT_END
542 }
void clear(const mrpt::poses::CPose2D &initialPose)
Clear all elements of the maps, and restore all paths to a single starting pose.
unsigned int getCurrentlyBuiltMapSize()
Returns just how many sensory-frames are stored in the currently build map.
mrpt::poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
size_t particlesCount() const MRPT_OVERRIDE
Get the m_particles count.
bool empty() const
Returns size()!=0.
Definition: CSimpleMap.cpp:72
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
CPose3D mean
The mean value.
void getMean(CPose3D &mean_pose) const MRPT_OVERRIDE
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
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.
double ESS() const MRPT_OVERRIDE
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
float insertionLinDistance
Distances (linear and angular) for inserting a new observation into the map.
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
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: CImage.h:209
float localizeLinDistance
Distances (linear and angular) for updating the robot pose estimate (and particles weighs...
TDATA getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
Scalar * iterator
Definition: eigen_plugins.h:23
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...
void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
const_reverse_iterator rbegin() const
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...
mrpt::poses::CPose3DPDFPtr getCurrentPoseEstimation() const
Returns a copy of the current best pose estimation as a pose PDF.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
GLuint src
Definition: glext.h:6303
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
TSet::const_iterator const_iterator
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:3962
void clear()
Clear all elements of the maps.
This class allows loading and storing values and vectors of different types from a configuration text...
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const
Returns the map built so far.
mrpt::synch::CCriticalSection critZoneChangingMap
Critical zones.
TEstimationMethod estimationMethod
This fields indicates the way this estimation was obtained.
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage: ...
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...
Represents a probabilistic 3D (6D) movement.
Represents a probabilistic 2D movement of the robot mobile base.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
CParticleList m_particles
The array of particles.
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
void executeOn(CParticleFilterCapable &obj, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, TParticleFilterStats *stats=NULL)
Executes a complete prediction + update step of the selected particle filtering algorithm.
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...
#define MRPT_END
mrpt::maps::CMultiMetricMapPDF mapPDF
The map PDF: It includes a path and associated map for each particle.
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:135
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:174
#define MRPT_LOG_WARN(_STRING)
A helper class that can convert an enum value into its textual representation, and viceversa...
std::deque< CObservationPtr >::iterator iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
GLint GLvoid * img
Definition: glext.h:3645
#define MRPT_LOG_INFO(_STRING)
mrpt::utils::CListOfClasses alwaysInsertByClass
A list of observation classes (derived from mrpt::obs::CObservation) which will be always inserted in...
CMetricMapBuilderRBPF()
This second constructor is created for the situation where a class member needs to be of type CMetric...
A RGB color - 8bit.
Definition: TColor.h:26
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
A set of utility objects for metaprogramming with STL algorithms.
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
#define MRPT_LOG_DEBUG(_STRING)
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:67
This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilt...
#define DEG2RAD
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei const GLchar ** string
Definition: glext.h:3919
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...
Definition: CPoint.h:17
void getCovarianceAndMean(mrpt::math::CMatrixDouble66 &cov, CPose3D &mean_point) const MRPT_OVERRIDE
Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once...
ENUMTYPE read_enum(const std::string &section, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
#define MRPT_START
void updateSensoryFrameSequence()
Update the poses estimation of the member "SFs" according to the current path belief.
#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...
This class represents a Windows Enhanced Meta File (EMF) for generating and saving graphics...
#define RAD2DEG
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::maps::CSimpleMap SFs
The SFs and their corresponding pose estimations.
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
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:36
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:72
double getCurrentJointEntropy()
Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Explora...
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:254
void getCurrentMostLikelyPath(std::deque< mrpt::math::TPose3D > &outPath) const
Returns the current most-likely path estimation (the path associated to the most likely particle)...
#define CH_GRAY
Definition: CImage.h:41
void initialize(const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), mrpt::poses::CPosePDF *x0=NULL)
Initialize the method, starting with a known location PDF "x0" (or set to NULL to use the last keyfra...
#define ASSERT_(f)
bool debugForceInsertion
Always insert into map. Default is false: detect if necesary.
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
#define MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT(variableName, variableType, targetVariable, configFileObject, sectionNameStr)
This virtual class defines the interface of any object accepting drawing primitives on it...
Definition: CCanvas.h:40
mrpt::poses::CPose3DPDFGaussian odoIncrementSinceLastLocalization
Traveled distance since last localization update.
GLenum GLint GLint y
Definition: glext.h:3516
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...
T::Ptr getActionByClass(const size_t &ith=0) const
Access to the i&#39;th action of a given class, or a NULL smart pointer if there is no action of that cla...
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...
void drawCurrentEstimationToImage(utils::CCanvas *img)
A useful method for debugging: draws the current map and path hypotheses to a CCanvas.
GLenum GLint x
Definition: glext.h:3516
const_iterator end() const
Returns a constant iterator to the end of the list of observations: this is an example of usage: ...
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...
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
size_t size() const
Returns the number of observations in the list.
GLfloat GLfloat p
Definition: glext.h:5587
Options for building a CMetricMapBuilderRBPF object, passed to the constructor.
virtual void drawImage(int x, int y, const utils::CImage &img)
Draws an image as a bitmap at a given position.
Definition: CCanvas.cpp:271
bayes::CParticleFilter::TParticleFilterOptions m_PF_options
The configuration of the particle filter.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
mrpt::system::TTimeStamp timestamp
The associated time-stamp.
Definition: obs/CAction.h:50
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
size_t getHeight() const MRPT_OVERRIDE
Returns the height of the image in pixels.
Definition: CImage.cpp:884
double getW(size_t i) const MRPT_OVERRIDE
Access to i&#39;th particle (logarithm) weight, where first one is index 0.
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.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019