MRPT  2.0.2
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-2020, 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 
13 #include <mrpt/core/lock_helper.h>
20 #include <mrpt/slam/CICP.h>
22 #include <mrpt/system/CTicTac.h>
23 
24 using namespace std;
25 using namespace mrpt::slam;
26 using namespace mrpt::obs;
27 using namespace mrpt::maps;
28 using namespace mrpt::poses;
29 using namespace mrpt::img;
30 using namespace mrpt::math;
31 using namespace mrpt::system;
32 
33 CMetricMapBuilderICP::CMetricMapBuilderICP()
34  : ICP_options(m_min_verbosity_level)
35 {
36  this->setLoggerName("CMetricMapBuilderICP");
37  this->initialize(CSimpleMap());
38 }
39 
40 /*---------------------------------------------------------------
41  Destructor
42  ---------------------------------------------------------------*/
44 {
45  // Ensure, we have exit all critical zones:
48 
49  // Save current map to current file:
51 }
52 
53 /*---------------------------------------------------------------
54  Options
55  ---------------------------------------------------------------*/
57  mrpt::system::VerbosityLevel& parent_verbosity_level)
58  : matchAgainstTheGrid(false),
59  insertionLinDistance(1.0),
60  insertionAngDistance(30.0_deg),
61  localizationLinDistance(0.20),
62  localizationAngDistance(30.0_deg),
63  minICPgoodnessToAccept(0.40),
64  verbosity_level(parent_verbosity_level),
65  mapInitializers()
66 {
67 }
68 
71 {
72  matchAgainstTheGrid = other.matchAgainstTheGrid;
73  insertionLinDistance = other.insertionLinDistance;
74  insertionAngDistance = other.insertionAngDistance;
75  localizationLinDistance = other.localizationLinDistance;
76  localizationAngDistance = other.localizationAngDistance;
77  minICPgoodnessToAccept = other.minICPgoodnessToAccept;
78  // We can't copy a reference type
79  // verbosity_level = other.verbosity_level;
80  mapInitializers = other.mapInitializers;
81  return *this;
82 }
83 
85  const mrpt::config::CConfigFileBase& source, const std::string& section)
86 {
87  MRPT_LOAD_CONFIG_VAR(matchAgainstTheGrid, bool, source, section)
88  MRPT_LOAD_CONFIG_VAR(insertionLinDistance, double, source, section)
89  MRPT_LOAD_CONFIG_VAR_DEGREES(insertionAngDistance, source, section)
90  MRPT_LOAD_CONFIG_VAR(localizationLinDistance, double, source, section)
91  MRPT_LOAD_CONFIG_VAR_DEGREES(localizationAngDistance, source, section)
92  verbosity_level = source.read_enum<mrpt::system::VerbosityLevel>(
93  section, "verbosity_level", verbosity_level);
94 
95  MRPT_LOAD_CONFIG_VAR(minICPgoodnessToAccept, double, source, section)
96 
97  mapInitializers.loadFromConfigFile(source, section);
98 }
99 
101  std::ostream& out) const
102 {
103  out << "\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 << " 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 {
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  10.0 /*seconds*/,
199  "processObservation(): new pose extrapolation failed, "
200  "using last pose as is.");
201  }
202  }
203  else
204  {
206  "processObservation(): invalid observation timestamp.");
207  m_lastPoseEst.getLatestRobotPose(initialEstimatedRobotPose);
208  }
209  }
210 
211  // To know the total path length:
212  CPose2D previousKnownRobotPose;
213  m_lastPoseEst.getLatestRobotPose(previousKnownRobotPose);
214 
215  // Increment (this may only include the effects of extrapolation with
216  // velocity...):
218  previousKnownRobotPose); // initialEstimatedRobotPose-previousKnownRobotPose);
219 
220  // We'll skip ICP-based localization for this observation only if:
221  // - We had some odometry since the last pose correction
222  // (m_there_has_been_an_odometry=true).
223  // - AND, the traversed distance is small enough:
224  const bool we_skip_ICP_pose_correction =
226  m_distSinceLastICP.lin < std::min(
229  m_distSinceLastICP.ang < std::min(
232 
234  "processObservation(): skipping ICP pose correction due to small "
235  "odometric displacement? : "
236  << (we_skip_ICP_pose_correction ? "YES" : "NO"));
237 
238  CICP::TReturnInfo icpReturn;
239  bool can_do_icp = false;
240 
241  // Select the map to match with ....
242  CMetricMap* matchWith = nullptr;
243  if (auto pGrid = metricMap.mapByClass<COccupancyGridMap2D>();
245  {
246  matchWith = static_cast<CMetricMap*>(pGrid.get());
247  MRPT_LOG_DEBUG("processObservation(): matching against gridmap.");
248  }
249  else
250  {
251  auto pPts = metricMap.mapByClass<CPointsMap>();
252  ASSERTMSG_(pPts, "No points map in multi-metric map.");
253 
254  matchWith = static_cast<CMetricMap*>(pPts.get());
255  MRPT_LOG_DEBUG("processObservation(): matching against point map.");
256  }
257  ASSERT_(matchWith != nullptr);
258 
259  if (!we_skip_ICP_pose_correction)
260  {
262 
263  // --------------------------------------------------------------------------------------
264  // Any other observation:
265  // 1) If the observation generates points in a point map,
266  // do ICP 2) In any case, insert the observation if the
267  // minimum distance has been satisfaced.
268  // --------------------------------------------------------------------------------------
269  CSimplePointsMap sensedPoints;
270  sensedPoints.insertionOptions.minDistBetweenLaserPoints = 0.02f;
271  sensedPoints.insertionOptions.also_interpolate = false;
272 
273  // Create points representation of the observation:
274  // Insert only those planar range scans in the altitude of
275  // the grid map:
280  {
281  // Use grid altitude:
283  {
285  std::dynamic_pointer_cast<CObservation2DRangeScan>(obs);
286  if (std::abs(
289  obsLaser->sensorPose.z()) < 0.01)
290  can_do_icp = sensedPoints.insertObservationPtr(obs);
291  }
292  }
293  else
294  {
295  // Do not use grid altitude:
296  can_do_icp = sensedPoints.insertObservationPtr(obs);
297  }
298 
299  if (IS_DERIVED(*matchWith, CPointsMap) &&
300  static_cast<CPointsMap*>(matchWith)->empty())
301  can_do_icp = false; // The reference map is empty!
302 
303  if (can_do_icp)
304  {
305  // We DO HAVE points with this observation:
306  // Execute ICP over the current points map and the
307  // sensed points:
308  // ----------------------------------------------------------------------
309  CICP ICP;
310  ICP.options = ICP_params;
311 
312  // a first gross estimation of map 2 relative to map 1.
313  const auto firstGuess =
314  mrpt::poses::CPose2D(initialEstimatedRobotPose);
315 
316  CPosePDF::Ptr pestPose = ICP.Align(
317  matchWith, // Map 1
318  &sensedPoints, // Map 2
319  firstGuess, icpReturn);
320 
322  {
323  // save estimation:
324  CPosePDFGaussian pEst2D;
325  pEst2D.copyFrom(*pestPose);
326 
328  pEst2D.mean.asTPose(), obs->timestamp);
329  m_lastPoseEst_cov = pEst2D.cov;
330 
332 
333  // Debug output to console:
335  "processObservation: previousPose="
336  << previousKnownRobotPose << "-> currentPose="
337  << pEst2D.getMeanVal() << std::endl);
339  "[CMetricMapBuilderICP] Fit:%.1f%% Itr:%i In "
340  "%.02fms \n",
341  icpReturn.goodness * 100, icpReturn.nIterations,
342  1000 * icpReturn.executionTime));
343  }
344  else
345  {
347  "Ignoring ICP of low quality: "
348  << icpReturn.goodness * 100 << std::endl);
349  }
350 
351  // Compute the transversed length:
352  CPose2D currentKnownRobotPose;
353  m_lastPoseEst.getLatestRobotPose(currentKnownRobotPose);
354 
356  currentKnownRobotPose); // currentKnownRobotPose -
357  // previousKnownRobotPose);
358 
359  } // end we can do ICP.
360  else
361  {
363  "Cannot do ICP: empty pointmap or not suitable "
364  "gridmap...\n");
365  }
366 
367  } // else, we do ICP pose correction
368 
369  // ----------------------------------------------------------
370  // CRITERION TO DECIDE MAP UPDATE:
371  // A distance large-enough from the last update for each
372  // sensor, AND
373  // either: (i) this was a good match or (ii) this is the
374  // first time for this sensor.
375  // ----------------------------------------------------------
376  const bool firstTimeForThisSensor =
377  m_distSinceLastInsertion.find(obs->sensorLabel) ==
379  bool update =
380  firstTimeForThisSensor ||
381  ((!can_do_icp ||
383  (m_distSinceLastInsertion[obs->sensorLabel].lin >=
385  m_distSinceLastInsertion[obs->sensorLabel].ang >=
387 
388  // Used any "options.alwaysInsertByClass" ??
389  if (options.alwaysInsertByClass.contains(obs->GetRuntimeClass()))
390  update = true;
391 
392  // We need to always insert ALL the observations at the
393  // beginning until the first one
394  // that actually insert some points into the map used as a
395  // reference, since otherwise we'll not be able to do ICP
396  // against an empty map!!
397  if (matchWith && matchWith->isEmpty()) update = true;
398 
400  "update map: " << (update ? "YES" : "NO")
401  << " options.enableMapUpdating: "
402  << (options.enableMapUpdating ? "YES" : "NO"));
403 
404  if (options.enableMapUpdating && update)
405  {
406  CTicTac tictac;
407 
408  tictac.Tic();
409 
410  // Insert the observation:
411  CPose2D currentKnownRobotPose;
412  m_lastPoseEst.getLatestRobotPose(currentKnownRobotPose);
413 
414  // Create new entry:
415  m_distSinceLastInsertion[obs->sensorLabel].last_update =
416  currentKnownRobotPose.asTPose();
417 
418  // Reset distance counters:
419  resetRobotDisplacementCounters(currentKnownRobotPose);
420  // m_distSinceLastInsertion[obs->sensorLabel].updatePose(currentKnownRobotPose);
421 
423  "Updating map from pose %s\n",
424  currentKnownRobotPose.asString().c_str()));
425 
426  CPose3D estimatedPose3D(currentKnownRobotPose);
427  const bool anymap_update =
428  metricMap.insertObservationPtr(obs, &estimatedPose3D);
429  if (!anymap_update)
431  "**No map was updated** after inserting an "
432  "observation of "
433  "type `"
434  << obs->GetRuntimeClass()->className << "`");
435 
436  // Add to the vector of "poses"-"SFs" pairs:
437  CPosePDFGaussian posePDF(currentKnownRobotPose);
438  CPose3DPDF::Ptr pose3D =
439  CPose3DPDF::Ptr(CPose3DPDF::createFrom2D(posePDF));
440 
441  CSensoryFrame::Ptr sf = std::make_shared<CSensoryFrame>();
442  sf->insert(obs);
443 
444  SF_Poses_seq.insert(pose3D, sf);
445 
447  "Map updated OK. Done in "
448  << mrpt::system::formatTimeInterval(tictac.Tac()) << std::endl);
449  }
450 
451  } // end other observation
452 
453  // Robot path history:
454  {
455  TPose2D p;
456  if (m_lastPoseEst.getLatestRobotPose(p)) m_estRobotPath.push_back(p);
457  }
458 
459  MRPT_END
460 
461 } // end processObservation
462 
463 /*---------------------------------------------------------------
464 
465  processActionObservation
466 
467  ---------------------------------------------------------------*/
469  CActionCollection& action, CSensoryFrame& in_SF)
470 {
471  // 1) process action:
472  CActionRobotMovement2D::Ptr movEstimation =
473  action.getBestMovementEstimation();
474  if (movEstimation)
475  {
477  m_auxAccumOdometry, movEstimation->poseChange->getMeanVal());
478 
480  std::make_shared<CObservationOdometry>();
481  obs->timestamp = movEstimation->timestamp;
482  obs->odometry = m_auxAccumOdometry;
483  this->processObservation(obs);
484  }
485 
486  // 2) Process observations one by one:
487  for (auto& i : in_SF) this->processObservation(i);
488 }
489 
490 /*---------------------------------------------------------------
491  setCurrentMapFile
492  ---------------------------------------------------------------*/
494 {
495  // Save current map to current file:
497 
498  // Sets new current map file:
499  currentMapFile = mapFile;
500 
501  // Load map from file or create an empty one:
502  if (currentMapFile.size()) loadCurrentMapFromFile(mapFile);
503 }
504 
505 /*---------------------------------------------------------------
506  getCurrentPoseEstimation
507  ---------------------------------------------------------------*/
509 {
510  CPosePDFGaussian pdf2D;
512  pdf2D.cov = m_lastPoseEst_cov;
513 
514  CPose3DPDFGaussian::Ptr pdf3D = std::make_shared<CPose3DPDFGaussian>();
515  pdf3D->copyFrom(pdf2D);
516  return pdf3D;
517 }
518 
519 /*---------------------------------------------------------------
520  initialize
521  ---------------------------------------------------------------*/
523  const CSimpleMap& initialMap, const CPosePDF* x0)
524 {
525  MRPT_START
526 
527  // Reset vars:
528  m_estRobotPath.clear();
529  m_auxAccumOdometry = CPose2D(0, 0, 0);
530 
532  m_distSinceLastInsertion.clear();
533 
535 
536  // Init path & map:
538 
539  // Create metric maps:
541 
542  // copy map:
543  SF_Poses_seq = initialMap;
544 
545  // Load estimated pose from given PDF:
547 
548  if (x0)
550  x0->getMeanVal().asTPose(), mrpt::Clock::now());
551 
552  for (size_t i = 0; i < SF_Poses_seq.size(); i++)
553  {
554  CPose3DPDF::Ptr posePDF;
556 
557  // Get the SF and its pose:
558  SF_Poses_seq.get(i, posePDF, SF);
559 
560  CPose3D estimatedPose3D;
561  posePDF->getMean(estimatedPose3D);
562 
563  // Insert observations into the map:
564  SF->insertObservationsInto(&metricMap, &estimatedPose3D);
565  }
566 
567  MRPT_END
568 }
569 
571  std::vector<float>& x, std::vector<float>& y)
572 {
574 
575  auto pPts = metricMap.mapByClass<CPointsMap>(0);
576 
577  ASSERT_(pPts);
578  pPts->getAllPoints(x, y);
579 }
580 
581 /*---------------------------------------------------------------
582  getCurrentlyBuiltMap
583  ---------------------------------------------------------------*/
585 {
586  out_map = SF_Poses_seq;
587 }
588 
590 {
591  return &metricMap;
592 }
593 
594 /*---------------------------------------------------------------
595  getCurrentlyBuiltMapSize
596  ---------------------------------------------------------------*/
598 {
599  return SF_Poses_seq.size();
600 }
601 
602 /*---------------------------------------------------------------
603  saveCurrentEstimationToImage
604  ---------------------------------------------------------------*/
606  const std::string& file, bool formatEMF_BMP)
607 {
608  MRPT_START
609 
610  CImage img;
611  const size_t nPoses = m_estRobotPath.size();
612 
613  if (!formatEMF_BMP) THROW_EXCEPTION("Not implemented yet for BMP!");
614 
615  // grid map as bitmap:
616  auto pGrid = metricMap.mapByClass<COccupancyGridMap2D>();
617  if (pGrid) pGrid->getAsImage(img);
618 
619  // Draw paths (using vectorial plots!) over the EMF file:
620  // -------------------------------------------------
621  CEnhancedMetaFile EMF(file, 1000);
622 
623  EMF.drawImage(0, 0, img);
624 
625  unsigned int imgHeight = img.getHeight();
626 
627  // Path hypothesis:
628  // ----------------------------------
629  int x1, x2, y1, y2;
630 
631  // First point: (0,0)
632  x2 = pGrid->x2idx(0.0f);
633  y2 = pGrid->y2idx(0.0f);
634 
635  // Draw path in the bitmap:
636  for (size_t j = 0; j < nPoses; j++)
637  {
638  // For next segment
639  x1 = x2;
640  y1 = y2;
641 
642  // Coordinates -> pixels
643  x2 = pGrid->x2idx(m_estRobotPath[j].x);
644  y2 = pGrid->y2idx(m_estRobotPath[j].y);
645 
646  // Draw line:
647  EMF.line(
648  x1, imgHeight - 1 - y1, x2, imgHeight - 1 - y2, TColor::black());
649  }
650 
651  MRPT_END
652 }
653 
655  const CPose2D& new_pose)
656 {
658  for (auto& m : m_distSinceLastInsertion) m.second.updateDistances(new_pose);
659 }
660 
662  const CPose2D& new_pose)
663 {
664  m_distSinceLastICP.updatePose(new_pose);
665  for (auto& m : m_distSinceLastInsertion) m.second.updatePose(new_pose);
666 }
667 
669 {
670  const auto Ap = p - mrpt::poses::CPose2D(this->last_update);
671  lin = Ap.norm();
672  ang = std::abs(Ap.phi());
673 }
674 
676 {
677  this->last_update = p.asTPose();
678  lin = 0;
679  ang = 0;
680 }
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)
mrpt::poses::CPosePDF::Ptr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt)
The method for aligning a pair of metric maps, for SE(2) relative poses.
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.
VerbosityLevel
Enumeration of available verbosity levels.
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.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:64
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. ...
unsigned int nIterations
The number of executed iterations until convergence.
Definition: CICP.h:196
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:
T::Ptr mapByClass(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 ...
static time_point now() noexcept
Returns the current time using the currently selected Clock source.
Definition: Clock.cpp:94
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.
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...
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.
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.
LockHelper< T > lockHelper(T &t)
Syntactic sugar to easily create a locker to any kind of std::mutex.
Definition: lock_helper.h:50
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.
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:151
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:146
double goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:200
#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);
type_value getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
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:191
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:240
#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:85
mrpt::vision::TStereoCalibResults out
~CMetricMapBuilderICP() override
Destructor:
constexpr double RAD2DEG(const double x)
Radians to degrees.
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
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 (...
#define MRPT_LOG_WARN(_STRING)
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:272
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.
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.
unsigned int getCurrentlyBuiltMapSize() override
Returns just how many sensory-frames are stored in the currently build map.
std::mutex critZoneChangingMap
Critical zones.
#define MRPT_LOG_THROTTLE_WARN(_PERIOD_SECONDS, _STRING)
#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:148
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
Definition: CPointsMap.h:233
#define MRPT_LOG_INFO(_STRING)
void accumulateRobotDisplacementCounters(const mrpt::poses::CPose2D &new_pose)



Page generated by Doxygen 1.8.14 for MRPT 2.0.2 Git: 9efc2a654 Mon Apr 6 11:24:47 2020 +0200 at lun abr 6 11:30:12 CEST 2020