Main MRPT website > C++ reference for MRPT 1.5.6
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 }
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
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.
TDATA getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
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)
CPose2D mean
The mean value.
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.
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...
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
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)
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 enableMapUpdating
Enable map updating, default is true.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
void get(size_t index, mrpt::poses::CPose3DPDFPtr &out_posePDF, mrpt::obs::CSensoryFramePtr &out_SF) const
Access to the i'th pair, first one is index '0'.
Definition: CSimpleMap.cpp:95
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. ...
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
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.
GLint GLvoid * img
Definition: glew.h:1290
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:67
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:106
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...
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:
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...
#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...
EIGEN_STRONG_INLINE bool empty() const
std::deque< CObservationPtr >::iterator iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
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.
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 getLatestRobotPose(mrpt::math::TPose2D &pose) const
Get the latest known robot pose, either from odometry or localization.
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
void saveCurrentMapToFile(const std::string &fileName, bool compressGZ=true) const
Save map (mrpt::maps::CSimpleMap) to a ".simplemap" file.
mrpt::utils::CListOfClasses alwaysInsertByClass
A list of observation classes (derived from mrpt::obs::CObservation) which will be always inserted in...
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
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)
void loadCurrentMapFromFile(const std::string &fileName)
Load map (mrpt::maps::CSimpleMap) from a ".simplemap" file.
GLfloat GLfloat p
Definition: glew.h:10113
#define DEG2RAD
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
CActionRobotMovement2DPtr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
unsigned short nIterations
The number of executed iterations until convergence.
Definition: CICP.h:136
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage: ...
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
#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...
#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
bool contains(const mrpt::utils::TRuntimeClassId *id) const
Does the list contains this class?
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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' content...
#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:103
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:122
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:174
The ICP algorithm return information.
Definition: CICP.h:128
Lightweight 2D pose.
double Tac()
Stops the stopwatch.
Definition: CTicTac.cpp:92
#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 (...
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.
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.
void drawImage(int x, int y, const utils::CImage &img) MRPT_OVERRIDE
Draws an image as a bitmap at a given position.
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
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)
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



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