Main MRPT website > C++ reference for MRPT 1.9.9
CMetricMapBuilderICP.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "slam-precomp.h" // Precompiled headers
11 
12 #include <mrpt/slam/CICP.h>
18 #include <mrpt/system/CTicTac.h>
20 
21 using namespace std;
22 using namespace mrpt::slam;
23 using namespace mrpt::obs;
24 using namespace mrpt::maps;
25 using namespace mrpt::poses;
26 using namespace mrpt::img;
27 using namespace mrpt::math;
28 using namespace mrpt::system;
29 
30 CMetricMapBuilderICP::CMetricMapBuilderICP()
31  : ICP_options(m_min_verbosity_level)
32 {
33  this->setLoggerName("CMetricMapBuilderICP");
34  this->initialize(CSimpleMap());
35 }
36 
37 /*---------------------------------------------------------------
38  Destructor
39  ---------------------------------------------------------------*/
41 {
42  // Asure, we have exit all critical zones:
45 
46  // Save current map to current file:
48 }
49 
50 /*---------------------------------------------------------------
51  Options
52  ---------------------------------------------------------------*/
54  mrpt::system::VerbosityLevel& parent_verbosity_level)
55  : matchAgainstTheGrid(false),
56  insertionLinDistance(1.0),
57  insertionAngDistance(DEG2RAD(30)),
58  localizationLinDistance(0.20),
59  localizationAngDistance(DEG2RAD(30)),
60  minICPgoodnessToAccept(0.40),
61  verbosity_level(parent_verbosity_level),
62  mapInitializers()
63 {
64 }
65 
68 {
69  matchAgainstTheGrid = other.matchAgainstTheGrid;
70  insertionLinDistance = other.insertionLinDistance;
71  insertionAngDistance = other.insertionAngDistance;
72  localizationLinDistance = other.localizationLinDistance;
73  localizationAngDistance = other.localizationAngDistance;
74  minICPgoodnessToAccept = other.minICPgoodnessToAccept;
75  // We can't copy a reference type
76  // verbosity_level = other.verbosity_level;
77  mapInitializers = other.mapInitializers;
78  return *this;
79 }
80 
82  const mrpt::config::CConfigFileBase& source, const std::string& section)
83 {
84  MRPT_LOAD_CONFIG_VAR(matchAgainstTheGrid, bool, source, section)
85  MRPT_LOAD_CONFIG_VAR(insertionLinDistance, double, source, section)
86  MRPT_LOAD_CONFIG_VAR_DEGREES(insertionAngDistance, source, section)
87  MRPT_LOAD_CONFIG_VAR(localizationLinDistance, double, source, section)
88  MRPT_LOAD_CONFIG_VAR_DEGREES(localizationAngDistance, source, section)
89  verbosity_level = source.read_enum<mrpt::system::VerbosityLevel>(
90  section, "verbosity_level", verbosity_level);
91 
92  MRPT_LOAD_CONFIG_VAR(minICPgoodnessToAccept, double, source, section)
93 
94  mapInitializers.loadFromConfigFile(source, section);
95 }
96 
98  std::ostream& out) const
99 {
100  out << mrpt::format(
101  "\n----------- [CMetricMapBuilderICP::TConfigParams] ------------ "
102  "\n\n");
103 
104  out << mrpt::format(
105  "insertionLinDistance = %f m\n",
106  insertionLinDistance);
107  out << mrpt::format(
108  "insertionAngDistance = %f deg\n",
109  RAD2DEG(insertionAngDistance));
110  out << mrpt::format(
111  "localizationLinDistance = %f m\n",
112  localizationLinDistance);
113  out << mrpt::format(
114  "localizationAngDistance = %f deg\n",
115  RAD2DEG(localizationAngDistance));
116  out << mrpt::format(
117  "verbosity_level = %s\n",
119  verbosity_level)
120  .c_str());
121 
122  out << mrpt::format(" Now showing 'mapsInitializers':\n");
123  mapInitializers.dumpToTextStream(out);
124 }
125 
126 /*---------------------------------------------------------------
127  processObservation
128  This is the new entry point of the algorithm (the old one
129  was processActionObservation, which now is a wrapper to
130  this method).
131  ---------------------------------------------------------------*/
133 {
134  std::lock_guard<std::mutex> lock_cs(critZoneChangingMap);
135 
136  MRPT_START
137 
138  if (metricMap.m_pointsMaps.empty() && metricMap.m_gridMaps.empty())
139  throw std::runtime_error(
140  "Neither grid maps nor points map: Have you called initialize() "
141  "after setting ICP_options.mapInitializers?");
142 
143  ASSERT_(obs);
144 
145  // Is it an odometry observation??
146  if (IS_CLASS(obs, CObservationOdometry))
147  {
148  MRPT_LOG_DEBUG("processObservation(): obs is CObservationOdometry");
150 
151  const CObservationOdometry::Ptr odo =
152  std::dynamic_pointer_cast<CObservationOdometry>(obs);
153  ASSERT_(odo->timestamp != INVALID_TIMESTAMP);
154 
155  CPose2D pose_before;
156  bool pose_before_valid = m_lastPoseEst.getLatestRobotPose(pose_before);
157 
158  // Move our estimation:
160  odo->odometry.asTPose(), odo->timestamp, odo->hasVelocities,
161  odo->velocityLocal);
162 
163  if (pose_before_valid)
164  {
165  // Accumulate movement:
166  CPose2D pose_after;
167  if (m_lastPoseEst.getLatestRobotPose(pose_after))
168  this->accumulateRobotDisplacementCounters(pose_after);
170  "processObservation(): obs is CObservationOdometry, new "
171  "post_after="
172  << pose_after);
173  }
174  } // end it's odometry
175  else
176  {
177  // Current robot pose given the timestamp of the observation (this can
178  // include a small extrapolation
179  // using the latest known robot velocities):
180  TPose2D initialEstimatedRobotPose(0, 0, 0);
181  {
182  mrpt::math::TTwist2D robotVelLocal, robotVelGlobal;
183  if (obs->timestamp != INVALID_TIMESTAMP)
184  {
186  "processObservation(): extrapolating pose from latest pose "
187  "and new observation timestamp...");
189  initialEstimatedRobotPose, robotVelLocal,
190  robotVelGlobal, obs->timestamp))
191  { // couldn't had a good extrapolation estimate... we'll have
192  // to live with the latest pose:
193  m_lastPoseEst.getLatestRobotPose(initialEstimatedRobotPose);
195  "processObservation(): new pose extrapolation failed, "
196  "using last pose as is.");
197  }
198  }
199  else
200  {
202  "processObservation(): invalid observation timestamp.");
203  m_lastPoseEst.getLatestRobotPose(initialEstimatedRobotPose);
204  }
205  }
206 
207  // To know the total path length:
208  CPose2D previousKnownRobotPose;
209  m_lastPoseEst.getLatestRobotPose(previousKnownRobotPose);
210 
211  // Increment (this may only include the effects of extrapolation with
212  // velocity...):
214  previousKnownRobotPose); // initialEstimatedRobotPose-previousKnownRobotPose);
215 
216  // We'll skip ICP-based localization for this observation only if:
217  // - We had some odometry since the last pose correction
218  // (m_there_has_been_an_odometry=true).
219  // - AND, the traversed distance is small enough:
220  const bool we_skip_ICP_pose_correction =
228 
230  "processObservation(): skipping ICP pose correction due to small "
231  "odometric displacement? : "
232  << (we_skip_ICP_pose_correction ? "YES" : "NO"));
233 
234  CICP::TReturnInfo icpReturn;
235  bool can_do_icp = false;
236 
237  // Select the map to match with ....
238  CMetricMap* matchWith = nullptr;
240  {
241  matchWith = static_cast<CMetricMap*>(metricMap.m_gridMaps[0].get());
242  MRPT_LOG_DEBUG("processObservation(): matching against gridmap.");
243  }
244  else
245  {
246  ASSERTMSG_(
247  metricMap.m_pointsMaps.size(),
248  "No points map in multi-metric map.");
249  matchWith =
250  static_cast<CMetricMap*>(metricMap.m_pointsMaps[0].get());
251  MRPT_LOG_DEBUG("processObservation(): matching against point map.");
252  }
253  ASSERT_(matchWith != nullptr);
254 
255  if (!we_skip_ICP_pose_correction)
256  {
258 
259  // --------------------------------------------------------------------------------------
260  // Any other observation:
261  // 1) If the observation generates points in a point map, do ICP
262  // 2) In any case, insert the observation if the minimum distance
263  // has been satisfaced.
264  // --------------------------------------------------------------------------------------
265  CSimplePointsMap sensedPoints;
266  sensedPoints.insertionOptions.minDistBetweenLaserPoints = 0.02f;
267  sensedPoints.insertionOptions.also_interpolate = false;
268 
269  // Create points representation of the observation:
270  // Insert only those planar range scans in the altitude of the grid
271  // map:
273  !metricMap.m_gridMaps.empty() &&
274  metricMap.m_gridMaps[0]->insertionOptions.useMapAltitude)
275  {
276  // Use grid altitude:
278  {
280  std::dynamic_pointer_cast<CObservation2DRangeScan>(obs);
281  if (std::abs(
283  ->insertionOptions.mapAltitude -
284  obsLaser->sensorPose.z()) < 0.01)
285  can_do_icp = sensedPoints.insertObservationPtr(obs);
286  }
287  }
288  else
289  {
290  // Do not use grid altitude:
291  can_do_icp = sensedPoints.insertObservationPtr(obs);
292  }
293 
294  if (IS_DERIVED(matchWith, CPointsMap) &&
295  static_cast<CPointsMap*>(matchWith)->empty())
296  can_do_icp = false; // The reference map is empty!
297 
298  if (can_do_icp)
299  {
300  // We DO HAVE points with this observation:
301  // Execute ICP over the current points map and the sensed
302  // points:
303  // ----------------------------------------------------------------------
304  CICP ICP;
305  float runningTime;
306 
307  ICP.options = ICP_params;
308 
309  CPosePDF::Ptr pestPose = ICP.Align(
310  matchWith, // Map 1
311  &sensedPoints, // Map 2
313  initialEstimatedRobotPose), // a first gross estimation
314  // of map 2 relative to map
315  // 1.
316  &runningTime, // Running time
317  &icpReturn // Returned information
318  );
319 
321  {
322  // save estimation:
323  CPosePDFGaussian pEst2D;
324  pEst2D.copyFrom(*pestPose);
325 
327  pEst2D.mean.asTPose(), obs->timestamp);
328  m_lastPoseEst_cov = pEst2D.cov;
329 
331 
332  // Debug output to console:
334  "processObservation: previousPose="
335  << previousKnownRobotPose << "-> currentPose="
336  << pEst2D.getMeanVal() << std::endl);
338  format(
339  "[CMetricMapBuilderICP] Fit:%.1f%% Itr:%i In "
340  "%.02fms \n",
341  icpReturn.goodness * 100, icpReturn.nIterations,
342  1000 * runningTime));
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 sensor, AND
372  // either: (i) this was a good match or (ii) this is the first time
373  // for this sensor.
374  // ----------------------------------------------------------
375  const bool firstTimeForThisSensor =
376  m_distSinceLastInsertion.find(obs->sensorLabel) ==
378  bool update =
379  firstTimeForThisSensor ||
380  ((!can_do_icp ||
382  (m_distSinceLastInsertion[obs->sensorLabel].lin >=
384  m_distSinceLastInsertion[obs->sensorLabel].ang >=
386 
387  // Used any "options.alwaysInsertByClass" ??
388  if (options.alwaysInsertByClass.contains(obs->GetRuntimeClass()))
389  update = true;
390 
391  // We need to always insert ALL the observations at the beginning until
392  // the first one
393  // that actually insert some points into the map used as a reference,
394  // since otherwise
395  // we'll not be able to do ICP against an empty map!!
396  if (matchWith && matchWith->isEmpty()) update = true;
397 
399  "update map: " << (update ? "YES" : "NO")
400  << " options.enableMapUpdating: "
401  << (options.enableMapUpdating ? "YES" : "NO"));
402 
403  if (options.enableMapUpdating && update)
404  {
405  CTicTac tictac;
406 
407  tictac.Tic();
408 
409  // Insert the observation:
410  CPose2D currentKnownRobotPose;
411  m_lastPoseEst.getLatestRobotPose(currentKnownRobotPose);
412 
413  // Create new entry:
414  m_distSinceLastInsertion[obs->sensorLabel].last_update =
415  currentKnownRobotPose;
416 
417  // Reset distance counters:
418  resetRobotDisplacementCounters(currentKnownRobotPose);
419  // m_distSinceLastInsertion[obs->sensorLabel].updatePose(currentKnownRobotPose);
420 
422  mrpt::format(
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 observation of "
432  "type `"
433  << obs->GetRuntimeClass()->className << "`");
434 
435  // Add to the vector of "poses"-"SFs" pairs:
436  CPosePDFGaussian posePDF(currentKnownRobotPose);
437  CPose3DPDF::Ptr pose3D =
438  CPose3DPDF::Ptr(CPose3DPDF::createFrom2D(posePDF));
439 
440  CSensoryFrame::Ptr sf = mrpt::make_aligned_shared<CSensoryFrame>();
441  sf->insert(obs);
442 
443  SF_Poses_seq.insert(pose3D, sf);
444 
446  "Map updated OK. Done in "
447  << mrpt::system::formatTimeInterval(tictac.Tac()) << std::endl);
448  }
449 
450  } // end other observation
451 
452  // Robot path history:
453  {
454  TPose2D p;
456  }
457 
458  MRPT_END
459 
460 } // end processObservation
461 
462 /*---------------------------------------------------------------
463 
464  processActionObservation
465 
466  ---------------------------------------------------------------*/
468  CActionCollection& action, CSensoryFrame& in_SF)
469 {
470  // 1) process action:
471  CActionRobotMovement2D::Ptr movEstimation =
472  action.getBestMovementEstimation();
473  if (movEstimation)
474  {
476  m_auxAccumOdometry, movEstimation->poseChange->getMeanVal());
477 
479  mrpt::make_aligned_shared<CObservationOdometry>();
480  obs->timestamp = movEstimation->timestamp;
481  obs->odometry = m_auxAccumOdometry;
482  this->processObservation(obs);
483  }
484 
485  // 2) Process observations one by one:
486  for (CSensoryFrame::iterator i = in_SF.begin(); i != in_SF.end(); ++i)
487  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 
515  mrpt::make_aligned_shared<CPose3DPDFGaussian>();
516  pdf3D->copyFrom(pdf2D);
517  return pdf3D;
518 }
519 
520 /*---------------------------------------------------------------
521  initialize
522  ---------------------------------------------------------------*/
524  const CSimpleMap& initialMap, const CPosePDF* x0)
525 {
526  MRPT_START
527 
528  // Reset vars:
529  m_estRobotPath.clear();
530  m_auxAccumOdometry = CPose2D(0, 0, 0);
531 
533  m_distSinceLastInsertion.clear();
534 
536 
537  // Init path & map:
538  std::lock_guard<std::mutex> lock_cs(critZoneChangingMap);
539 
540  // Create metric maps:
542 
543  // copy map:
544  SF_Poses_seq = initialMap;
545 
546  // Parse SFs to the hybrid map:
547  // Set options:
548  // ---------------------
549  // if (metricMap.m_pointsMaps.size())
550  //{
551  // metricMap.m_pointsMaps[0]->insertionOptions.fuseWithExisting =
552  // false;
553  // metricMap.m_pointsMaps[0]->insertionOptions.minDistBetweenLaserPoints =
554  // 0.05f;
555  // metricMap.m_pointsMaps[0]->insertionOptions.disableDeletion =
556  // true;
557  // metricMap.m_pointsMaps[0]->insertionOptions.isPlanarMap =
558  // true;
559  // metricMap.m_pointsMaps[0]->insertionOptions.matchStaticPointsOnly =
560  // true;
561  //}
562 
563  // Load estimated pose from given PDF:
565 
566  if (x0)
569 
570  for (size_t i = 0; i < SF_Poses_seq.size(); i++)
571  {
572  CPose3DPDF::Ptr posePDF;
574 
575  // Get the SF and its pose:
576  SF_Poses_seq.get(i, posePDF, SF);
577 
578  CPose3D estimatedPose3D;
579  posePDF->getMean(estimatedPose3D);
580 
581  // Insert observations into the map:
582  SF->insertObservationsInto(&metricMap, &estimatedPose3D);
583  }
584 
585  MRPT_LOG_INFO("loadCurrentMapFromFile() OK.\n");
586 
587  MRPT_END
588 }
589 
590 /*---------------------------------------------------------------
591  getCurrentMapPoints
592  ---------------------------------------------------------------*/
594  std::vector<float>& x, std::vector<float>& y)
595 {
596  // Critical section: We are using our global metric map
598 
599  ASSERT_(metricMap.m_pointsMaps.size() > 0);
600  metricMap.m_pointsMaps[0]->getAllPoints(x, y);
601 
602  // Exit critical zone.
604 }
605 
606 /*---------------------------------------------------------------
607  getCurrentlyBuiltMap
608  ---------------------------------------------------------------*/
610 {
611  out_map = SF_Poses_seq;
612 }
613 
615 {
616  return &metricMap;
617 }
618 
619 /*---------------------------------------------------------------
620  getCurrentlyBuiltMapSize
621  ---------------------------------------------------------------*/
623 {
624  return SF_Poses_seq.size();
625 }
626 
627 /*---------------------------------------------------------------
628  saveCurrentEstimationToImage
629  ---------------------------------------------------------------*/
631  const std::string& file, bool formatEMF_BMP)
632 {
633  MRPT_START
634 
635  CImage img;
636  const size_t nPoses = m_estRobotPath.size();
637 
638  ASSERT_(metricMap.m_gridMaps.size() > 0);
639 
640  if (!formatEMF_BMP) THROW_EXCEPTION("Not implemented yet for BMP!");
641 
642  // grid map as bitmap:
643  // ----------------------------------
644  metricMap.m_gridMaps[0]->getAsImage(img);
645 
646  // Draw paths (using vectorial plots!) over the EMF file:
647  // -------------------------------------------------
648  // float SCALE = 1000;
649  CEnhancedMetaFile EMF(file, 1000);
650 
651  EMF.drawImage(0, 0, img);
652 
653  unsigned int imgHeight = img.getHeight();
654 
655  // Path hypothesis:
656  // ----------------------------------
657  int x1, x2, y1, y2;
658 
659  // First point: (0,0)
660  x2 = metricMap.m_gridMaps[0]->x2idx(0.0f);
661  y2 = metricMap.m_gridMaps[0]->y2idx(0.0f);
662 
663  // Draw path in the bitmap:
664  for (size_t j = 0; j < nPoses; j++)
665  {
666  // For next segment
667  x1 = x2;
668  y1 = y2;
669 
670  // Coordinates -> pixels
671  x2 = metricMap.m_gridMaps[0]->x2idx(m_estRobotPath[j].x);
672  y2 = metricMap.m_gridMaps[0]->y2idx(m_estRobotPath[j].y);
673 
674  // Draw line:
675  EMF.line(
676  x1, imgHeight - 1 - y1, x2, imgHeight - 1 - y2, TColor::black());
677  }
678 
679  MRPT_END
680 }
681 
683  const CPose2D& new_pose)
684 {
686  for (auto& m : m_distSinceLastInsertion) m.second.updateDistances(new_pose);
687 }
688 
690  const CPose2D& new_pose)
691 {
692  m_distSinceLastICP.updatePose(new_pose);
693  for (auto& m : m_distSinceLastInsertion) m.second.updatePose(new_pose);
694 }
695 
697 {
698  mrpt::poses::CPose2D Ap = p - this->last_update;
699  lin = Ap.norm();
700  ang = std::abs(Ap.phi());
701 }
702 
704 {
705  this->last_update = p;
706  lin = 0;
707  ang = 0;
708 }
void saveCurrentMapToFile(const std::string &fileName, bool compressGZ=true) const
Save map (mrpt::maps::CSimpleMap) to a ".simplemap" file.
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:90
This class represents a Windows Enhanced Meta File (EMF) for generating and saving graphics...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
EIGEN_STRONG_INLINE bool empty() const
double localizationAngDistance
Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be u...
void updatePose(const mrpt::poses::CPose2D &p)
std::deque< CObservation::Ptr >::iterator iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
#define MRPT_START
Definition: exceptions.h:262
#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:418
mrpt::maps::TSetOfMetricMapInitializers mapInitializers
What maps to create (at least one points map and/or a grid map are needed).
#define min(a, b)
VerbosityLevel
Enumeration of available verbosity levels.
double RAD2DEG(const double x)
Radians to degrees.
mrpt::maps::CMultiMetricMap metricMap
The metric map representation as a points map:
ProxyFilterContainerByClass< mrpt::maps::CSimplePointsMap::Ptr, TListMaps > m_pointsMaps
STL-like proxy to access this kind of maps in maps.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
double minICPgoodnessToAccept
Minimum ICP goodness (0,1) to accept the resulting corrected position (default: 0.40)
void getCurrentMapPoints(std::vector< float > &x, std::vector< float > &y)
Returns the 2D points of current local map.
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:35
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:67
double DEG2RAD(const double x)
Degrees to radians.
mrpt::rtti::CListOfClasses alwaysInsertByClass
A list of observation classes (derived from mrpt::obs::CObservation) which will be always inserted in...
std::deque< mrpt::math::TPose2D > m_estRobotPath
The estimated robot path:
void leaveCriticalSection()
Leave critical section for map updating.
void drawImage(int x, int y, const mrpt::img::CImage &img) override
Draws an image as a bitmap at a given position.
virtual ~CMetricMapBuilderICP()
Destructor:
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.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:75
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
TDATA getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
std::string formatTimeInterval(const double timeSeconds)
Returns a formated string with the given time difference (passed as the number of seconds)...
Definition: datetime.cpp:232
mrpt::maps::CSimpleMap SF_Poses_seq
The set of observations that leads to current map:
TConfigParams options
The options employed by the ICP align.
Definition: CICP.h:182
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:441
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)
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage: ...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
CICP::TConfigParams ICP_params
Options for the ICP algorithm itself.
#define IS_DERIVED(ptrObj, class_name)
Evaluates to true if a pointer to an object (derived from mrpt::rtti::CObject) is an instance of the ...
Definition: CObject.h:109
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:199
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:64
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)
void processUpdateNewPoseLocalization(const mrpt::math::TPose2D &newPose, mrpt::system::TTimeStamp tim)
Updates the filter with new global-coordinates localization data from a localization or SLAM source...
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:253
std::string currentMapFile
Current map file.
A helper class that can convert an enum value into its textual representation, and viceversa...
GLint GLvoid * img
Definition: glext.h:3763
void line(int x0, int y0, int x1, int y1, const mrpt::img::TColor color, unsigned int width=1, TPenStyle penStyle=psSolid) override
Draws a line.
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const override
Returns the map built so far.
void setLoggerName(const std::string &name)
Set the name of the COutputLogger instance.
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
ProxyFilterContainerByClass< mrpt::maps::COccupancyGridMap2D::Ptr, TListMaps > m_gridMaps
STL-like proxy to access this kind of maps in maps.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
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:196
GLsizei const GLchar ** string
Definition: glext.h:4101
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:41
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:15
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...
void processUpdateNewOdometry(const mrpt::math::TPose2D &newGlobalOdometry, mrpt::system::TTimeStamp cur_tim, bool hasVelocities=false, const mrpt::math::TTwist2D &newRobotVelLocal=mrpt::math::TTwist2D())
Updates the filter with new odometry readings.
bool also_interpolate
If set to true, far points (<1m) are interpolated with samples at "minDistSqrBetweenLaserPoints" inte...
Definition: CPointsMap.h:225
#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)
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.
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:56
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers *initializers)
Sets the list of internal map according to the passed list of map initializers (Current maps&#39; content...
bool getCurrentEstimate(mrpt::math::TPose2D &pose, mrpt::math::TTwist2D &velLocal, mrpt::math::TTwist2D &velGlobal, mrpt::system::TTimeStamp tim_query=mrpt::system::now()) const
Get the estimate for a given timestamp (defaults to now()), obtained as:
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:82
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:103
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:111
#define MRPT_END
Definition: exceptions.h:266
The ICP algorithm return information.
Definition: CICP.h:192
bool insertObservationPtr(const mrpt::obs::CObservation::Ptr &obs, const mrpt::poses::CPose3D *robotPose=NULL)
A wrapper for smart pointers, just calls the non-smart pointer version.
Definition: CMetricMap.cpp:109
Lightweight 2D pose.
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
void processObservation(const mrpt::obs::CObservation::Ptr &obs)
The main method of this class: Process one odometry or sensor observation.
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:3538
#define MRPT_LOG_WARN(_STRING)
mrpt::aligned_std_map< std::string, TDist > m_distSinceLastInsertion
Indexed by sensor label.
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:257
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:3538
const_iterator end() const
Returns a constant iterator to the end of the list of observations: this is an example of usage: ...
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:79
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:136
virtual bool isEmpty() const =0
Returns true if the map is empty/no observation has been inserted.
GLfloat GLfloat p
Definition: glext.h:6305
unsigned int getCurrentlyBuiltMapSize() override
Returns just how many sensory-frames are stored in the currently build map.
std::mutex critZoneChangingMap
Critical zones.
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...
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:130
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
Definition: CPointsMap.h:218
#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: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019