MRPT  1.9.9
CMetricMapBuilderICP.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "slam-precomp.h" // Precompiled headers
11 
19 #include <mrpt/slam/CICP.h>
21 #include <mrpt/system/CTicTac.h>
22 
23 using namespace std;
24 using namespace mrpt::slam;
25 using namespace mrpt::obs;
26 using namespace mrpt::maps;
27 using namespace mrpt::poses;
28 using namespace mrpt::img;
29 using namespace mrpt::math;
30 using namespace mrpt::system;
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  Options
54  ---------------------------------------------------------------*/
56  mrpt::system::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 
70 {
71  matchAgainstTheGrid = other.matchAgainstTheGrid;
72  insertionLinDistance = other.insertionLinDistance;
73  insertionAngDistance = other.insertionAngDistance;
74  localizationLinDistance = other.localizationLinDistance;
75  localizationAngDistance = other.localizationAngDistance;
76  minICPgoodnessToAccept = other.minICPgoodnessToAccept;
77  // We can't copy a reference type
78  // verbosity_level = other.verbosity_level;
79  mapInitializers = other.mapInitializers;
80  return *this;
81 }
82 
84  const mrpt::config::CConfigFileBase& source, const std::string& section)
85 {
86  MRPT_LOAD_CONFIG_VAR(matchAgainstTheGrid, bool, source, section)
87  MRPT_LOAD_CONFIG_VAR(insertionLinDistance, double, source, section)
88  MRPT_LOAD_CONFIG_VAR_DEGREES(insertionAngDistance, source, section)
89  MRPT_LOAD_CONFIG_VAR(localizationLinDistance, double, source, section)
90  MRPT_LOAD_CONFIG_VAR_DEGREES(localizationAngDistance, source, section)
91  verbosity_level = source.read_enum<mrpt::system::VerbosityLevel>(
92  section, "verbosity_level", verbosity_level);
93 
94  MRPT_LOAD_CONFIG_VAR(minICPgoodnessToAccept, double, source, section)
95 
96  mapInitializers.loadFromConfigFile(source, section);
97 }
98 
100  std::ostream& out) const
101 {
102  out << mrpt::format(
103  "\n----------- [CMetricMapBuilderICP::TConfigParams] ------------ "
104  "\n\n");
105 
106  out << mrpt::format(
107  "insertionLinDistance = %f m\n",
108  insertionLinDistance);
109  out << mrpt::format(
110  "insertionAngDistance = %f deg\n",
111  RAD2DEG(insertionAngDistance));
112  out << mrpt::format(
113  "localizationLinDistance = %f m\n",
114  localizationLinDistance);
115  out << mrpt::format(
116  "localizationAngDistance = %f deg\n",
117  RAD2DEG(localizationAngDistance));
118  out << mrpt::format(
119  "verbosity_level = %s\n",
121  verbosity_level)
122  .c_str());
123 
124  out << mrpt::format(" Now showing 'mapsInitializers':\n");
125  mapInitializers.dumpToTextStream(out);
126 }
127 
128 /*---------------------------------------------------------------
129  processObservation
130  This is the new entry point of the algorithm (the old one
131  was processActionObservation, which now is a wrapper to
132  this method).
133  ---------------------------------------------------------------*/
135 {
136  std::lock_guard<std::mutex> lock_cs(critZoneChangingMap);
137 
138  MRPT_START
139 
142  throw std::runtime_error(
143  "Neither grid maps nor points map: Have you called initialize() "
144  "after setting ICP_options.mapInitializers?");
145 
146  ASSERT_(obs);
147 
148  // Is it an odometry observation??
149  if (IS_CLASS(obs, CObservationOdometry))
150  {
151  MRPT_LOG_DEBUG("processObservation(): obs is CObservationOdometry");
153 
154  const CObservationOdometry::Ptr odo =
155  std::dynamic_pointer_cast<CObservationOdometry>(obs);
156  ASSERT_(odo->timestamp != INVALID_TIMESTAMP);
157 
158  CPose2D pose_before;
159  bool pose_before_valid = m_lastPoseEst.getLatestRobotPose(pose_before);
160 
161  // Move our estimation:
163  odo->odometry.asTPose(), odo->timestamp, odo->hasVelocities,
164  odo->velocityLocal);
165 
166  if (pose_before_valid)
167  {
168  // Accumulate movement:
169  CPose2D pose_after;
170  if (m_lastPoseEst.getLatestRobotPose(pose_after))
171  this->accumulateRobotDisplacementCounters(pose_after);
173  "processObservation(): obs is CObservationOdometry, new "
174  "post_after="
175  << pose_after);
176  }
177  } // end it's odometry
178  else
179  {
180  // Current robot pose given the timestamp of the observation (this can
181  // include a small extrapolation
182  // using the latest known robot velocities):
183  TPose2D initialEstimatedRobotPose(0, 0, 0);
184  {
185  mrpt::math::TTwist2D robotVelLocal, robotVelGlobal;
186  if (obs->timestamp != INVALID_TIMESTAMP)
187  {
189  "processObservation(): extrapolating pose from latest pose "
190  "and new observation timestamp...");
192  initialEstimatedRobotPose, robotVelLocal,
193  robotVelGlobal, obs->timestamp))
194  { // couldn't had a good extrapolation estimate... we'll have
195  // to live with the latest pose:
196  m_lastPoseEst.getLatestRobotPose(initialEstimatedRobotPose);
198  "processObservation(): new pose extrapolation failed, "
199  "using last pose as is.");
200  }
201  }
202  else
203  {
205  "processObservation(): invalid observation timestamp.");
206  m_lastPoseEst.getLatestRobotPose(initialEstimatedRobotPose);
207  }
208  }
209 
210  // To know the total path length:
211  CPose2D previousKnownRobotPose;
212  m_lastPoseEst.getLatestRobotPose(previousKnownRobotPose);
213 
214  // Increment (this may only include the effects of extrapolation with
215  // velocity...):
217  previousKnownRobotPose); // initialEstimatedRobotPose-previousKnownRobotPose);
218 
219  // We'll skip ICP-based localization for this observation only if:
220  // - We had some odometry since the last pose correction
221  // (m_there_has_been_an_odometry=true).
222  // - AND, the traversed distance is small enough:
223  const bool we_skip_ICP_pose_correction =
231 
233  "processObservation(): skipping ICP pose correction due to small "
234  "odometric displacement? : "
235  << (we_skip_ICP_pose_correction ? "YES" : "NO"));
236 
237  CICP::TReturnInfo icpReturn;
238  bool can_do_icp = false;
239 
240  // Select the map to match with ....
241  CMetricMap* matchWith = nullptr;
242  if (auto pGrid = metricMap.mapByClass<COccupancyGridMap2D>();
244  {
245  matchWith = static_cast<CMetricMap*>(pGrid.get());
246  MRPT_LOG_DEBUG("processObservation(): matching against gridmap.");
247  }
248  else
249  {
250  auto pPts = metricMap.mapByClass<CPointsMap>();
251  ASSERTMSG_(pPts, "No points map in multi-metric map.");
252 
253  matchWith = static_cast<CMetricMap*>(pPts.get());
254  MRPT_LOG_DEBUG("processObservation(): matching against point map.");
255  }
256  ASSERT_(matchWith != nullptr);
257 
258  if (!we_skip_ICP_pose_correction)
259  {
261 
262  // --------------------------------------------------------------------------------------
263  // Any other observation:
264  // 1) If the observation generates points in a point map,
265  // do ICP 2) In any case, insert the observation if the
266  // minimum distance has been satisfaced.
267  // --------------------------------------------------------------------------------------
268  CSimplePointsMap sensedPoints;
269  sensedPoints.insertionOptions.minDistBetweenLaserPoints = 0.02f;
270  sensedPoints.insertionOptions.also_interpolate = false;
271 
272  // Create points representation of the observation:
273  // Insert only those planar range scans in the altitude of
274  // the grid map:
279  {
280  // Use grid altitude:
282  {
284  std::dynamic_pointer_cast<CObservation2DRangeScan>(obs);
285  if (std::abs(
288  obsLaser->sensorPose.z()) < 0.01)
289  can_do_icp = sensedPoints.insertObservationPtr(obs);
290  }
291  }
292  else
293  {
294  // Do not use grid altitude:
295  can_do_icp = sensedPoints.insertObservationPtr(obs);
296  }
297 
298  if (IS_DERIVED(matchWith, CPointsMap) &&
299  static_cast<CPointsMap*>(matchWith)->empty())
300  can_do_icp = false; // The reference map is empty!
301 
302  if (can_do_icp)
303  {
304  // We DO HAVE points with this observation:
305  // Execute ICP over the current points map and the
306  // sensed points:
307  // ----------------------------------------------------------------------
308  CICP ICP;
309  float runningTime;
310 
311  ICP.options = ICP_params;
312 
313  CPosePDF::Ptr pestPose = ICP.Align(
314  matchWith, // Map 1
315  &sensedPoints, // Map 2
317  initialEstimatedRobotPose), // a first gross
318  // estimation
319  // of map 2 relative to map
320  // 1.
321  &runningTime, // Running time
322  &icpReturn // Returned information
323  );
324 
326  {
327  // save estimation:
328  CPosePDFGaussian pEst2D;
329  pEst2D.copyFrom(*pestPose);
330 
332  pEst2D.mean.asTPose(), obs->timestamp);
333  m_lastPoseEst_cov = pEst2D.cov;
334 
336 
337  // Debug output to console:
339  "processObservation: previousPose="
340  << previousKnownRobotPose << "-> currentPose="
341  << pEst2D.getMeanVal() << std::endl);
343  "[CMetricMapBuilderICP] Fit:%.1f%% Itr:%i In "
344  "%.02fms \n",
345  icpReturn.goodness * 100, icpReturn.nIterations,
346  1000 * runningTime));
347  }
348  else
349  {
351  "Ignoring ICP of low quality: "
352  << icpReturn.goodness * 100 << std::endl);
353  }
354 
355  // Compute the transversed length:
356  CPose2D currentKnownRobotPose;
357  m_lastPoseEst.getLatestRobotPose(currentKnownRobotPose);
358 
360  currentKnownRobotPose); // currentKnownRobotPose -
361  // previousKnownRobotPose);
362 
363  } // end we can do ICP.
364  else
365  {
367  "Cannot do ICP: empty pointmap or not suitable "
368  "gridmap...\n");
369  }
370 
371  } // else, we do ICP pose correction
372 
373  // ----------------------------------------------------------
374  // CRITERION TO DECIDE MAP UPDATE:
375  // A distance large-enough from the last update for each
376  // sensor, AND
377  // either: (i) this was a good match or (ii) this is the
378  // first time for this sensor.
379  // ----------------------------------------------------------
380  const bool firstTimeForThisSensor =
381  m_distSinceLastInsertion.find(obs->sensorLabel) ==
383  bool update =
384  firstTimeForThisSensor ||
385  ((!can_do_icp ||
387  (m_distSinceLastInsertion[obs->sensorLabel].lin >=
389  m_distSinceLastInsertion[obs->sensorLabel].ang >=
391 
392  // Used any "options.alwaysInsertByClass" ??
393  if (options.alwaysInsertByClass.contains(obs->GetRuntimeClass()))
394  update = true;
395 
396  // We need to always insert ALL the observations at the
397  // beginning until the first one
398  // that actually insert some points into the map used as a
399  // reference, since otherwise we'll not be able to do ICP
400  // against an empty map!!
401  if (matchWith && matchWith->isEmpty()) update = true;
402 
404  "update map: " << (update ? "YES" : "NO")
405  << " options.enableMapUpdating: "
406  << (options.enableMapUpdating ? "YES" : "NO"));
407 
408  if (options.enableMapUpdating && update)
409  {
410  CTicTac tictac;
411 
412  tictac.Tic();
413 
414  // Insert the observation:
415  CPose2D currentKnownRobotPose;
416  m_lastPoseEst.getLatestRobotPose(currentKnownRobotPose);
417 
418  // Create new entry:
419  m_distSinceLastInsertion[obs->sensorLabel].last_update =
420  currentKnownRobotPose.asTPose();
421 
422  // Reset distance counters:
423  resetRobotDisplacementCounters(currentKnownRobotPose);
424  // m_distSinceLastInsertion[obs->sensorLabel].updatePose(currentKnownRobotPose);
425 
427  "Updating map from pose %s\n",
428  currentKnownRobotPose.asString().c_str()));
429 
430  CPose3D estimatedPose3D(currentKnownRobotPose);
431  const bool anymap_update =
432  metricMap.insertObservationPtr(obs, &estimatedPose3D);
433  if (!anymap_update)
435  "**No map was updated** after inserting an "
436  "observation of "
437  "type `"
438  << obs->GetRuntimeClass()->className << "`");
439 
440  // Add to the vector of "poses"-"SFs" pairs:
441  CPosePDFGaussian posePDF(currentKnownRobotPose);
442  CPose3DPDF::Ptr pose3D =
443  CPose3DPDF::Ptr(CPose3DPDF::createFrom2D(posePDF));
444 
445  CSensoryFrame::Ptr sf = mrpt::make_aligned_shared<CSensoryFrame>();
446  sf->insert(obs);
447 
448  SF_Poses_seq.insert(pose3D, sf);
449 
451  "Map updated OK. Done in "
452  << mrpt::system::formatTimeInterval(tictac.Tac()) << std::endl);
453  }
454 
455  } // end other observation
456 
457  // Robot path history:
458  {
459  TPose2D p;
461  }
462 
463  MRPT_END
464 
465 } // end processObservation
466 
467 /*---------------------------------------------------------------
468 
469  processActionObservation
470 
471  ---------------------------------------------------------------*/
473  CActionCollection& action, CSensoryFrame& in_SF)
474 {
475  // 1) process action:
476  CActionRobotMovement2D::Ptr movEstimation =
477  action.getBestMovementEstimation();
478  if (movEstimation)
479  {
481  m_auxAccumOdometry, movEstimation->poseChange->getMeanVal());
482 
484  mrpt::make_aligned_shared<CObservationOdometry>();
485  obs->timestamp = movEstimation->timestamp;
486  obs->odometry = m_auxAccumOdometry;
487  this->processObservation(obs);
488  }
489 
490  // 2) Process observations one by one:
491  for (auto& i : in_SF) this->processObservation(i);
492 }
493 
494 /*---------------------------------------------------------------
495  setCurrentMapFile
496  ---------------------------------------------------------------*/
498 {
499  // Save current map to current file:
501 
502  // Sets new current map file:
503  currentMapFile = mapFile;
504 
505  // Load map from file or create an empty one:
506  if (currentMapFile.size()) loadCurrentMapFromFile(mapFile);
507 }
508 
509 /*---------------------------------------------------------------
510  getCurrentPoseEstimation
511  ---------------------------------------------------------------*/
513 {
514  CPosePDFGaussian pdf2D;
516  pdf2D.cov = m_lastPoseEst_cov;
517 
519  mrpt::make_aligned_shared<CPose3DPDFGaussian>();
520  pdf3D->copyFrom(pdf2D);
521  return pdf3D;
522 }
523 
524 /*---------------------------------------------------------------
525  initialize
526  ---------------------------------------------------------------*/
528  const CSimpleMap& initialMap, const CPosePDF* x0)
529 {
530  MRPT_START
531 
532  // Reset vars:
533  m_estRobotPath.clear();
534  m_auxAccumOdometry = CPose2D(0, 0, 0);
535 
537  m_distSinceLastInsertion.clear();
538 
540 
541  // Init path & map:
542  std::lock_guard<std::mutex> lock_cs(critZoneChangingMap);
543 
544  // Create metric maps:
546 
547  // copy map:
548  SF_Poses_seq = initialMap;
549 
550  // Parse SFs to the hybrid map:
551  // Set options:
552  // ---------------------
553  // if (metricMap.m_pointsMaps.size())
554  //{
555  // metricMap.m_pointsMaps[0]->insertionOptions.fuseWithExisting =
556  // false;
557  // metricMap.m_pointsMaps[0]->insertionOptions.minDistBetweenLaserPoints
558  //=
559  // 0.05f;
560  // metricMap.m_pointsMaps[0]->insertionOptions.disableDeletion =
561  // true;
562  // metricMap.m_pointsMaps[0]->insertionOptions.isPlanarMap =
563  // true;
564  // metricMap.m_pointsMaps[0]->insertionOptions.matchStaticPointsOnly =
565  // true;
566  //}
567 
568  // Load estimated pose from given PDF:
570 
571  if (x0)
573  x0->getMeanVal().asTPose(), mrpt::Clock::now());
574 
575  for (size_t i = 0; i < SF_Poses_seq.size(); i++)
576  {
577  CPose3DPDF::Ptr posePDF;
579 
580  // Get the SF and its pose:
581  SF_Poses_seq.get(i, posePDF, SF);
582 
583  CPose3D estimatedPose3D;
584  posePDF->getMean(estimatedPose3D);
585 
586  // Insert observations into the map:
587  SF->insertObservationsInto(&metricMap, &estimatedPose3D);
588  }
589 
590  MRPT_LOG_INFO("loadCurrentMapFromFile() OK.\n");
591 
592  MRPT_END
593 }
594 
596  std::vector<float>& x, std::vector<float>& y)
597 {
598  // Critical section: We are using our global metric map
600 
601  auto pPts = metricMap.mapByClass<CPointsMap>(0);
602 
603  ASSERT_(pPts);
604  pPts->getAllPoints(x, y);
605 
606  // Exit critical zone.
608 }
609 
610 /*---------------------------------------------------------------
611  getCurrentlyBuiltMap
612  ---------------------------------------------------------------*/
614 {
615  out_map = SF_Poses_seq;
616 }
617 
619 {
620  return &metricMap;
621 }
622 
623 /*---------------------------------------------------------------
624  getCurrentlyBuiltMapSize
625  ---------------------------------------------------------------*/
627 {
628  return SF_Poses_seq.size();
629 }
630 
631 /*---------------------------------------------------------------
632  saveCurrentEstimationToImage
633  ---------------------------------------------------------------*/
635  const std::string& file, bool formatEMF_BMP)
636 {
637  MRPT_START
638 
639  CImage img;
640  const size_t nPoses = m_estRobotPath.size();
641 
642  if (!formatEMF_BMP) THROW_EXCEPTION("Not implemented yet for BMP!");
643 
644  // grid map as bitmap:
645  auto pGrid = metricMap.mapByClass<COccupancyGridMap2D>();
646  if (pGrid) pGrid->getAsImage(img);
647 
648  // Draw paths (using vectorial plots!) over the EMF file:
649  // -------------------------------------------------
650  CEnhancedMetaFile EMF(file, 1000);
651 
652  EMF.drawImage(0, 0, img);
653 
654  unsigned int imgHeight = img.getHeight();
655 
656  // Path hypothesis:
657  // ----------------------------------
658  int x1, x2, y1, y2;
659 
660  // First point: (0,0)
661  x2 = pGrid->x2idx(0.0f);
662  y2 = pGrid->y2idx(0.0f);
663 
664  // Draw path in the bitmap:
665  for (size_t j = 0; j < nPoses; j++)
666  {
667  // For next segment
668  x1 = x2;
669  y1 = y2;
670 
671  // Coordinates -> pixels
672  x2 = pGrid->x2idx(m_estRobotPath[j].x);
673  y2 = pGrid->y2idx(m_estRobotPath[j].y);
674 
675  // Draw line:
676  EMF.line(
677  x1, imgHeight - 1 - y1, x2, imgHeight - 1 - y2, TColor::black());
678  }
679 
680  MRPT_END
681 }
682 
684  const CPose2D& new_pose)
685 {
687  for (auto& m : m_distSinceLastInsertion) m.second.updateDistances(new_pose);
688 }
689 
691  const CPose2D& new_pose)
692 {
693  m_distSinceLastICP.updatePose(new_pose);
694  for (auto& m : m_distSinceLastInsertion) m.second.updatePose(new_pose);
695 }
696 
698 {
699  const auto Ap = p - mrpt::poses::CPose2D(this->last_update);
700  lin = Ap.norm();
701  ang = std::abs(Ap.phi());
702 }
703 
705 {
706  this->last_update = p.asTPose();
707  lin = 0;
708  ang = 0;
709 }
bool insertObservationPtr(const mrpt::obs::CObservation::Ptr &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
A wrapper for smart pointers, just calls the non-smart pointer version.
Definition: CMetricMap.cpp:108
void saveCurrentMapToFile(const std::string &fileName, bool compressGZ=true) const
Save map (mrpt::maps::CSimpleMap) to a ".simplemap" file.
std::size_t countMapsByClass() const
Count how many maps exist of the given class (or derived class)
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
This class represents a Windows Enhanced Meta File (EMF) for generating and saving graphics...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
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...
void updatePose(const mrpt::poses::CPose2D &p)
#define MRPT_START
Definition: exceptions.h:282
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
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:440
mrpt::maps::TSetOfMetricMapInitializers mapInitializers
What maps to create (at least one points map and/or a grid map are needed).
std::map< std::string, TDist > m_distSinceLastInsertion
Indexed by sensor label.
#define min(a, b)
VerbosityLevel
Enumeration of available verbosity levels.
double RAD2DEG(const double x)
Radians to degrees.
mrpt::maps::CMultiMetricMap metricMap
The metric map representation as a points map:
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:108
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...
Definition: CSimpleMap.h:32
mrpt::poses::CPosePDF::Ptr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, float *runningTime=nullptr, void *info=nullptr)
The method for aligning a pair of metric maps, aligning only 2D + orientation.
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:64
double DEG2RAD(const double x)
Degrees to radians.
mrpt::rtti::CListOfClasses alwaysInsertByClass
A list of observation classes (derived from mrpt::obs::CObservation) which will be always inserted in...
std::deque< mrpt::math::TPose2D > m_estRobotPath
The estimated robot path:
void leaveCriticalSection()
Leave critical section for map updating.
void drawImage(int x, int y, const mrpt::img::CImage &img) override
Draws an image as a bitmap at a given position.
A high-performance stopwatch, with typical resolution of nanoseconds.
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.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
TDATA getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
std::string formatTimeInterval(const double timeSeconds)
Returns a formated string with the given time difference (passed as the number of seconds)...
Definition: datetime.cpp:124
mrpt::maps::CSimpleMap SF_Poses_seq
The set of observations that leads to current map:
static time_point now() noexcept
Returns the current time, with the highest resolution available.
Definition: Clock.cpp:46
TConfigParams options
The options employed by the ICP align.
Definition: CICP.h:180
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...
void initialize(const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), const mrpt::poses::CPosePDF *x0=nullptr) override
Initialize the method, starting with a known location PDF "x0"(if supplied, set to nullptr to left un...
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
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...
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:463
void updateDistances(const mrpt::poses::CPose2D &p)
void enterCriticalSection()
Enter critical section for map updating.
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:161
CICP::TConfigParams ICP_params
Options for the ICP algorithm itself.
#define IS_DERIVED(ptrObj, class_name)
Evaluates to true if a pointer to an object (derived from mrpt::rtti::CObject) is an instance of the ...
Definition: CObject.h:108
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:197
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:65
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
TConfigParams(mrpt::system::VerbosityLevel &parent_verbosity_level)
Initializer.
CActionRobotMovement2D::Ptr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
TConfigParams & operator=(const TConfigParams &other)
std::string currentMapFile
Current map file.
A helper class that can convert an enum value into its textual representation, and viceversa...
void processUpdateNewOdometry(const mrpt::math::TPose2D &newGlobalOdometry, mrpt::Clock::time_point cur_tim, bool hasVelocities=false, const mrpt::math::TTwist2D &newRobotVelLocal=mrpt::math::TTwist2D())
Updates the filter with new odometry readings.
GLint GLvoid * img
Definition: glext.h:3769
void line(int x0, int y0, int x1, int y1, const mrpt::img::TColor color, unsigned int width=1, TPenStyle penStyle=psSolid) override
Draws a line.
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const override
Returns the map built so far.
void setLoggerName(const std::string &name)
Set the name of the COutputLogger instance.
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...
Definition: CSensoryFrame.h:51
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:149
void resetRobotDisplacementCounters(const mrpt::poses::CPose2D &new_pose)
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:53
void loadCurrentMapFromFile(const std::string &fileName)
Load map (mrpt::maps::CSimpleMap) from a ".simplemap" file.
void get(size_t index, mrpt::poses::CPose3DPDF::Ptr &out_posePDF, mrpt::obs::CSensoryFrame::Ptr &out_SF) const
Access to the i&#39;th pair, first one is index &#39;0&#39;.
Definition: CSimpleMap.cpp:56
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
unsigned short nIterations
The number of executed iterations until convergence.
Definition: CICP.h:194
GLsizei const GLchar ** string
Definition: glext.h:4116
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:38
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void 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...
bool also_interpolate
If set to true, far points (<1m) are interpolated with samples at "minDistSqrBetweenLaserPoints" inte...
Definition: CPointsMap.h:239
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
A class for storing an occupancy grid map.
void getAsImage(mrpt::img::CImage &img, bool verticalFlip=false, bool forceRGB=false, bool tricolor=false) const
Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if ...
mrpt::poses::CRobot2DPoseEstimator m_lastPoseEst
The pose estimation by the alignment algorithm (ICP).
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
bool contains(const mrpt::rtti::TRuntimeClassId *id) const
Does the list contains this class?
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
bool getCurrentEstimate(mrpt::math::TPose2D &pose, mrpt::math::TTwist2D &velLocal, mrpt::math::TTwist2D &velGlobal, mrpt::Clock::time_point tim_query=mrpt::Clock::now()) const
Get the estimate for a given timestamp (defaults to now()), obtained as:
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:53
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:82
T::Ptr mapByClass(const size_t &ith=0) const
Returns the i&#39;th map of a given class (or of a derived class), or empty smart pointer if there is no ...
~CMetricMapBuilderICP() override
Destructor:
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:102
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:133
#define MRPT_END
Definition: exceptions.h:286
bool useMapAltitude
The parameter "mapAltitude" has effect while inserting observations in the grid only if this is true...
The ICP algorithm return information.
Definition: CICP.h:190
Lightweight 2D pose.
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
void processObservation(const mrpt::obs::CObservation::Ptr &obs)
The main method of this class: Process one odometry or sensor observation.
float mapAltitude
The altitude (z-axis) of 2D scans (within a 0.01m tolerance) for they to be inserted in this map! ...
void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const override
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
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:3542
#define MRPT_LOG_WARN(_STRING)
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:271
mrpt::poses::CPose3DPDF::Ptr getCurrentPoseEstimation() const override
Returns a copy of the current best pose estimation as a pose PDF.
#define MRPT_LOAD_CONFIG_VAR_DEGREES( variableName, configFileObject, sectionNameStr)
Loads a double variable, stored as radians but entered in the INI-file as degrees.
An observation of the current (cumulative) odometry for a wheeled robot.
GLenum GLint x
Definition: glext.h:3542
void processUpdateNewPoseLocalization(const mrpt::math::TPose2D &newPose, mrpt::Clock::time_point tim)
Updates the filter with new global-coordinates localization data from a localization or SLAM source...
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
This class stores any customizable set of metric maps.
void insert(const mrpt::poses::CPose3DPDF *in_posePDF, const mrpt::obs::CSensoryFrame &in_SF)
Add a new pair to the sequence.
Definition: CSimpleMap.cpp:138
virtual bool isEmpty() const =0
Returns true if the map is empty/no observation has been inserted.
GLfloat GLfloat p
Definition: glext.h:6398
unsigned int getCurrentlyBuiltMapSize() override
Returns just how many sensory-frames are stored in the currently build map.
std::mutex critZoneChangingMap
Critical zones.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
void processActionObservation(mrpt::obs::CActionCollection &action, mrpt::obs::CSensoryFrame &in_SF) override
Appends a new action and observations to update this map: See the description of the class at the top...
double insertionLinDistance
Minimum robot linear (m) displacement for a new observation to be inserted in the map...
void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP=true) override
A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file...
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers &init)
Sets the list of internal map according to the passed list of map initializers (current maps will be ...
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
Definition: CPointsMap.h:232
#define MRPT_LOG_INFO(_STRING)
void accumulateRobotDisplacementCounters(const mrpt::poses::CPose2D &new_pose)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: bef67d9c5 Mon Apr 15 00:03:11 2019 +0200 at lun abr 15 00:10:13 CEST 2019