MRPT  2.0.2
ICP_SLAM_App.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "apps-precomp.h" // Precompiled headers
11 
12 #include <mrpt/apps/ICP_SLAM_App.h>
21 #include <mrpt/opengl/CPlanarLaserScan.h> // from lib [mrpt-maps]
25 #include <mrpt/system/CRateTimer.h>
26 #include <mrpt/system/filesystem.h>
27 #include <mrpt/system/memory.h>
28 #include <mrpt/system/os.h>
29 
30 using namespace mrpt::apps;
31 
32 constexpr auto sect = "MappingApplication";
33 
34 // ---------------------------------------
35 // ICP_SLAM_App_Base
36 // ---------------------------------------
38 {
39  // Set logger display name:
40  this->setLoggerName("ICP_SLAM_App");
41 }
42 
43 void ICP_SLAM_App_Base::initialize(int argc, const char** argv)
44 {
46 
48  " icp-slam - Part of the MRPT\n"
49  " MRPT C++ Library: %s - Sources timestamp: %s\n\n",
52 
53  // Process arguments:
54  if (argc < 2)
55  {
56  THROW_EXCEPTION_FMT("Usage: %s", impl_get_usage().c_str());
57  }
58 
59  // Config file:
60  const std::string configFile = std::string(argv[1]);
61 
62  ASSERT_FILE_EXISTS_(configFile);
64 
66 
67  MRPT_END
68 }
69 
71 {
73 
74  using namespace mrpt;
75  using namespace mrpt::slam;
76  using namespace mrpt::obs;
77  using namespace mrpt::maps;
78  using namespace mrpt::opengl;
79  using namespace mrpt::gui;
80  using namespace mrpt::io;
81  using namespace mrpt::gui;
82  using namespace mrpt::config;
83  using namespace mrpt::system;
84  using namespace mrpt::math;
85  using namespace mrpt::poses;
86  using namespace std;
87 
88  // ------------------------------------------
89  // Load config from file:
90  // ------------------------------------------
91 
92  const string OUT_DIR_STD =
93  params.read_string(sect, "logOutput_dir", "log_out", true);
94  const char* OUT_DIR = OUT_DIR_STD.c_str();
95 
96  const int LOG_FREQUENCY = params.read_int(sect, "LOG_FREQUENCY", 5, true);
97  const bool SAVE_POSE_LOG =
98  params.read_bool(sect, "SAVE_POSE_LOG", false, true);
99  const bool SAVE_3D_SCENE =
100  params.read_bool(sect, "SAVE_3D_SCENE", false, true);
101  const bool CAMERA_3DSCENE_FOLLOWS_ROBOT =
102  params.read_bool(sect, "CAMERA_3DSCENE_FOLLOWS_ROBOT", true, true);
103 
104  bool SHOW_PROGRESS_3D_REAL_TIME = false;
105  int SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS = 0;
106  bool SHOW_LASER_SCANS_3D = true;
107 
108  MRPT_LOAD_CONFIG_VAR(SHOW_PROGRESS_3D_REAL_TIME, bool, params, sect);
109  MRPT_LOAD_CONFIG_VAR(SHOW_LASER_SCANS_3D, bool, params, sect);
111  SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS, int, params, sect);
112 
113  if (params.keyExists(sect, "verbosity"))
116 
117  // ------------------------------------
118  // Constructor of ICP-SLAM object
119  // ------------------------------------
120  CMetricMapBuilderICP mapBuilder;
121  mapBuilder.setVerbosityLevel(this->getMinLoggingLevel());
122 
124  mapBuilder.ICP_params.loadFromConfigFile(params, "ICP");
125 
126  // Construct the maps with the loaded configuration.
127  mapBuilder.initialize();
128 
129  // ---------------------------------
130  // CMetricMapBuilder::TOptions
131  // ---------------------------------
133  params.read_string(sect, "alwaysInsertByClass", ""));
134 
135  // Print params:
136  MRPT_LOG_INFO_FMT("Output directory: `%s`", OUT_DIR);
137 
138  {
139  std::stringstream ss;
140  mapBuilder.ICP_params.dumpToTextStream(ss);
141  mapBuilder.ICP_options.dumpToTextStream(ss);
142  MRPT_LOG_INFO(ss.str());
143  }
144 
145  CTicTac tictac, tictacGlobal;
146  int step = 0;
147  CSimpleMap finalMap;
148  float t_exec;
149 
150  // Prepare output directory:
151  // --------------------------------
152  deleteFilesInDirectory(OUT_DIR);
153  createDirectory(OUT_DIR);
154 
155  // Open log files:
156  // ----------------------------------
157  CFileOutputStream f_log(format("%s/log_times.txt", OUT_DIR));
158  CFileOutputStream f_path(format("%s/log_estimated_path.txt", OUT_DIR));
159  CFileOutputStream f_pathOdo(format("%s/log_odometry_path.txt", OUT_DIR));
160 
161  // Create 3D window if requested:
162  CDisplayWindow3D::Ptr win3D;
163 #if MRPT_HAS_WXWIDGETS
164  if (SHOW_PROGRESS_3D_REAL_TIME)
165  {
166  win3D = std::make_shared<CDisplayWindow3D>(
167  "ICP-SLAM @ MRPT C++ Library", 600, 500);
168  win3D->setCameraZoom(20);
169  win3D->setCameraAzimuthDeg(-45);
170  }
171 #endif
172 
173  // ----------------------------------------------------------
174  // Map Building
175  // ----------------------------------------------------------
176  CPose2D odoPose(0, 0, 0);
177 
178  tictacGlobal.Tic();
179  for (;;)
180  {
181  CActionCollection::Ptr action;
182  CSensoryFrame::Ptr observations;
183  CObservation::Ptr observation;
184 
185  if (quits_with_esc_key && os::kbhit())
186  {
187  char c = os::getch();
188  if (c == 27) break;
189  }
190 
191  // Load action/observation pair from the rawlog:
192  // --------------------------------------------------
193  if (!impl_get_next_observations(action, observations, observation))
194  break; // EOF
195 
196  const bool isObsBasedRawlog = observation ? true : false;
197 
198  ASSERT_(
199  (isObsBasedRawlog && observation->timestamp != INVALID_TIMESTAMP) ||
200  (!isObsBasedRawlog && !observations->empty() &&
201  *observations->begin() &&
202  (*observations->begin())->timestamp != INVALID_TIMESTAMP));
203 
204  const mrpt::system::TTimeStamp cur_timestamp =
205  isObsBasedRawlog ? observation->timestamp
206  : (*observations->begin())->timestamp;
207 
208  // For drawing in 3D views:
209  std::vector<mrpt::obs::CObservation2DRangeScan::Ptr> lst_lidars;
210 
211  // Update odometry:
212  if (isObsBasedRawlog)
213  {
214  static CPose2D lastOdo;
215  static bool firstOdo = true;
216  if (IS_CLASS(*observation, CObservationOdometry))
217  {
218  auto o = std::dynamic_pointer_cast<CObservationOdometry>(
219  observation);
220  if (!firstOdo) odoPose = odoPose + (o->odometry - lastOdo);
221 
222  lastOdo = o->odometry;
223  firstOdo = false;
224  }
225  }
226  else
227  {
229  action->getBestMovementEstimation();
230  if (act) odoPose = odoPose + act->poseChange->getMeanVal();
231  }
232 
233  // Build list of scans:
234  if (SHOW_LASER_SCANS_3D)
235  {
236  // Rawlog in "Observation-only" format:
237  if (isObsBasedRawlog)
238  {
239  if (IS_CLASS(*observation, CObservation2DRangeScan))
240  {
241  lst_lidars.push_back(
242  std::dynamic_pointer_cast<CObservation2DRangeScan>(
243  observation));
244  }
245  }
246  else
247  {
248  // Rawlog in the Actions-SF format:
249  for (size_t i = 0;; i++)
250  {
252  observations
253  ->getObservationByClass<CObservation2DRangeScan>(i);
254  if (!new_obs)
255  break; // There're no more scans
256  else
257  lst_lidars.push_back(new_obs);
258  }
259  }
260  }
261 
262  // Execute:
263  // ----------------------------------------
264  tictac.Tic();
265  if (isObsBasedRawlog)
266  mapBuilder.processObservation(observation);
267  else
268  mapBuilder.processActionObservation(*action, *observations);
269  t_exec = tictac.Tac();
270  MRPT_LOG_INFO_FMT("Map building executed in %.03fms", 1000.0f * t_exec);
271 
272  // Info log:
273  // -----------
274  f_log.printf(
275  "%f %i\n", 1000.0f * t_exec, mapBuilder.getCurrentlyBuiltMapSize());
276 
277  const CMultiMetricMap* mostLikMap =
278  mapBuilder.getCurrentlyBuiltMetricMap();
279 
280  if (LOG_FREQUENCY > 0 && 0 == (step % LOG_FREQUENCY))
281  {
282  // Pose log:
283  // -------------
284  if (SAVE_POSE_LOG)
285  {
286  auto str =
287  mrpt::format("%s/mapbuild_posepdf_%03u.txt", OUT_DIR, step);
289  "Saving pose log information to `%s`", str.c_str());
290  mapBuilder.getCurrentPoseEstimation()->saveToTextFile(str);
291  }
292  }
293 
294  const CPose3D robotPose =
295  mapBuilder.getCurrentPoseEstimation()->getMeanVal();
296 
297  // Save a 3D scene view of the mapping process:
298  if ((LOG_FREQUENCY > 0 && 0 == (step % LOG_FREQUENCY)) ||
299  (SAVE_3D_SCENE || win3D))
300  {
301  auto scene = mrpt::opengl::COpenGLScene::Create();
302 
303  COpenGLViewport::Ptr view = scene->getViewport("main");
304  ASSERT_(view);
305 
306  COpenGLViewport::Ptr view_map = scene->createViewport("mini-map");
307  view_map->setBorderSize(2);
308  view_map->setViewportPosition(0.01, 0.01, 0.35, 0.35);
309  view_map->setTransparent(false);
310 
311  {
312  mrpt::opengl::CCamera& cam = view_map->getCamera();
313  cam.setAzimuthDegrees(-90);
314  cam.setElevationDegrees(90);
315  cam.setPointingAt(robotPose);
316  cam.setZoomDistance(20);
317  // cam.setOrthogonal();
318  }
319 
320  // The ground:
321  mrpt::opengl::CGridPlaneXY::Ptr groundPlane =
322  mrpt::opengl::CGridPlaneXY::Create(-200, 200, -200, 200, 0, 5);
323  groundPlane->setColor(0.4f, 0.4f, 0.4f);
324  view->insert(groundPlane);
325  view_map->insert(CRenderizable::Ptr(groundPlane)); // A copy
326 
327  // The camera pointing to the current robot pose:
328  if (CAMERA_3DSCENE_FOLLOWS_ROBOT)
329  {
330  scene->enableFollowCamera(true);
331 
332  mrpt::opengl::CCamera& cam = view_map->getCamera();
333  cam.setAzimuthDegrees(-45);
334  cam.setElevationDegrees(45);
335  cam.setPointingAt(robotPose);
336  }
337 
338  // The maps:
339  {
340  auto obj = opengl::CSetOfObjects::Create();
341  mostLikMap->getAs3DObject(obj);
342  view->insert(obj);
343 
344  // Only the point map:
345  auto ptsMap = opengl::CSetOfObjects::Create();
346  if (auto pMap = mostLikMap->mapByClass<CPointsMap>(); pMap)
347  {
348  pMap->getAs3DObject(ptsMap);
349  view_map->insert(ptsMap);
350  }
351  }
352 
353  // Draw the robot path:
354  {
356  obj->setPose(robotPose);
357  view->insert(obj);
358  }
359  {
361  obj->setPose(robotPose);
362  view_map->insert(obj);
363  }
364 
365  // Draw laser scanners in 3D:
366  if (SHOW_LASER_SCANS_3D)
367  {
368  for (auto& lst_current_laser_scan : lst_lidars)
369  {
370  // Create opengl object and load scan data from the scan
371  // observation:
373  obj->setScan(*lst_current_laser_scan);
374  obj->setPose(robotPose);
375  obj->setSurfaceColor(1.0f, 0.0f, 0.0f, 0.5f);
376  // inser into the scene:
377  view->insert(obj);
378  }
379  }
380 
381  // Save as file:
382  if (LOG_FREQUENCY > 0 && 0 == (step % LOG_FREQUENCY) &&
383  SAVE_3D_SCENE)
384  {
386  mrpt::format("%s/buildingmap_%05u.3Dscene", OUT_DIR, step));
388  }
389 
390  // Show 3D?
391  if (win3D)
392  {
393  opengl::COpenGLScene::Ptr& ptrScene =
394  win3D->get3DSceneAndLock();
395  ptrScene = scene;
396 
397  win3D->unlockAccess3DScene();
398 
399  // Move camera:
400  win3D->setCameraPointingToPoint(
401  robotPose.x(), robotPose.y(), robotPose.z());
402 
403  // Update:
404  win3D->forceRepaint();
405 
406  std::this_thread::sleep_for(std::chrono::milliseconds(
407  SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS));
408  }
409  }
410 
411  // Save the memory usage:
412  // ------------------------------------------------------------------
413  {
414  auto str = mrpt::format("%s/log_MemoryUsage.txt", OUT_DIR);
415  unsigned long memUsage = getMemoryUsage();
416  FILE* f = os::fopen(str.c_str(), "at");
417  if (f)
418  {
419  os::fprintf(f, "%u\t%lu\n", step, memUsage);
420  os::fclose(f);
421  }
423  "Memory usage:%.04f MiB", memUsage / (1024.0 * 1024.0));
424  }
425 
426  // Save the robot estimated pose for each step:
427  f_path.printf(
428  "%i %f %f %f\n", step, robotPose.x(), robotPose.y(),
429  robotPose.yaw());
430 
431  // Also keep the robot path as a vector, for the convenience of the app
432  // user:
433  out_estimated_path[cur_timestamp] = robotPose.asTPose();
434 
435  f_pathOdo.printf(
436  "%i %f %f %f\n", step, odoPose.x(), odoPose.y(), odoPose.phi());
437 
438  step++;
439  MRPT_LOG_INFO_FMT("------------- STEP %u ----------------", step);
440  };
441 
443  "----------- **END** (total time: %.03f sec) ---------",
444  tictacGlobal.Tac());
445 
446  // Save map:
447  mapBuilder.getCurrentlyBuiltMap(finalMap);
448 
449  {
450  auto str = format("%s/_finalmap_.simplemap", OUT_DIR);
452  "Dumping final map in binary format to: %s\n", str.c_str());
453  mapBuilder.saveCurrentMapToFile(str);
454  }
455 
456  {
457  const CMultiMetricMap* finalPointsMap =
458  mapBuilder.getCurrentlyBuiltMetricMap();
459  auto str = format("%s/_finalmaps_.txt", OUT_DIR);
460  MRPT_LOG_INFO_FMT("Dumping final metric maps to %s_XXX\n", str.c_str());
461  finalPointsMap->saveMetricMapRepresentationToFile(str);
462  }
463 
464  if (win3D) win3D->waitForKey();
465 
466  MRPT_END
467 }
468 
469 // ---------------------------------------
470 // ICP_SLAM_App_Rawlog
471 // ---------------------------------------
473 {
474  setLoggerName("ICP_SLAM_App_Rawlog");
475 }
476 
478 {
479  MRPT_START
480  // Rawlog file: from args. line or from config file:
481  if (argc == 3)
482  m_rawlogFileName = std::string(argv[2]);
483  else
484  m_rawlogFileName = params.read_string(
485  sect, "rawlog_file", std::string("log.rawlog"), true);
486 
487  m_rawlog_offset = params.read_int(sect, "rawlog_offset", 0, true);
488 
489  ASSERT_FILE_EXISTS_(m_rawlogFileName);
490 
491  MRPT_END
492 }
493 
494 // ---------------------------------------
495 // ICP_SLAM_App_Live
496 // ---------------------------------------
498 {
499  this->setLoggerName("ICP_SLAM_App_Live");
500 }
501 
503 
505 {
506  MRPT_START
507 
508  if (argc != 2)
509  {
510  THROW_EXCEPTION_FMT("Usage: %s", impl_get_usage().c_str());
511  }
512 
513  // Config file already loaded into "params".
514 
515  // Load sensor params from section: "LIDAR_SENSOR"
516  std::thread hSensorThread;
517  {
518  TThreadParams threParms;
519  threParms.cfgFile = &params;
520  threParms.section_name = "LIDAR_SENSOR";
521  MRPT_LOG_INFO("Launching LIDAR grabbing thread...");
522  hSensorThread =
523  std::thread(&ICP_SLAM_App_Live::SensorThread, this, threParms);
524  }
525  // Wait and check if the sensor is ready:
526  using namespace std::chrono_literals;
527  std::this_thread::sleep_for(2000ms);
528 
529  if (m_allThreadsMustExit)
530  throw std::runtime_error(
531  "\n\n==== ABORTING: It seems that we could not connect to the "
532  "LIDAR. See reported errors. ==== \n");
533 
534  MRPT_END
535 }
536 
538  [[maybe_unused]] mrpt::obs::CActionCollection::Ptr& action,
539  [[maybe_unused]] mrpt::obs::CSensoryFrame::Ptr& observations,
540  mrpt::obs::CObservation::Ptr& observation)
541 {
542  MRPT_START
543 
545 
546  // Check if we had any hardware failure:
547  if (m_allThreadsMustExit) return false;
548 
549  const auto t0 = mrpt::Clock::now();
550 
551  // Load sensor LIDAR data from live capture:
553  {
555  {
557  {
558  std::lock_guard<std::mutex> csl(m_cs_global_list_obs);
559  obs_copy = m_global_list_obs;
560  m_global_list_obs.clear();
561  }
562  // Keep the most recent laser scan:
563  for (auto it = obs_copy.rbegin(); !new_obs && it != obs_copy.rend();
564  ++it)
565  if (it->second &&
566  IS_CLASS(*it->second, CObservation2DRangeScan))
567  new_obs =
568  std::dynamic_pointer_cast<CObservation2DRangeScan>(
569  it->second);
570  }
571 
572  if (new_obs)
573  {
574  observation = std::move(new_obs);
575  return true;
576  }
577  else
578  {
579  using namespace std::chrono_literals;
580  std::this_thread::sleep_for(10ms);
581  }
582  }
583 
584  // timeout:
585  MRPT_LOG_ERROR("Timeout waiting for next lidar scan.");
586  return false;
587 
588  MRPT_END
589 }
590 
592 {
593  using namespace mrpt::hwdrivers;
594  using namespace mrpt::system;
595  try
596  {
597  std::string driver_name =
598  tp.cfgFile->read_string(tp.section_name, "driver", "", true);
599  CGenericSensor::Ptr sensor =
600  CGenericSensor::createSensorPtr(driver_name);
601  if (!sensor)
602  throw std::runtime_error(
603  std::string("***ERROR***: Class name not recognized: ") +
604  driver_name);
605 
606  // Load common & sensor specific parameters:
607  sensor->loadConfig(*tp.cfgFile, tp.section_name);
608  std::cout << mrpt::format(
609  "[thread_%s] Starting...", tp.section_name.c_str())
610  << " at " << sensor->getProcessRate() << " Hz" << std::endl;
611 
612  ASSERTMSG_(
613  sensor->getProcessRate() > 0,
614  "process_rate must be set to a valid value (>0 Hz).");
615 
616  mrpt::system::CRateTimer rate(sensor->getProcessRate());
617 
618  sensor->initialize(); // Init device:
619  while (!m_allThreadsMustExit)
620  {
621  sensor->doProcess(); // Process
622  // Get new observations
624  sensor->getObservations(lstObjs);
625  {
626  std::lock_guard<std::mutex> lock(m_cs_global_list_obs);
627  m_global_list_obs.insert(lstObjs.begin(), lstObjs.end());
628  }
629  lstObjs.clear();
630 
631  // wait for the process period:
632  rate.sleep();
633  }
634  sensor.reset();
635  printf("[thread_%s] Closing...", tp.section_name.c_str());
636  }
637  catch (const std::exception& e)
638  {
639  std::cerr << "[SensorThread] Closing due to exception:\n"
640  << mrpt::exception_to_str(e) << std::endl;
641  m_allThreadsMustExit = true;
642  }
643  catch (...)
644  {
645  std::cerr << "[SensorThread] Untyped exception! Closing." << std::endl;
646  m_allThreadsMustExit = true;
647  }
648 }
void saveCurrentMapToFile(const std::string &fileName, bool compressGZ=true) const
Save map (mrpt::maps::CSimpleMap) to a ".simplemap" file.
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:759
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
virtual bool impl_get_next_observations(mrpt::obs::CActionCollection::Ptr &action, mrpt::obs::CSensoryFrame::Ptr &observations, mrpt::obs::CObservation::Ptr &observation)=0
Get next sensory data.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:30
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
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.
A class for calling sleep() in a loop, such that the amount of sleep time will be computed to make th...
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:161
#define MRPT_START
Definition: exceptions.h:241
VerbosityLevel
Enumeration of available verbosity levels.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
int getch() noexcept
An OS-independent version of getch, which waits until a key is pushed.
Definition: os.cpp:370
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
static Ptr Create(Args &&... args)
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
mrpt::config::CConfigFileMemory params
Populated in initialize().
Definition: ICP_SLAM_App.h:62
mrpt::rtti::CListOfClasses alwaysInsertByClass
A list of observation classes (derived from mrpt::obs::CObservation) which will be always inserted in...
A high-performance stopwatch, with typical resolution of nanoseconds.
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account...
Contains classes for various device interfaces.
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
STL namespace.
static Ptr Create(Args &&... args)
Definition: CSetOfObjects.h:28
std::map< mrpt::system::TTimeStamp, mrpt::math::TPose3D > out_estimated_path
Definition: ICP_SLAM_App.h:73
std::string MRPT_getCompilationDate()
Returns the MRPT source code timestamp, according to the Reproducible-Builds specifications: https://...
Definition: os.cpp:154
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 ...
bool impl_get_next_observations(mrpt::obs::CActionCollection::Ptr &action, mrpt::obs::CSensoryFrame::Ptr &observations, mrpt::obs::CObservation::Ptr &observation) override
Get next sensory data.
static time_point now() noexcept
Returns the current time using the currently selected Clock source.
Definition: Clock.cpp:94
TConfigParams ICP_options
Options for the ICP-SLAM application.
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...
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.
Definition: CICP.cpp:80
virtual std::string impl_get_usage() const =0
mrpt::config::CConfigFileBase * cfgFile
Definition: ICP_SLAM_App.h:115
void initialize(int argc, const char **argv)
Initializes the application from CLI parameters.
void run()
Runs with the current parameter set.
void setZoomDistance(float z)
Definition: CCamera.h:63
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
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...
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:592
void setAzimuthDegrees(float ang)
Definition: CCamera.h:67
VerbosityLevel getMinLoggingLevel() const
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
CICP::TConfigParams ICP_params
Options for the ICP algorithm itself.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
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 base provides a set of functions for maths stuff.
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
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.
#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
constexpr auto sect
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
virtual void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
This CStream derived class allow using a file as a write-only, binary stream.
bool keyExists(const std::string &section, const std::string &key) const
Checks if a given key exists inside a section (case insensitive)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
CSetOfObjects::Ptr RobotPioneer()
Returns a representation of a Pioneer II mobile base.
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:408
#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...
void setVerbosityLevel(const VerbosityLevel level)
alias of setMinLoggingLevel()
void impl_initialize(int argc, const char **argv) override
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const char * argv[]
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
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.
void setElevationDegrees(float ang)
Definition: CCamera.h:68
unsigned long getMemoryUsage()
Returns the memory occupied by this process, in bytes.
Definition: memory.cpp:114
std::multimap< mrpt::system::TTimeStamp, mrpt::serialization::CSerializable::Ptr > TListObservations
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
#define MRPT_LOG_ERROR(_STRING)
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
void setContent(const std::vector< std::string > &stringList)
Changes the contents of the virtual "config file".
void impl_initialize(int argc, const char **argv) override
#define MRPT_END
Definition: exceptions.h:245
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
bool kbhit() noexcept
An OS-independent version of kbhit, which returns true if a key has been pushed.
Definition: os.cpp:392
std::string file_get_contents(const std::string &fileName)
Loads an entire text file and return its contents as a single std::string.
void processObservation(const mrpt::obs::CObservation::Ptr &obs)
The main method of this class: Process one odometry or sensor observation.
std::string exception_to_str(const std::exception &e)
Builds a nice textual representation of a nested exception, which if generated using MRPT macros (THR...
Definition: exceptions.cpp:59
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.
bool deleteFilesInDirectory(const std::string &s, bool deleteDirectoryAsWell=false)
Delete all the files in a given directory (nothing done if directory does not exists, or path is a file).
Definition: filesystem.cpp:218
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
mrpt::poses::CPose3DPDF::Ptr getCurrentPoseEstimation() const override
Returns a copy of the current best pose estimation as a pose PDF.
static Ptr Create(Args &&... args)
Definition: COpenGLScene.h:58
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:14
const int argc
A class for very simple 2D SLAM based on ICP.
double timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
Definition: datetime.h:123
std::string MRPT_getVersion()
Returns a string describing the MRPT version.
Definition: os.cpp:187
An observation of the current (cumulative) odometry for a wheeled robot.
void SensorThread(TThreadParams params)
Saves data to a file and transparently compress the data using the given compression level...
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
This class stores any customizable set of metric maps.
#define ASSERT_FILE_EXISTS_(FIL)
Definition: filesystem.h:22
A camera: if added to a scene, the viewpoint defined by this camera will be used instead of the camer...
Definition: CCamera.h:33
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
unsigned int getCurrentlyBuiltMapSize() override
Returns just how many sensory-frames are stored in the currently build map.
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:31
virtual ~ICP_SLAM_App_Live() override
void fromString(const std::string &s)
Builds from a string representation of the list, for example: "CPose2D, CObservation, CPose3D".
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
bool quits_with_esc_key
If enabled (default), stdin will be watched and application quits if ESC is pressed.
Definition: ICP_SLAM_App.h:66
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...
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
virtual void impl_initialize(int argc, const char **argv)=0
#define MRPT_LOG_INFO(_STRING)
void setPointingAt(float x, float y, float z)
Definition: CCamera.h:41



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