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  "processObservation(): new pose extrapolation failed, "
198  "using last pose as is.");
199  }
200  }
201  else
202  {
204  "processObservation(): invalid observation timestamp.");
205  m_lastPoseEst.getLatestRobotPose(initialEstimatedRobotPose);
206  }
207  }
208 
209  // To know the total path length:
210  CPose2D previousKnownRobotPose;
211  m_lastPoseEst.getLatestRobotPose(previousKnownRobotPose);
212 
213  // Increment (this may only include the effects of extrapolation with
214  // velocity...):
216  previousKnownRobotPose); // initialEstimatedRobotPose-previousKnownRobotPose);
217 
218  // We'll skip ICP-based localization for this observation only if:
219  // - We had some odometry since the last pose correction
220  // (m_there_has_been_an_odometry=true).
221  // - AND, the traversed distance is small enough:
222  const bool we_skip_ICP_pose_correction =
224  m_distSinceLastICP.lin < std::min(
227  m_distSinceLastICP.ang < std::min(
230 
232  "processObservation(): skipping ICP pose correction due to small "
233  "odometric displacement? : "
234  << (we_skip_ICP_pose_correction ? "YES" : "NO"));
235 
236  CICP::TReturnInfo icpReturn;
237  bool can_do_icp = false;
238 
239  // Select the map to match with ....
240  CMetricMap* matchWith = nullptr;
241  if (auto pGrid = metricMap.mapByClass<COccupancyGridMap2D>();
243  {
244  matchWith = static_cast<CMetricMap*>(pGrid.get());
245  MRPT_LOG_DEBUG("processObservation(): matching against gridmap.");
246  }
247  else
248  {
249  auto pPts = metricMap.mapByClass<CPointsMap>();
250  ASSERTMSG_(pPts, "No points map in multi-metric map.");
251 
252  matchWith = static_cast<CMetricMap*>(pPts.get());
253  MRPT_LOG_DEBUG("processObservation(): matching against point map.");
254  }
255  ASSERT_(matchWith != nullptr);
256 
257  if (!we_skip_ICP_pose_correction)
258  {
260 
261  // --------------------------------------------------------------------------------------
262  // Any other observation:
263  // 1) If the observation generates points in a point map,
264  // do ICP 2) In any case, insert the observation if the
265  // minimum distance has been satisfaced.
266  // --------------------------------------------------------------------------------------
267  CSimplePointsMap sensedPoints;
268  sensedPoints.insertionOptions.minDistBetweenLaserPoints = 0.02f;
269  sensedPoints.insertionOptions.also_interpolate = false;
270 
271  // Create points representation of the observation:
272  // Insert only those planar range scans in the altitude of
273  // the grid map:
278  {
279  // Use grid altitude:
281  {
283  std::dynamic_pointer_cast<CObservation2DRangeScan>(obs);
284  if (std::abs(
287  obsLaser->sensorPose.z()) < 0.01)
288  can_do_icp = sensedPoints.insertObservationPtr(obs);
289  }
290  }
291  else
292  {
293  // Do not use grid altitude:
294  can_do_icp = sensedPoints.insertObservationPtr(obs);
295  }
296 
297  if (IS_DERIVED(*matchWith, CPointsMap) &&
298  static_cast<CPointsMap*>(matchWith)->empty())
299  can_do_icp = false; // The reference map is empty!
300 
301  if (can_do_icp)
302  {
303  // We DO HAVE points with this observation:
304  // Execute ICP over the current points map and the
305  // sensed points:
306  // ----------------------------------------------------------------------
307  CICP ICP;
308  float runningTime;
309 
310  ICP.options = ICP_params;
311 
312  CPosePDF::Ptr pestPose = ICP.Align(
313  matchWith, // Map 1
314  &sensedPoints, // Map 2
316  initialEstimatedRobotPose), // a first gross
317  // estimation
318  // of map 2 relative to map
319  // 1.
320  &runningTime, // Running time
321  &icpReturn // Returned information
322  );
323 
325  {
326  // save estimation:
327  CPosePDFGaussian pEst2D;
328  pEst2D.copyFrom(*pestPose);
329 
331  pEst2D.mean.asTPose(), obs->timestamp);
332  m_lastPoseEst_cov = pEst2D.cov;
333 
335 
336  // Debug output to console:
338  "processObservation: previousPose="
339  << previousKnownRobotPose << "-> currentPose="
340  << pEst2D.getMeanVal() << std::endl);
342  "[CMetricMapBuilderICP] Fit:%.1f%% Itr:%i In "
343  "%.02fms \n",
344  icpReturn.goodness * 100, icpReturn.nIterations,
345  1000 * runningTime));
346  }
347  else
348  {
350  "Ignoring ICP of low quality: "
351  << icpReturn.goodness * 100 << std::endl);
352  }
353 
354  // Compute the transversed length:
355  CPose2D currentKnownRobotPose;
356  m_lastPoseEst.getLatestRobotPose(currentKnownRobotPose);
357 
359  currentKnownRobotPose); // currentKnownRobotPose -
360  // previousKnownRobotPose);
361 
362  } // end we can do ICP.
363  else
364  {
366  "Cannot do ICP: empty pointmap or not suitable "
367  "gridmap...\n");
368  }
369 
370  } // else, we do ICP pose correction
371 
372  // ----------------------------------------------------------
373  // CRITERION TO DECIDE MAP UPDATE:
374  // A distance large-enough from the last update for each
375  // sensor, AND
376  // either: (i) this was a good match or (ii) this is the
377  // first time for this sensor.
378  // ----------------------------------------------------------
379  const bool firstTimeForThisSensor =
380  m_distSinceLastInsertion.find(obs->sensorLabel) ==
382  bool update =
383  firstTimeForThisSensor ||
384  ((!can_do_icp ||
386  (m_distSinceLastInsertion[obs->sensorLabel].lin >=
388  m_distSinceLastInsertion[obs->sensorLabel].ang >=
390 
391  // Used any "options.alwaysInsertByClass" ??
392  if (options.alwaysInsertByClass.contains(obs->GetRuntimeClass()))
393  update = true;
394 
395  // We need to always insert ALL the observations at the
396  // beginning until the first one
397  // that actually insert some points into the map used as a
398  // reference, since otherwise we'll not be able to do ICP
399  // against an empty map!!
400  if (matchWith && matchWith->isEmpty()) update = true;
401 
403  "update map: " << (update ? "YES" : "NO")
404  << " options.enableMapUpdating: "
405  << (options.enableMapUpdating ? "YES" : "NO"));
406 
407  if (options.enableMapUpdating && update)
408  {
409  CTicTac tictac;
410 
411  tictac.Tic();
412 
413  // Insert the observation:
414  CPose2D currentKnownRobotPose;
415  m_lastPoseEst.getLatestRobotPose(currentKnownRobotPose);
416 
417  // Create new entry:
418  m_distSinceLastInsertion[obs->sensorLabel].last_update =
419  currentKnownRobotPose.asTPose();
420 
421  // Reset distance counters:
422  resetRobotDisplacementCounters(currentKnownRobotPose);
423  // m_distSinceLastInsertion[obs->sensorLabel].updatePose(currentKnownRobotPose);
424 
426  "Updating map from pose %s\n",
427  currentKnownRobotPose.asString().c_str()));
428 
429  CPose3D estimatedPose3D(currentKnownRobotPose);
430  const bool anymap_update =
431  metricMap.insertObservationPtr(obs, &estimatedPose3D);
432  if (!anymap_update)
434  "**No map was updated** after inserting an "
435  "observation of "
436  "type `"
437  << obs->GetRuntimeClass()->className << "`");
438 
439  // Add to the vector of "poses"-"SFs" pairs:
440  CPosePDFGaussian posePDF(currentKnownRobotPose);
441  CPose3DPDF::Ptr pose3D =
442  CPose3DPDF::Ptr(CPose3DPDF::createFrom2D(posePDF));
443 
444  CSensoryFrame::Ptr sf = std::make_shared<CSensoryFrame>();
445  sf->insert(obs);
446 
447  SF_Poses_seq.insert(pose3D, sf);
448 
450  "Map updated OK. Done in "
451  << mrpt::system::formatTimeInterval(tictac.Tac()) << std::endl);
452  }
453 
454  } // end other observation
455 
456  // Robot path history:
457  {
458  TPose2D p;
460  }
461 
462  MRPT_END
463 
464 } // end processObservation
465 
466 /*---------------------------------------------------------------
467 
468  processActionObservation
469 
470  ---------------------------------------------------------------*/
472  CActionCollection& action, CSensoryFrame& in_SF)
473 {
474  // 1) process action:
475  CActionRobotMovement2D::Ptr movEstimation =
476  action.getBestMovementEstimation();
477  if (movEstimation)
478  {
480  m_auxAccumOdometry, movEstimation->poseChange->getMeanVal());
481 
483  std::make_shared<CObservationOdometry>();
484  obs->timestamp = movEstimation->timestamp;
485  obs->odometry = m_auxAccumOdometry;
486  this->processObservation(obs);
487  }
488 
489  // 2) Process observations one by one:
490  for (auto& i : in_SF) this->processObservation(i);
491 }
492 
493 /*---------------------------------------------------------------
494  setCurrentMapFile
495  ---------------------------------------------------------------*/
497 {
498  // Save current map to current file:
500 
501  // Sets new current map file:
502  currentMapFile = mapFile;
503 
504  // Load map from file or create an empty one:
505  if (currentMapFile.size()) loadCurrentMapFromFile(mapFile);
506 }
507 
508 /*---------------------------------------------------------------
509  getCurrentPoseEstimation
510  ---------------------------------------------------------------*/
512 {
513  CPosePDFGaussian pdf2D;
515  pdf2D.cov = m_lastPoseEst_cov;
516 
517  CPose3DPDFGaussian::Ptr pdf3D = std::make_shared<CPose3DPDFGaussian>();
518  pdf3D->copyFrom(pdf2D);
519  return pdf3D;
520 }
521 
522 /*---------------------------------------------------------------
523  initialize
524  ---------------------------------------------------------------*/
526  const CSimpleMap& initialMap, const CPosePDF* x0)
527 {
528  MRPT_START
529 
530  // Reset vars:
531  m_estRobotPath.clear();
532  m_auxAccumOdometry = CPose2D(0, 0, 0);
533 
535  m_distSinceLastInsertion.clear();
536 
538 
539  // Init path & map:
540  std::lock_guard<std::mutex> lock_cs(critZoneChangingMap);
541 
542  // Create metric maps:
544 
545  // copy map:
546  SF_Poses_seq = initialMap;
547 
548  // Load estimated pose from given PDF:
550 
551  if (x0)
553  x0->getMeanVal().asTPose(), mrpt::Clock::now());
554 
555  for (size_t i = 0; i < SF_Poses_seq.size(); i++)
556  {
557  CPose3DPDF::Ptr posePDF;
559 
560  // Get the SF and its pose:
561  SF_Poses_seq.get(i, posePDF, SF);
562 
563  CPose3D estimatedPose3D;
564  posePDF->getMean(estimatedPose3D);
565 
566  // Insert observations into the map:
567  SF->insertObservationsInto(&metricMap, &estimatedPose3D);
568  }
569 
570  MRPT_END
571 }
572 
574  std::vector<float>& x, std::vector<float>& y)
575 {
576  std::lock_guard<std::mutex> lck(critZoneChangingMap);
577 
578  auto pPts = metricMap.mapByClass<CPointsMap>(0);
579 
580  ASSERT_(pPts);
581  pPts->getAllPoints(x, y);
582 }
583 
584 /*---------------------------------------------------------------
585  getCurrentlyBuiltMap
586  ---------------------------------------------------------------*/
588 {
589  out_map = SF_Poses_seq;
590 }
591 
593 {
594  return &metricMap;
595 }
596 
597 /*---------------------------------------------------------------
598  getCurrentlyBuiltMapSize
599  ---------------------------------------------------------------*/
601 {
602  return SF_Poses_seq.size();
603 }
604 
605 /*---------------------------------------------------------------
606  saveCurrentEstimationToImage
607  ---------------------------------------------------------------*/
609  const std::string& file, bool formatEMF_BMP)
610 {
611  MRPT_START
612 
613  CImage img;
614  const size_t nPoses = m_estRobotPath.size();
615 
616  if (!formatEMF_BMP) THROW_EXCEPTION("Not implemented yet for BMP!");
617 
618  // grid map as bitmap:
619  auto pGrid = metricMap.mapByClass<COccupancyGridMap2D>();
620  if (pGrid) pGrid->getAsImage(img);
621 
622  // Draw paths (using vectorial plots!) over the EMF file:
623  // -------------------------------------------------
624  CEnhancedMetaFile EMF(file, 1000);
625 
626  EMF.drawImage(0, 0, img);
627 
628  unsigned int imgHeight = img.getHeight();
629 
630  // Path hypothesis:
631  // ----------------------------------
632  int x1, x2, y1, y2;
633 
634  // First point: (0,0)
635  x2 = pGrid->x2idx(0.0f);
636  y2 = pGrid->y2idx(0.0f);
637 
638  // Draw path in the bitmap:
639  for (size_t j = 0; j < nPoses; j++)
640  {
641  // For next segment
642  x1 = x2;
643  y1 = y2;
644 
645  // Coordinates -> pixels
646  x2 = pGrid->x2idx(m_estRobotPath[j].x);
647  y2 = pGrid->y2idx(m_estRobotPath[j].y);
648 
649  // Draw line:
650  EMF.line(
651  x1, imgHeight - 1 - y1, x2, imgHeight - 1 - y2, TColor::black());
652  }
653 
654  MRPT_END
655 }
656 
658  const CPose2D& new_pose)
659 {
661  for (auto& m : m_distSinceLastInsertion) m.second.updateDistances(new_pose);
662 }
663 
665  const CPose2D& new_pose)
666 {
667  m_distSinceLastICP.updatePose(new_pose);
668  for (auto& m : m_distSinceLastInsertion) m.second.updatePose(new_pose);
669 }
670 
672 {
673  const auto Ap = p - mrpt::poses::CPose2D(this->last_update);
674  lin = Ap.norm();
675  ang = std::abs(Ap.phi());
676 }
677 
679 {
680  this->last_update = p.asTPose();
681  lin = 0;
682  ang = 0;
683 }
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:
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: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)...
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: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
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:
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
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: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.
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: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: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019