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 = std::make_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  std::make_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 
518  CPose3DPDFGaussian::Ptr pdf3D = std::make_shared<CPose3DPDFGaussian>();
519  pdf3D->copyFrom(pdf2D);
520  return pdf3D;
521 }
522 
523 /*---------------------------------------------------------------
524  initialize
525  ---------------------------------------------------------------*/
527  const CSimpleMap& initialMap, const CPosePDF* x0)
528 {
529  MRPT_START
530 
531  // Reset vars:
532  m_estRobotPath.clear();
533  m_auxAccumOdometry = CPose2D(0, 0, 0);
534 
536  m_distSinceLastInsertion.clear();
537 
539 
540  // Init path & map:
541  std::lock_guard<std::mutex> lock_cs(critZoneChangingMap);
542 
543  // Create metric maps:
545 
546  // copy map:
547  SF_Poses_seq = initialMap;
548 
549  // Parse SFs to the hybrid map:
550  // Set options:
551  // ---------------------
552  // if (metricMap.m_pointsMaps.size())
553  //{
554  // metricMap.m_pointsMaps[0]->insertionOptions.fuseWithExisting =
555  // false;
556  // metricMap.m_pointsMaps[0]->insertionOptions.minDistBetweenLaserPoints
557  //=
558  // 0.05f;
559  // metricMap.m_pointsMaps[0]->insertionOptions.disableDeletion =
560  // true;
561  // metricMap.m_pointsMaps[0]->insertionOptions.isPlanarMap =
562  // true;
563  // metricMap.m_pointsMaps[0]->insertionOptions.matchStaticPointsOnly =
564  // true;
565  //}
566 
567  // Load estimated pose from given PDF:
569 
570  if (x0)
572  x0->getMeanVal().asTPose(), mrpt::Clock::now());
573 
574  for (size_t i = 0; i < SF_Poses_seq.size(); i++)
575  {
576  CPose3DPDF::Ptr posePDF;
578 
579  // Get the SF and its pose:
580  SF_Poses_seq.get(i, posePDF, SF);
581 
582  CPose3D estimatedPose3D;
583  posePDF->getMean(estimatedPose3D);
584 
585  // Insert observations into the map:
586  SF->insertObservationsInto(&metricMap, &estimatedPose3D);
587  }
588 
589  MRPT_LOG_INFO("loadCurrentMapFromFile() OK.\n");
590 
591  MRPT_END
592 }
593 
595  std::vector<float>& x, std::vector<float>& y)
596 {
597  // Critical section: We are using our global metric map
599 
600  auto pPts = metricMap.mapByClass<CPointsMap>(0);
601 
602  ASSERT_(pPts);
603  pPts->getAllPoints(x, y);
604 
605  // Exit critical zone.
607 }
608 
609 /*---------------------------------------------------------------
610  getCurrentlyBuiltMap
611  ---------------------------------------------------------------*/
613 {
614  out_map = SF_Poses_seq;
615 }
616 
618 {
619  return &metricMap;
620 }
621 
622 /*---------------------------------------------------------------
623  getCurrentlyBuiltMapSize
624  ---------------------------------------------------------------*/
626 {
627  return SF_Poses_seq.size();
628 }
629 
630 /*---------------------------------------------------------------
631  saveCurrentEstimationToImage
632  ---------------------------------------------------------------*/
634  const std::string& file, bool formatEMF_BMP)
635 {
636  MRPT_START
637 
638  CImage img;
639  const size_t nPoses = m_estRobotPath.size();
640 
641  if (!formatEMF_BMP) THROW_EXCEPTION("Not implemented yet for BMP!");
642 
643  // grid map as bitmap:
644  auto pGrid = metricMap.mapByClass<COccupancyGridMap2D>();
645  if (pGrid) pGrid->getAsImage(img);
646 
647  // Draw paths (using vectorial plots!) over the EMF file:
648  // -------------------------------------------------
649  CEnhancedMetaFile EMF(file, 1000);
650 
651  EMF.drawImage(0, 0, img);
652 
653  unsigned int imgHeight = img.getHeight();
654 
655  // Path hypothesis:
656  // ----------------------------------
657  int x1, x2, y1, y2;
658 
659  // First point: (0,0)
660  x2 = pGrid->x2idx(0.0f);
661  y2 = pGrid->y2idx(0.0f);
662 
663  // Draw path in the bitmap:
664  for (size_t j = 0; j < nPoses; j++)
665  {
666  // For next segment
667  x1 = x2;
668  y1 = y2;
669 
670  // Coordinates -> pixels
671  x2 = pGrid->x2idx(m_estRobotPath[j].x);
672  y2 = pGrid->y2idx(m_estRobotPath[j].y);
673 
674  // Draw line:
675  EMF.line(
676  x1, imgHeight - 1 - y1, x2, imgHeight - 1 - y2, TColor::black());
677  }
678 
679  MRPT_END
680 }
681 
683  const CPose2D& new_pose)
684 {
686  for (auto& m : m_distSinceLastInsertion) m.second.updateDistances(new_pose);
687 }
688 
690  const CPose2D& new_pose)
691 {
692  m_distSinceLastICP.updatePose(new_pose);
693  for (auto& m : m_distSinceLastInsertion) m.second.updatePose(new_pose);
694 }
695 
697 {
698  const auto Ap = p - mrpt::poses::CPose2D(this->last_update);
699  lin = Ap.norm();
700  ang = std::abs(Ap.phi());
701 }
702 
704 {
705  this->last_update = p.asTPose();
706  lin = 0;
707  ang = 0;
708 }
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:107
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) ...
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:241
#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:445
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:67
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.
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:468
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)
Definition: TTwist2D.h:19
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
CICP::TConfigParams ICP_params
Options for the ICP algorithm itself.
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.
#define IS_DERIVED(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is an instance of the given ...
Definition: CObject.h:138
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 IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:133
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
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
type_value getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
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...
bool empty() const
Definition: ts_hash_map.h:190
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:52
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
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
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:135
#define MRPT_END
Definition: exceptions.h:245
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.
Definition: TPose2D.h:22
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: 1de0e027c Sat Sep 14 16:15:22 2019 +0200 at sáb sep 14 16:20:14 CEST 2019