Main MRPT website > C++ reference for MRPT 1.5.9
CMetricMapBuilderICP.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 
12 #include <mrpt/slam/CICP.h>
18 #include <mrpt/utils/CTicTac.h>
20 
21 using namespace std;
22 using namespace mrpt::slam;
23 using namespace mrpt::obs;
24 using namespace mrpt::maps;
25 using namespace mrpt::utils;
26 using namespace mrpt::poses;
27 using namespace mrpt::math;
28 
29 /*---------------------------------------------------------------
30  Constructor
31  ---------------------------------------------------------------*/
32 CMetricMapBuilderICP::CMetricMapBuilderICP() :
33  ICP_options(m_min_verbosity_level)
34 {
35  this->setLoggerName("CMetricMapBuilderICP");
36  this->initialize( CSimpleMap() );
37 }
38 
39 /*---------------------------------------------------------------
40  Destructor
41  ---------------------------------------------------------------*/
43 {
44  // Asure, we have exit all critical zones:
47 
48  // Save current map to current file:
50 }
51 
52 
53 /*---------------------------------------------------------------
54  Options
55  ---------------------------------------------------------------*/
56 CMetricMapBuilderICP::TConfigParams::TConfigParams(mrpt::utils::VerbosityLevel &parent_verbosity_level) :
57  matchAgainstTheGrid( false ),
58  insertionLinDistance(1.0),
59  insertionAngDistance(DEG2RAD(30)),
60  localizationLinDistance(0.20),
61  localizationAngDistance(DEG2RAD(30)),
62  minICPgoodnessToAccept(0.40),
63  verbosity_level(parent_verbosity_level),
64  mapInitializers()
65 {
66 }
67 
69  matchAgainstTheGrid = other.matchAgainstTheGrid;
70  insertionLinDistance = other.insertionLinDistance;
71  insertionAngDistance = other.insertionAngDistance;
72  localizationLinDistance = other.localizationLinDistance;
73  localizationAngDistance = other.localizationAngDistance;
74  minICPgoodnessToAccept = other.minICPgoodnessToAccept;
75 // We can't copy a reference type
76 // verbosity_level = other.verbosity_level;
77  mapInitializers = other.mapInitializers;
78  return *this;
79 }
80 
83  const std::string &section)
84 {
85  MRPT_LOAD_CONFIG_VAR(matchAgainstTheGrid, bool ,source,section)
86  MRPT_LOAD_CONFIG_VAR(insertionLinDistance, double ,source,section)
87  MRPT_LOAD_CONFIG_VAR_DEGREES(insertionAngDistance,source,section)
88  MRPT_LOAD_CONFIG_VAR(localizationLinDistance, double ,source,section)
89  MRPT_LOAD_CONFIG_VAR_DEGREES(localizationAngDistance, source,section)
90  verbosity_level = source.read_enum<mrpt::utils::VerbosityLevel>(section,"verbosity_level", verbosity_level );
91 
92  MRPT_LOAD_CONFIG_VAR(minICPgoodnessToAccept, double ,source,section)
93 
94 
95  mapInitializers.loadFromConfigFile(source,section);
96 }
97 
99 {
100  out.printf("\n----------- [CMetricMapBuilderICP::TConfigParams] ------------ \n\n");
101 
102  out.printf("insertionLinDistance = %f m\n", insertionLinDistance );
103  out.printf("insertionAngDistance = %f deg\n", RAD2DEG(insertionAngDistance) );
104  out.printf("localizationLinDistance = %f m\n", localizationLinDistance );
105  out.printf("localizationAngDistance = %f deg\n", RAD2DEG(localizationAngDistance) );
106  out.printf("verbosity_level = %s\n", mrpt::utils::TEnumType<mrpt::utils::VerbosityLevel>::value2name(verbosity_level).c_str());
107 
108  out.printf(" Now showing 'mapsInitializers':\n");
109  mapInitializers.dumpToTextStream(out);
110 }
111 
112 /*---------------------------------------------------------------
113  processObservation
114  This is the new entry point of the algorithm (the old one
115  was processActionObservation, which now is a wrapper to
116  this method).
117  ---------------------------------------------------------------*/
118 void CMetricMapBuilderICP::processObservation(const CObservationPtr &obs)
119 {
121 
122  MRPT_START
123 
125  throw std::runtime_error("Neither grid maps nor points map: Have you called initialize() after setting ICP_options.mapInitializers?");
126 
127  ASSERT_(obs.present())
128 
129  // Is it an odometry observation??
131  {
132  MRPT_LOG_DEBUG("processObservation(): obs is CObservationOdometry");
134 
135  const CObservationOdometryPtr odo = CObservationOdometryPtr(obs);
136  ASSERT_(odo->timestamp!=INVALID_TIMESTAMP)
137 
138  CPose2D pose_before;
139  bool pose_before_valid = m_lastPoseEst.getLatestRobotPose(pose_before);
140 
141  // Move our estimation:
142  m_lastPoseEst.processUpdateNewOdometry(odo->odometry, odo->timestamp, odo->hasVelocities, odo->velocityLocal);
143 
144  if (pose_before_valid)
145  {
146  // Accumulate movement:
147  CPose2D pose_after;
148  if (m_lastPoseEst.getLatestRobotPose(pose_after))
149  this->accumulateRobotDisplacementCounters(pose_after);
150  MRPT_LOG_DEBUG_STREAM( "processObservation(): obs is CObservationOdometry, new post_after=" << pose_after);
151  }
152  } // end it's odometry
153  else
154  {
155  // Current robot pose given the timestamp of the observation (this can include a small extrapolation
156  // using the latest known robot velocities):
157  TPose2D initialEstimatedRobotPose(0,0,0);
158  {
159  mrpt::math::TTwist2D robotVelLocal, robotVelGlobal;
160  if (obs->timestamp!=INVALID_TIMESTAMP)
161  {
162  MRPT_LOG_DEBUG("processObservation(): extrapolating pose from latest pose and new observation timestamp...");
163  if (!m_lastPoseEst.getCurrentEstimate(initialEstimatedRobotPose,robotVelLocal,robotVelGlobal, obs->timestamp))
164  { // couldn't had a good extrapolation estimate... we'll have to live with the latest pose:
165  m_lastPoseEst.getLatestRobotPose(initialEstimatedRobotPose);
166  MRPT_LOG_WARN("processObservation(): new pose extrapolation failed, using last pose as is.");
167  }
168  }
169  else
170  {
171  MRPT_LOG_WARN("processObservation(): invalid observation timestamp.");
172  m_lastPoseEst.getLatestRobotPose(initialEstimatedRobotPose);
173  }
174  }
175 
176  // To know the total path length:
177  CPose2D previousKnownRobotPose;
178  m_lastPoseEst.getLatestRobotPose(previousKnownRobotPose);
179 
180  // Increment (this may only include the effects of extrapolation with velocity...):
181  this->accumulateRobotDisplacementCounters(previousKnownRobotPose); // initialEstimatedRobotPose-previousKnownRobotPose);
182 
183  // We'll skip ICP-based localization for this observation only if:
184  // - We had some odometry since the last pose correction (m_there_has_been_an_odometry=true).
185  // - AND, the traversed distance is small enough:
186  const bool we_skip_ICP_pose_correction =
190 
191  MRPT_LOG_DEBUG_STREAM( "processObservation(): skipping ICP pose correction due to small odometric displacement? : " << (we_skip_ICP_pose_correction ? "YES":"NO"));
192 
193  CICP::TReturnInfo icpReturn;
194  bool can_do_icp=false;
195 
196  // Select the map to match with ....
197  CMetricMap *matchWith = NULL;
199  {
200  matchWith = static_cast<CMetricMap*>(metricMap.m_gridMaps[0].pointer());
201  MRPT_LOG_DEBUG("processObservation(): matching against gridmap.");
202  }
203  else
204  {
205  ASSERTMSG_( metricMap.m_pointsMaps.size(), "No points map in multi-metric map." )
206  matchWith = static_cast<CMetricMap*>(metricMap.m_pointsMaps[0].pointer());
207  MRPT_LOG_DEBUG("processObservation(): matching against point map.");
208  }
209  ASSERT_(matchWith!=NULL)
210 
211  if (!we_skip_ICP_pose_correction)
212  {
214 
215  // --------------------------------------------------------------------------------------
216  // Any other observation:
217  // 1) If the observation generates points in a point map, do ICP
218  // 2) In any case, insert the observation if the minimum distance has been satisfaced.
219  // --------------------------------------------------------------------------------------
220  CSimplePointsMap sensedPoints;
221  sensedPoints.insertionOptions.minDistBetweenLaserPoints = 0.02f;
222  sensedPoints.insertionOptions.also_interpolate = false;
223 
224  // Create points representation of the observation:
225  // Insert only those planar range scans in the altitude of the grid map:
228  metricMap.m_gridMaps[0]->insertionOptions.useMapAltitude)
229  {
230  // Use grid altitude:
232  {
233  CObservation2DRangeScanPtr obsLaser = CObservation2DRangeScanPtr(obs);
234  if ( std::abs( metricMap.m_gridMaps[0]->insertionOptions.mapAltitude - obsLaser->sensorPose.z())<0.01)
235  can_do_icp = sensedPoints.insertObservationPtr(obs);
236  }
237  }
238  else
239  {
240  // Do not use grid altitude:
241  can_do_icp = sensedPoints.insertObservationPtr(obs);
242  }
243 
244  if (IS_DERIVED(matchWith,CPointsMap) && static_cast<CPointsMap*>(matchWith)->empty())
245  can_do_icp = false; // The reference map is empty!
246 
247  if (can_do_icp)
248  {
249  // We DO HAVE points with this observation:
250  // Execute ICP over the current points map and the sensed points:
251  // ----------------------------------------------------------------------
252  CICP ICP;
253  float runningTime;
254 
255  ICP.options = ICP_params;
256 
257  CPosePDFPtr pestPose= ICP.Align(
258  matchWith, // Map 1
259  &sensedPoints, // Map 2
260  mrpt::poses::CPose2D(initialEstimatedRobotPose), // a first gross estimation of map 2 relative to map 1.
261  &runningTime, // Running time
262  &icpReturn // Returned information
263  );
264 
266  {
267  // save estimation:
268  CPosePDFGaussian pEst2D;
269  pEst2D.copyFrom( *pestPose );
270 
271  m_lastPoseEst.processUpdateNewPoseLocalization( TPose2D(pEst2D.mean), obs->timestamp );
272  m_lastPoseEst_cov = pEst2D.cov;
273 
275 
276 
277  // Debug output to console:
278  MRPT_LOG_INFO_STREAM( "processObservation: previousPose="<<previousKnownRobotPose << "-> currentPose=" << pEst2D.getMeanVal() << std::endl);
279  MRPT_LOG_INFO( format("[CMetricMapBuilderICP] Fit:%.1f%% Itr:%i In %.02fms \n",
280  icpReturn.goodness*100,
281  icpReturn.nIterations,
282  1000*runningTime ) );
283  }
284  else
285  {
286  MRPT_LOG_WARN_STREAM( "Ignoring ICP of low quality: " << icpReturn.goodness*100 << std::endl);
287  }
288 
289  // Compute the transversed length:
290  CPose2D currentKnownRobotPose;
291  m_lastPoseEst.getLatestRobotPose(currentKnownRobotPose);
292 
293  this->accumulateRobotDisplacementCounters(currentKnownRobotPose); //currentKnownRobotPose - previousKnownRobotPose);
294 
295  } // end we can do ICP.
296  else
297  {
298  MRPT_LOG_WARN_STREAM( "Cannot do ICP: empty pointmap or not suitable gridmap...\n");
299  }
300 
301  } // else, we do ICP pose correction
302 
303 
304  // ----------------------------------------------------------
305  // CRITERION TO DECIDE MAP UPDATE:
306  // A distance large-enough from the last update for each sensor, AND
307  // either: (i) this was a good match or (ii) this is the first time for this sensor.
308  // ----------------------------------------------------------
309  const bool firstTimeForThisSensor = m_distSinceLastInsertion.find(obs->sensorLabel)==m_distSinceLastInsertion.end();
310  bool update = firstTimeForThisSensor ||
311  ( (!can_do_icp || icpReturn.goodness>ICP_options.minICPgoodnessToAccept) &&
312  ( m_distSinceLastInsertion[obs->sensorLabel].lin >= ICP_options.insertionLinDistance ||
313  m_distSinceLastInsertion[obs->sensorLabel].ang >= ICP_options.insertionAngDistance ) );
314 
315  // Used any "options.alwaysInsertByClass" ??
316  if (options.alwaysInsertByClass.contains(obs->GetRuntimeClass()))
317  update = true;
318 
319  // We need to always insert ALL the observations at the beginning until the first one
320  // that actually insert some points into the map used as a reference, since otherwise
321  // we'll not be able to do ICP against an empty map!!
322  if (matchWith && matchWith->isEmpty())
323  update = true;
324 
325  MRPT_LOG_DEBUG_STREAM( "update map: " << (update ? "YES":"NO") << " options.enableMapUpdating: " << (options.enableMapUpdating ? "YES":"NO"));
326 
327  if ( options.enableMapUpdating && update)
328  {
329  CTicTac tictac;
330 
331  tictac.Tic();
332 
333  // Insert the observation:
334  CPose2D currentKnownRobotPose;
335  m_lastPoseEst.getLatestRobotPose(currentKnownRobotPose);
336 
337  // Create new entry:
338  m_distSinceLastInsertion[obs->sensorLabel].last_update = currentKnownRobotPose;
339 
340  // Reset distance counters:
341  resetRobotDisplacementCounters(currentKnownRobotPose);
342  //m_distSinceLastInsertion[obs->sensorLabel].updatePose(currentKnownRobotPose);
343 
344  MRPT_LOG_INFO(mrpt::format("Updating map from pose %s\n",currentKnownRobotPose.asString().c_str()));
345 
346  CPose3D estimatedPose3D(currentKnownRobotPose);
347  const bool anymap_update = metricMap.insertObservationPtr(obs,&estimatedPose3D);
348  if (!anymap_update)
349  MRPT_LOG_WARN_STREAM( "**No map was updated** after inserting an observation of type `"<< obs->GetRuntimeClass()->className << "`");
350 
351  // Add to the vector of "poses"-"SFs" pairs:
352  CPosePDFGaussian posePDF(currentKnownRobotPose);
353  CPose3DPDFPtr pose3D = CPose3DPDFPtr( CPose3DPDF::createFrom2D( posePDF ) );
354 
355  CSensoryFramePtr sf = CSensoryFrame::Create();
356  sf->insert(obs);
357 
358  SF_Poses_seq.insert( pose3D, sf );
359 
360  MRPT_LOG_INFO_STREAM( "Map updated OK. Done in " << mrpt::system::formatTimeInterval( tictac.Tac() ) << std::endl);
361  }
362 
363  } // end other observation
364 
365  // Robot path history:
366  {
367  TPose2D p;
369  m_estRobotPath.push_back(p);
370  }
371 
372  MRPT_END
373 
374 } // end processObservation
375 
376 /*---------------------------------------------------------------
377 
378  processActionObservation
379 
380  ---------------------------------------------------------------*/
382  CActionCollection &action,
383  CSensoryFrame &in_SF )
384 {
385  // 1) process action:
386  CActionRobotMovement2DPtr movEstimation = action.getBestMovementEstimation();
387  if (movEstimation)
388  {
389  m_auxAccumOdometry.composeFrom( m_auxAccumOdometry, movEstimation->poseChange->getMeanVal() );
390 
391  CObservationOdometryPtr obs = CObservationOdometry::Create();
392  obs->timestamp = movEstimation->timestamp;
393  obs->odometry = m_auxAccumOdometry;
394  this->processObservation(obs);
395  }
396 
397  // 2) Process observations one by one:
398  for (CSensoryFrame::iterator i=in_SF.begin();i!=in_SF.end();++i)
399  this->processObservation(*i);
400 
401 }
402 
403 /*---------------------------------------------------------------
404  setCurrentMapFile
405  ---------------------------------------------------------------*/
406 void CMetricMapBuilderICP::setCurrentMapFile( const char *mapFile )
407 {
408  // Save current map to current file:
409  if (currentMapFile.size())
411 
412  // Sets new current map file:
413  currentMapFile = mapFile;
414 
415  // Load map from file or create an empty one:
416  if (currentMapFile.size())
417  loadCurrentMapFromFile( mapFile );
418 }
419 
420 
421 /*---------------------------------------------------------------
422  getCurrentPoseEstimation
423  ---------------------------------------------------------------*/
425 {
426  CPosePDFGaussian pdf2D;
428  pdf2D.cov = m_lastPoseEst_cov;
429 
430  CPose3DPDFGaussianPtr pdf3D = CPose3DPDFGaussian::Create();
431  pdf3D->copyFrom(pdf2D);
432  return pdf3D;
433 }
434 
435 /*---------------------------------------------------------------
436  initialize
437  ---------------------------------------------------------------*/
439  const CSimpleMap &initialMap,
440  CPosePDF *x0 )
441 {
442  MRPT_START
443 
444  // Reset vars:
445  m_estRobotPath.clear();
446  m_auxAccumOdometry = CPose2D(0,0,0);
447 
449  m_distSinceLastInsertion.clear();
450 
452 
453  // Init path & map:
455 
456  // Create metric maps:
458 
459  // copy map:
460  SF_Poses_seq = initialMap;
461 
462  // Parse SFs to the hybrid map:
463  // Set options:
464  // ---------------------
465  //if (metricMap.m_pointsMaps.size())
466  //{
467  // metricMap.m_pointsMaps[0]->insertionOptions.fuseWithExisting = false;
468  // metricMap.m_pointsMaps[0]->insertionOptions.minDistBetweenLaserPoints = 0.05f;
469  // metricMap.m_pointsMaps[0]->insertionOptions.disableDeletion = true;
470  // metricMap.m_pointsMaps[0]->insertionOptions.isPlanarMap = true;
471  // metricMap.m_pointsMaps[0]->insertionOptions.matchStaticPointsOnly = true;
472  //}
473 
474  // Load estimated pose from given PDF:
476 
477  if (x0)
479 
480  for (size_t i=0;i<SF_Poses_seq.size();i++)
481  {
482  CPose3DPDFPtr posePDF;
483  CSensoryFramePtr SF;
484 
485  // Get the SF and its pose:
486  SF_Poses_seq.get(i, posePDF,SF);
487 
488  CPose3D estimatedPose3D;
489  posePDF->getMean(estimatedPose3D);
490 
491  // Insert observations into the map:
492  SF->insertObservationsInto( &metricMap, &estimatedPose3D );
493  }
494 
495  MRPT_LOG_INFO("loadCurrentMapFromFile() OK.\n");
496 
497  MRPT_END
498 }
499 
500 /*---------------------------------------------------------------
501  getCurrentMapPoints
502  ---------------------------------------------------------------*/
504  std::vector<float> &x,
505  std::vector<float> &y)
506 {
507  // Critical section: We are using our global metric map
509 
511  metricMap.m_pointsMaps[0]->getAllPoints(x,y);
512 
513  // Exit critical zone.
515 }
516 
517 /*---------------------------------------------------------------
518  getCurrentlyBuiltMap
519  ---------------------------------------------------------------*/
521 {
522  out_map = SF_Poses_seq;
523 
524 }
525 
527 {
528  return &metricMap;
529 }
530 
531 
532 /*---------------------------------------------------------------
533  getCurrentlyBuiltMapSize
534  ---------------------------------------------------------------*/
536 {
537  return SF_Poses_seq.size();
538 }
539 
540 /*---------------------------------------------------------------
541  saveCurrentEstimationToImage
542  ---------------------------------------------------------------*/
544 {
545  MRPT_START
546 
547  CImage img;
548  const size_t nPoses = m_estRobotPath.size();
549 
551 
552  if (!formatEMF_BMP)
553  THROW_EXCEPTION("Not implemented yet for BMP!");
554 
555  // grid map as bitmap:
556  // ----------------------------------
557  metricMap.m_gridMaps[0]->getAsImage( img );
558 
559  // Draw paths (using vectorial plots!) over the EMF file:
560  // -------------------------------------------------
561 // float SCALE = 1000;
562  CEnhancedMetaFile EMF( file, 1000 );
563 
564  EMF.drawImage( 0,0, img );
565 
566  unsigned int imgHeight = img.getHeight();
567 
568  // Path hypothesis:
569  // ----------------------------------
570  int x1,x2,y1,y2;
571 
572  // First point: (0,0)
573  x2 = metricMap.m_gridMaps[0]->x2idx( 0.0f );
574  y2 = metricMap.m_gridMaps[0]->y2idx( 0.0f );
575 
576  // Draw path in the bitmap:
577  for (size_t j=0;j<nPoses;j++)
578  {
579  // For next segment
580  x1 = x2;
581  y1 = y2;
582 
583  // Coordinates -> pixels
584  x2 = metricMap.m_gridMaps[0]->x2idx( m_estRobotPath[j].x );
585  y2 = metricMap.m_gridMaps[0]->y2idx( m_estRobotPath[j].y );
586 
587  // Draw line:
588  EMF.line(
589  x1, imgHeight-1-y1,
590  x2, imgHeight-1-y2,
591  TColor::black );
592  }
593 
594  MRPT_END
595 }
596 
597 
599 {
601 
603  it->second.updateDistances(new_pose);
604 }
605 
607 {
608  m_distSinceLastICP.updatePose(new_pose);
609 
611  it->second.updatePose(new_pose);
612 }
613 
615 {
616  mrpt::poses::CPose2D Ap = p - this->last_update;
617  lin = Ap.norm();
618  ang = std::abs( Ap.phi() );
619 }
620 
622 {
623  this->last_update = p;
624  lin = 0;
625  ang = 0;
626 }
void saveCurrentMapToFile(const std::string &fileName, bool compressGZ=true) const
Save map (mrpt::maps::CSimpleMap) to a ".simplemap" file.
void processObservation(const mrpt::obs::CObservationPtr &obs)
The main method of this class: Process one odometry or sensor observation.
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
EIGEN_STRONG_INLINE bool empty() const
double localizationAngDistance
Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be u...
bool insertObservationPtr(const mrpt::obs::CObservationPtr &obs, const mrpt::poses::CPose3D *robotPose=NULL)
A wrapper for smart pointers, just calls the non-smart pointer version.
Definition: CMetricMap.cpp:118
void updatePose(const mrpt::poses::CPose2D &p)
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
CPose2D mean
The mean value.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees) ...
Definition: CPose2D.cpp:403
mrpt::maps::TSetOfMetricMapInitializers mapInitializers
What maps to create (at least one points map and/or a grid map are needed).
#define min(a, b)
mrpt::maps::CMultiMetricMap metricMap
The metric map representation as a points map:
mrpt::poses::CPosePDFPtr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, float *runningTime=NULL, void *info=NULL)
The method for aligning a pair of metric maps, aligning only 2D + orientation.
double minICPgoodnessToAccept
Minimum ICP goodness (0,1) to accept the resulting corrected position (default: 0.40)
void getCurrentMapPoints(std::vector< float > &x, std::vector< float > &y)
Returns the 2D points of current local map.
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:52
#define THROW_EXCEPTION(msg)
bool contains(const mrpt::utils::TRuntimeClassId *id) const
Does the list contains this class?
TDATA getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
std::deque< mrpt::math::TPose2D > m_estRobotPath
The estimated robot path:
void leaveCriticalSection()
Leave critical section for map updating.
virtual ~CMetricMapBuilderICP()
Destructor:
mrpt::aligned_containers< std::string, TDist >::map_t m_distSinceLastInsertion
Indexed by sensor label.
bool getLatestRobotPose(mrpt::math::TPose2D &pose) const
Get the latest known robot pose, either from odometry or localization.
bool enableMapUpdating
Enable map updating, default is true.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
void initialize(const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), mrpt::poses::CPosePDF *x0=NULL) MRPT_OVERRIDE
Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmod...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
unsigned int getCurrentlyBuiltMapSize() MRPT_OVERRIDE
Returns just how many sensory-frames are stored in the currently build map.
std::string BASE_IMPEXP formatTimeInterval(const double timeSeconds)
Returns a formated string with the given time difference (passed as the number of seconds)...
Definition: datetime.cpp:227
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const MRPT_OVERRIDE
Returns the map built so far.
mrpt::maps::CSimpleMap SF_Poses_seq
The set of observations that leads to current map:
ProxyFilterContainerByClass< mrpt::maps::COccupancyGridMap2DPtr, TListMaps > m_gridMaps
STL-like proxy to access this kind of maps in maps.
mrpt::poses::CPose3DPDFPtr getCurrentPoseEstimation() const MRPT_OVERRIDE
Returns a copy of the current best pose estimation as a pose PDF.
TConfigParams options
The options employed by the ICP align.
Definition: CICP.h:119
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:77
TConfigParams ICP_options
Options for the ICP-SLAM application.
double insertionAngDistance
Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be i...
Helper types for STL containers with Eigen memory allocators.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Declares a class for storing a collection of robot actions.
void reset()
Resets all internal state.
bool matchAgainstTheGrid
(default:false) Match against the occupancy grid or the points map? The former is quicker but less pr...
void updateDistances(const mrpt::poses::CPose2D &p)
This class allows loading and storing values and vectors of different types from a configuration text...
void enterCriticalSection()
Enter critical section for map updating.
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
mrpt::synch::CCriticalSection critZoneChangingMap
Critical zones.
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage: ...
CICP::TConfigParams ICP_params
Options for the ICP algorithm itself.
#define IS_DERIVED(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is an i...
Definition: CObject.h:96
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:137
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
#define MRPT_END
TConfigParams & operator=(const TConfigParams &other)
void processUpdateNewPoseLocalization(const mrpt::math::TPose2D &newPose, mrpt::system::TTimeStamp tim)
Updates the filter with new global-coordinates localization data from a localization or SLAM source...
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:174
#define MRPT_LOG_WARN(_STRING)
std::string currentMapFile
Current map file.
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
void line(int x0, int y0, int x1, int y1, const mrpt::utils::TColor color, unsigned int width=1, TPenStyle penStyle=psSolid) MRPT_OVERRIDE
Draws a line.
TConfigParams(mrpt::utils::VerbosityLevel &parent_verbosity_level)
Initializer.
#define MRPT_LOG_INFO(_STRING)
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
mrpt::utils::CListOfClasses alwaysInsertByClass
A list of observation classes (derived from mrpt::obs::CObservation) which will be always inserted in...
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
void processActionObservation(mrpt::obs::CActionCollection &action, mrpt::obs::CSensoryFrame &in_SF) MRPT_OVERRIDE
Appends a new action and observations to update this map: See the description of the class at the top...
void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const MRPT_OVERRIDE
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
#define MRPT_LOG_DEBUG(_STRING)
void resetRobotDisplacementCounters(const mrpt::poses::CPose2D &new_pose)
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:67
void loadCurrentMapFromFile(const std::string &fileName)
Load map (mrpt::maps::CSimpleMap) from a ".simplemap" file.
#define DEG2RAD
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
unsigned short nIterations
The number of executed iterations until convergence.
Definition: CICP.h:136
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
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
void setCurrentMapFile(const char *mapFile)
Sets the "current map file", thus that map will be loaded if it exists or a new one will be created i...
void processUpdateNewOdometry(const mrpt::math::TPose2D &newGlobalOdometry, mrpt::system::TTimeStamp cur_tim, bool hasVelocities=false, const mrpt::math::TTwist2D &newRobotVelLocal=mrpt::math::TTwist2D())
Updates the filter with new odometry readings.
bool also_interpolate
If set to true, far points (<1m) are interpolated with samples at "minDistSqrBetweenLaserPoints" inte...
void get(size_t index, mrpt::poses::CPose3DPDFPtr &out_posePDF, mrpt::obs::CSensoryFramePtr &out_SF) const
Access to the i&#39;th pair, first one is index &#39;0&#39;.
Definition: CSimpleMap.cpp:95
#define MRPT_START
#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...
mrpt::poses::CRobot2DPoseEstimator m_lastPoseEst
The pose estimation by the alignment algorithm (ICP).
#define RAD2DEG
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
#define MRPT_LOAD_CONFIG_VAR_DEGREES(variableName, configFileObject, sectionNameStr)
Loads a double variable, stored as radians but entered in the INI-file as degrees.
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers *initializers)
Sets the list of internal map according to the passed list of map initializers (Current maps&#39; content...
bool getCurrentEstimate(mrpt::math::TPose2D &pose, mrpt::math::TTwist2D &velLocal, mrpt::math::TTwist2D &velGlobal, mrpt::system::TTimeStamp tim_query=mrpt::system::now()) const
Get the estimate for a given timestamp (defaults to now()), obtained as:
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:93
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:122
CActionRobotMovement2DPtr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
The ICP algorithm return information.
Definition: CICP.h:128
Lightweight 2D pose.
double Tac()
Stops the stopwatch.
Definition: CTicTac.cpp:92
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
#define ASSERT_(f)
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::math::CMatrixDouble33 m_lastPoseEst_cov
Last pose estimation (covariance)
double localizationLinDistance
Minimum robot linear (m) displacement for a new observation to be used to do ICP-based localization (...
GLenum GLint GLint y
Definition: glext.h:3516
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
static CSensoryFramePtr Create()
An observation of the current (cumulative) odometry for a wheeled robot.
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: ...
ProxyFilterContainerByClass< mrpt::maps::CSimplePointsMapPtr, TListMaps > m_pointsMaps
STL-like proxy to access this kind of maps in maps.
This class stores any customizable set of metric maps.
void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP=true) MRPT_OVERRIDE
A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file...
void insert(const mrpt::poses::CPose3DPDF *in_posePDF, const mrpt::obs::CSensoryFrame &in_SF)
Add a new pair to the sequence.
Definition: CSimpleMap.cpp:201
static CObservationOdometryPtr Create()
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
#define ASSERTMSG_(f, __ERROR_MSG)
virtual bool isEmpty() const =0
Returns true if the map is empty/no observation has been inserted.
GLfloat GLfloat p
Definition: glext.h:5587
void drawImage(int x, int y, const utils::CImage &img) MRPT_OVERRIDE
Draws an image as a bitmap at a given position.
double insertionLinDistance
Minimum robot linear (m) displacement for a new observation to be inserted in the map...
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
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
void copyFrom(const CPosePDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
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.
void accumulateRobotDisplacementCounters(const mrpt::poses::CPose2D &new_pose)



Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020