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  // Ensure, 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(30.0_deg),
60  localizationLinDistance(0.20),
61  localizationAngDistance(30.0_deg),
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 << "\n----------- [CMetricMapBuilderICP::TConfigParams] ------------ "
103  "\n\n";
104 
105  out << mrpt::format(
106  "insertionLinDistance = %f m\n",
107  insertionLinDistance);
108  out << mrpt::format(
109  "insertionAngDistance = %f deg\n",
110  RAD2DEG(insertionAngDistance));
111  out << mrpt::format(
112  "localizationLinDistance = %f m\n",
113  localizationLinDistance);
114  out << mrpt::format(
115  "localizationAngDistance = %f deg\n",
116  RAD2DEG(localizationAngDistance));
117  out << mrpt::format(
118  "verbosity_level = %s\n",
120  verbosity_level)
121  .c_str());
122 
123  out << " Now showing 'mapsInitializers':\n";
124  mapInitializers.dumpToTextStream(out);
125 }
126 
127 /*---------------------------------------------------------------
128  processObservation
129  This is the new entry point of the algorithm (the old one
130  was processActionObservation, which now is a wrapper to
131  this method).
132  ---------------------------------------------------------------*/
134 {
135  std::lock_guard<std::mutex> lock_cs(critZoneChangingMap);
136 
137  MRPT_START
138 
141  throw std::runtime_error(
142  "Neither grid maps nor points map: Have you called initialize() "
143  "after setting ICP_options.mapInitializers?");
144 
145  ASSERT_(obs);
146 
147  // Is it an odometry observation??
148  if (IS_CLASS(*obs, CObservationOdometry))
149  {
150  MRPT_LOG_DEBUG("processObservation(): obs is CObservationOdometry");
152 
153  const CObservationOdometry::Ptr odo =
154  std::dynamic_pointer_cast<CObservationOdometry>(obs);
155  ASSERT_(odo->timestamp != INVALID_TIMESTAMP);
156 
157  CPose2D pose_before;
158  bool pose_before_valid = m_lastPoseEst.getLatestRobotPose(pose_before);
159 
160  // Move our estimation:
162  odo->odometry.asTPose(), odo->timestamp, odo->hasVelocities,
163  odo->velocityLocal);
164 
165  if (pose_before_valid)
166  {
167  // Accumulate movement:
168  CPose2D pose_after;
169  if (m_lastPoseEst.getLatestRobotPose(pose_after))
170  this->accumulateRobotDisplacementCounters(pose_after);
172  "processObservation(): obs is CObservationOdometry, new "
173  "post_after="
174  << pose_after);
175  }
176  } // end it's odometry
177  else
178  {
179  // Current robot pose given the timestamp of the observation (this can
180  // include a small extrapolation
181  // using the latest known robot velocities):
182  TPose2D initialEstimatedRobotPose(0, 0, 0);
183  {
184  mrpt::math::TTwist2D robotVelLocal, robotVelGlobal;
185  if (obs->timestamp != INVALID_TIMESTAMP)
186  {
188  "processObservation(): extrapolating pose from latest pose "
189  "and new observation timestamp...");
191  initialEstimatedRobotPose, robotVelLocal,
192  robotVelGlobal, obs->timestamp))
193  { // couldn't had a good extrapolation estimate... we'll have
194  // to live with the latest pose:
195  m_lastPoseEst.getLatestRobotPose(initialEstimatedRobotPose);
197  10.0 /*seconds*/,
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 =
225  m_distSinceLastICP.lin < std::min(
228  m_distSinceLastICP.ang < std::min(
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;
460  if (m_lastPoseEst.getLatestRobotPose(p)) m_estRobotPath.push_back(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  // Load estimated pose from given PDF:
551 
552  if (x0)
554  x0->getMeanVal().asTPose(), mrpt::Clock::now());
555 
556  for (size_t i = 0; i < SF_Poses_seq.size(); i++)
557  {
558  CPose3DPDF::Ptr posePDF;
560 
561  // Get the SF and its pose:
562  SF_Poses_seq.get(i, posePDF, SF);
563 
564  CPose3D estimatedPose3D;
565  posePDF->getMean(estimatedPose3D);
566 
567  // Insert observations into the map:
568  SF->insertObservationsInto(&metricMap, &estimatedPose3D);
569  }
570 
571  MRPT_END
572 }
573 
575  std::vector<float>& x, std::vector<float>& y)
576 {
577  std::lock_guard<std::mutex> lck(critZoneChangingMap);
578 
579  auto pPts = metricMap.mapByClass<CPointsMap>(0);
580 
581  ASSERT_(pPts);
582  pPts->getAllPoints(x, y);
583 }
584 
585 /*---------------------------------------------------------------
586  getCurrentlyBuiltMap
587  ---------------------------------------------------------------*/
589 {
590  out_map = SF_Poses_seq;
591 }
592 
594 {
595  return &metricMap;
596 }
597 
598 /*---------------------------------------------------------------
599  getCurrentlyBuiltMapSize
600  ---------------------------------------------------------------*/
602 {
603  return SF_Poses_seq.size();
604 }
605 
606 /*---------------------------------------------------------------
607  saveCurrentEstimationToImage
608  ---------------------------------------------------------------*/
610  const std::string& file, bool formatEMF_BMP)
611 {
612  MRPT_START
613 
614  CImage img;
615  const size_t nPoses = m_estRobotPath.size();
616 
617  if (!formatEMF_BMP) THROW_EXCEPTION("Not implemented yet for BMP!");
618 
619  // grid map as bitmap:
620  auto pGrid = metricMap.mapByClass<COccupancyGridMap2D>();
621  if (pGrid) pGrid->getAsImage(img);
622 
623  // Draw paths (using vectorial plots!) over the EMF file:
624  // -------------------------------------------------
625  CEnhancedMetaFile EMF(file, 1000);
626 
627  EMF.drawImage(0, 0, img);
628 
629  unsigned int imgHeight = img.getHeight();
630 
631  // Path hypothesis:
632  // ----------------------------------
633  int x1, x2, y1, y2;
634 
635  // First point: (0,0)
636  x2 = pGrid->x2idx(0.0f);
637  y2 = pGrid->y2idx(0.0f);
638 
639  // Draw path in the bitmap:
640  for (size_t j = 0; j < nPoses; j++)
641  {
642  // For next segment
643  x1 = x2;
644  y1 = y2;
645 
646  // Coordinates -> pixels
647  x2 = pGrid->x2idx(m_estRobotPath[j].x);
648  y2 = pGrid->y2idx(m_estRobotPath[j].y);
649 
650  // Draw line:
651  EMF.line(
652  x1, imgHeight - 1 - y1, x2, imgHeight - 1 - y2, TColor::black());
653  }
654 
655  MRPT_END
656 }
657 
659  const CPose2D& new_pose)
660 {
662  for (auto& m : m_distSinceLastInsertion) m.second.updateDistances(new_pose);
663 }
664 
666  const CPose2D& new_pose)
667 {
668  m_distSinceLastICP.updatePose(new_pose);
669  for (auto& m : m_distSinceLastInsertion) m.second.updatePose(new_pose);
670 }
671 
673 {
674  const auto Ap = p - mrpt::poses::CPose2D(this->last_update);
675  lin = Ap.norm();
676  ang = std::abs(Ap.phi());
677 }
678 
680 {
681  this->last_update = p.asTPose();
682  lin = 0;
683  ang = 0;
684 }
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.
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
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
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:
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, 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.
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.
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.
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
#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)...
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:241
#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:273
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: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:234
#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: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020