Main MRPT website > C++ reference for MRPT 1.5.6
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;
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.
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
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.
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const
Returns the map built so far.
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
size_t particlesCount() const MRPT_OVERRIDE
Get the m_particles count.
TDATA getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
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.
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::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
virtual void line(int x0, int y0, int x1, int y1, const mrpt::utils::TColor color, unsigned int width=1, TPenStyle penStyle=psSolid)
Draws a line.
Definition: CCanvas.cpp:125
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...
Scalar * iterator
Definition: eigen_plugins.h:23
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
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...
T::Ptr getActionByClass(const size_t &ith=0) const
Access to the i'th action of a given class, or a NULL smart pointer if there is no action of that cla...
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: glew.h:7126
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.
mrpt::poses::CPose3DPDFPtr getCurrentPoseEstimation() const
Returns a copy of the current best pose estimation as a pose PDF.
const_reverse_iterator rbegin() const
TSet::const_iterator const_iterator
Declares a class for storing a collection of robot actions.
void clear()
Clear all elements of the maps.
This class allows loading and storing values and vectors of different types from a configuration text...
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
mrpt::synch::CCriticalSection critZoneChangingMap
Critical zones.
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...
GLint GLvoid * img
Definition: glew.h:1290
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:67
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.
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...
void getEstimatedPosePDF(mrpt::poses::CPose3DPDFParticles &out_estimation) const
Returns the current estimate of the robot pose, as a particles PDF.
#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
#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.
const_iterator end() const
Returns a constant iterator to the end of the list of observations: this is an example of usage: ...
#define MRPT_LOG_INFO(_STRING)
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:299
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...
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
A RGB color - 8bit.
Definition: TColor.h:26
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
GLhandleARB obj
Definition: glew.h:3276
void getPath(size_t i, std::deque< math::TPose3D > &out_path) const
Return the path (in absolute coordinate poses) for the i'th particle.
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
#define MRPT_LOG_DEBUG(_STRING)
This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilt...
#define DEG2RAD
GLfloat GLfloat p
Definition: glew.h:10113
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage: ...
size_t size() const
Returns the number of observations in the list.
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
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...
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
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.
GLsizei const GLcharARB ** string
Definition: glew.h:3293
bool empty() const
Returns size()!=0.
Definition: CSimpleMap.cpp:72
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...
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:174
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
#define CH_GRAY
Definition: CImage.h:41
void getCurrentMostLikelyPath(std::deque< mrpt::math::TPose3D > &outPath) const
Returns the current most-likely path estimation (the path associated to the most likely particle)...
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.
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...
bool insertObservation(mrpt::obs::CSensoryFrame &sf)
Insert an observation to the map, at each particle's pose and to each particle's metric map...
void drawCurrentEstimationToImage(utils::CCanvas *img)
A useful method for debugging: draws the current map and path hypotheses to a CCanvas.
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)
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.
const CMultiMetricMap * getCurrentMostLikelyMetricMap() const
Returns a pointer to the current most likely map (associated to the most likely particle) ...
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'th particle (logarithm) weight, where first one is index 0.
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092
void insert(CAction &action)
Add a new object to the list.
void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018