MRPT  2.0.5
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>
30 
31 using namespace mrpt::apps;
32 
33 constexpr auto sect = "MappingApplication";
34 
35 // ---------------------------------------
36 // ICP_SLAM_App_Base
37 // ---------------------------------------
39 {
40  // Set logger display name:
41  this->setLoggerName("ICP_SLAM_App");
42 }
43 
44 void ICP_SLAM_App_Base::initialize(int argc, const char** argv)
45 {
47 
49  " icp-slam - Part of the MRPT\n"
50  " MRPT C++ Library: %s - Sources timestamp: %s\n\n",
53 
54  // Process arguments:
55  if (argc < 2)
56  {
57  THROW_EXCEPTION_FMT("Usage: %s", impl_get_usage().c_str());
58  }
59 
60  // Config file:
61  const std::string configFile = std::string(argv[1]);
62 
63  ASSERT_FILE_EXISTS_(configFile);
65 
67 
68  MRPT_END
69 }
70 
72 {
74 
75  using namespace mrpt;
76  using namespace mrpt::slam;
77  using namespace mrpt::obs;
78  using namespace mrpt::maps;
79  using namespace mrpt::opengl;
80  using namespace mrpt::gui;
81  using namespace mrpt::io;
82  using namespace mrpt::gui;
83  using namespace mrpt::config;
84  using namespace mrpt::system;
85  using namespace mrpt::math;
86  using namespace mrpt::poses;
87  using namespace std;
88 
89  // ------------------------------------------
90  // Load config from file:
91  // ------------------------------------------
92 
93  const string OUT_DIR_STD =
94  params.read_string(sect, "logOutput_dir", "log_out", true);
95  const char* OUT_DIR = OUT_DIR_STD.c_str();
96 
97  const int LOG_FREQUENCY = params.read_int(sect, "LOG_FREQUENCY", 5, true);
98  const bool SAVE_POSE_LOG =
99  params.read_bool(sect, "SAVE_POSE_LOG", false, true);
100  const bool SAVE_3D_SCENE =
101  params.read_bool(sect, "SAVE_3D_SCENE", false, true);
102  const bool CAMERA_3DSCENE_FOLLOWS_ROBOT =
103  params.read_bool(sect, "CAMERA_3DSCENE_FOLLOWS_ROBOT", true, true);
104 
105  bool SHOW_PROGRESS_3D_REAL_TIME = false;
106  int SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS = 0;
107  bool SHOW_LASER_SCANS_3D = true;
108 
109  MRPT_LOAD_CONFIG_VAR(SHOW_PROGRESS_3D_REAL_TIME, bool, params, sect);
110  MRPT_LOAD_CONFIG_VAR(SHOW_LASER_SCANS_3D, bool, params, sect);
112  SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS, int, params, sect);
113 
114  if (params.keyExists(sect, "verbosity"))
117 
118  // ------------------------------------
119  // Constructor of ICP-SLAM object
120  // ------------------------------------
121  CMetricMapBuilderICP mapBuilder;
122  mapBuilder.setVerbosityLevel(this->getMinLoggingLevel());
123 
125  mapBuilder.ICP_params.loadFromConfigFile(params, "ICP");
126 
127  // Construct the maps with the loaded configuration.
128  mapBuilder.initialize();
129 
130  // ---------------------------------
131  // CMetricMapBuilder::TOptions
132  // ---------------------------------
134  params.read_string(sect, "alwaysInsertByClass", ""));
135 
136  // Print params:
137  MRPT_LOG_INFO_FMT("Output directory: `%s`", OUT_DIR);
138 
139  {
140  std::stringstream ss;
141  mapBuilder.ICP_params.dumpToTextStream(ss);
142  mapBuilder.ICP_options.dumpToTextStream(ss);
143  MRPT_LOG_INFO(ss.str());
144  }
145 
146  CTicTac tictac, tictacGlobal;
147  int step = 0;
148  CSimpleMap finalMap;
149  float t_exec;
150 
151  // Prepare output directory:
152  // --------------------------------
153  deleteFilesInDirectory(OUT_DIR);
154  createDirectory(OUT_DIR);
155 
156  // Open log files:
157  // ----------------------------------
158  CFileOutputStream f_log(format("%s/log_times.txt", OUT_DIR));
159  CFileOutputStream f_path(format("%s/log_estimated_path.txt", OUT_DIR));
160  CFileOutputStream f_pathOdo(format("%s/log_odometry_path.txt", OUT_DIR));
161 
162  // Create 3D window if requested:
163  CDisplayWindow3D::Ptr win3D;
164 #if MRPT_HAS_WXWIDGETS
165  if (SHOW_PROGRESS_3D_REAL_TIME)
166  {
167  win3D = std::make_shared<CDisplayWindow3D>(
168  "ICP-SLAM @ MRPT C++ Library", 600, 500);
169  win3D->setCameraZoom(20);
170  win3D->setCameraAzimuthDeg(-45);
171  }
172 #endif
173 
174  // ----------------------------------------------------------
175  // Map Building
176  // ----------------------------------------------------------
177  CPose2D odoPose(0, 0, 0);
178 
179  tictacGlobal.Tic();
180  for (;;)
181  {
182  CActionCollection::Ptr action;
183  CSensoryFrame::Ptr observations;
184  CObservation::Ptr observation;
185 
186  if (quits_with_esc_key && os::kbhit())
187  {
188  char c = os::getch();
189  if (c == 27) break;
190  }
191 
192  // Load action/observation pair from the rawlog:
193  // --------------------------------------------------
194  if (!impl_get_next_observations(action, observations, observation))
195  break; // EOF
196 
197  const bool isObsBasedRawlog = observation ? true : false;
198 
199  ASSERT_(
200  (isObsBasedRawlog && observation->timestamp != INVALID_TIMESTAMP) ||
201  (!isObsBasedRawlog && !observations->empty() &&
202  *observations->begin() &&
203  (*observations->begin())->timestamp != INVALID_TIMESTAMP));
204 
205  const mrpt::system::TTimeStamp cur_timestamp =
206  isObsBasedRawlog ? observation->timestamp
207  : (*observations->begin())->timestamp;
208 
209  // For drawing in 3D views:
210  std::vector<mrpt::obs::CObservation2DRangeScan::Ptr> lst_lidars;
211 
212  // Update odometry:
213  if (isObsBasedRawlog)
214  {
215  static CPose2D lastOdo;
216  static bool firstOdo = true;
217  if (IS_CLASS(*observation, CObservationOdometry))
218  {
219  auto o = std::dynamic_pointer_cast<CObservationOdometry>(
220  observation);
221  if (!firstOdo) odoPose = odoPose + (o->odometry - lastOdo);
222 
223  lastOdo = o->odometry;
224  firstOdo = false;
225  }
226  }
227  else
228  {
230  action->getBestMovementEstimation();
231  if (act) odoPose = odoPose + act->poseChange->getMeanVal();
232  }
233 
234  // Build list of scans:
235  if (SHOW_LASER_SCANS_3D)
236  {
237  // Rawlog in "Observation-only" format:
238  if (isObsBasedRawlog)
239  {
240  if (IS_CLASS(*observation, CObservation2DRangeScan))
241  {
242  lst_lidars.push_back(
243  std::dynamic_pointer_cast<CObservation2DRangeScan>(
244  observation));
245  }
246  }
247  else
248  {
249  // Rawlog in the Actions-SF format:
250  for (size_t i = 0;; i++)
251  {
253  observations
254  ->getObservationByClass<CObservation2DRangeScan>(i);
255  if (!new_obs)
256  break; // There're no more scans
257  else
258  lst_lidars.push_back(new_obs);
259  }
260  }
261  }
262 
263  // Execute:
264  // ----------------------------------------
265  tictac.Tic();
266  if (isObsBasedRawlog)
267  mapBuilder.processObservation(observation);
268  else
269  mapBuilder.processActionObservation(*action, *observations);
270  t_exec = tictac.Tac();
271  MRPT_LOG_INFO_FMT("Map building executed in %.03fms", 1000.0f * t_exec);
272 
273  // Info log:
274  // -----------
275  f_log.printf(
276  "%f %i\n", 1000.0f * t_exec, mapBuilder.getCurrentlyBuiltMapSize());
277 
278  const CMultiMetricMap* mostLikMap =
279  mapBuilder.getCurrentlyBuiltMetricMap();
280 
281  if (LOG_FREQUENCY > 0 && 0 == (step % LOG_FREQUENCY))
282  {
283  // Pose log:
284  // -------------
285  if (SAVE_POSE_LOG)
286  {
287  auto str =
288  mrpt::format("%s/mapbuild_posepdf_%03u.txt", OUT_DIR, step);
290  "Saving pose log information to `%s`", str.c_str());
291  mapBuilder.getCurrentPoseEstimation()->saveToTextFile(str);
292  }
293  }
294 
295  const CPose3D robotPose =
296  mapBuilder.getCurrentPoseEstimation()->getMeanVal();
297 
298  // Save a 3D scene view of the mapping process:
299  if ((LOG_FREQUENCY > 0 && 0 == (step % LOG_FREQUENCY)) ||
300  (SAVE_3D_SCENE || win3D))
301  {
302  auto scene = mrpt::opengl::COpenGLScene::Create();
303 
304  COpenGLViewport::Ptr view = scene->getViewport("main");
305  ASSERT_(view);
306 
307  COpenGLViewport::Ptr view_map = scene->createViewport("mini-map");
308  view_map->setBorderSize(2);
309  view_map->setViewportPosition(0.01, 0.01, 0.35, 0.35);
310  view_map->setTransparent(false);
311 
312  {
313  mrpt::opengl::CCamera& cam = view_map->getCamera();
314  cam.setAzimuthDegrees(-90);
315  cam.setElevationDegrees(90);
316  cam.setPointingAt(robotPose);
317  cam.setZoomDistance(20);
318  // cam.setOrthogonal();
319  }
320 
321  // The ground:
322  mrpt::opengl::CGridPlaneXY::Ptr groundPlane =
323  mrpt::opengl::CGridPlaneXY::Create(-200, 200, -200, 200, 0, 5);
324  groundPlane->setColor(0.4f, 0.4f, 0.4f);
325  view->insert(groundPlane);
326  view_map->insert(CRenderizable::Ptr(groundPlane)); // A copy
327 
328  // The camera pointing to the current robot pose:
329  if (CAMERA_3DSCENE_FOLLOWS_ROBOT)
330  {
331  scene->enableFollowCamera(true);
332 
333  mrpt::opengl::CCamera& cam = view_map->getCamera();
334  cam.setAzimuthDegrees(-45);
335  cam.setElevationDegrees(45);
336  cam.setPointingAt(robotPose);
337  }
338 
339  // The maps:
340  {
341  auto obj = opengl::CSetOfObjects::Create();
342  mostLikMap->getAs3DObject(obj);
343  view->insert(obj);
344 
345  // Only the point map:
346  auto ptsMap = opengl::CSetOfObjects::Create();
347  if (auto pMap = mostLikMap->mapByClass<CPointsMap>(); pMap)
348  {
349  pMap->getAs3DObject(ptsMap);
350  view_map->insert(ptsMap);
351  }
352  }
353 
354  // Draw the robot path:
355  {
357  obj->setPose(robotPose);
358  view->insert(obj);
359  }
360  {
362  obj->setPose(robotPose);
363  view_map->insert(obj);
364  }
365 
366  // Draw laser scanners in 3D:
367  if (SHOW_LASER_SCANS_3D)
368  {
369  for (auto& lst_current_laser_scan : lst_lidars)
370  {
371  // Create opengl object and load scan data from the scan
372  // observation:
374  obj->setScan(*lst_current_laser_scan);
375  obj->setPose(robotPose);
376  obj->setSurfaceColor(1.0f, 0.0f, 0.0f, 0.5f);
377  // inser into the scene:
378  view->insert(obj);
379  }
380  }
381 
382  // Save as file:
383  if (LOG_FREQUENCY > 0 && 0 == (step % LOG_FREQUENCY) &&
384  SAVE_3D_SCENE)
385  {
387  mrpt::format("%s/buildingmap_%05u.3Dscene", OUT_DIR, step));
389  }
390 
391  // Show 3D?
392  if (win3D)
393  {
394  opengl::COpenGLScene::Ptr& ptrScene =
395  win3D->get3DSceneAndLock();
396  ptrScene = scene;
397 
398  win3D->unlockAccess3DScene();
399 
400  // Move camera:
401  win3D->setCameraPointingToPoint(
402  robotPose.x(), robotPose.y(), robotPose.z());
403 
404  // Update:
405  win3D->forceRepaint();
406 
407  std::this_thread::sleep_for(std::chrono::milliseconds(
408  SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS));
409  }
410  }
411 
412  // Save the memory usage:
413  // ------------------------------------------------------------------
414  {
415  auto str = mrpt::format("%s/log_MemoryUsage.txt", OUT_DIR);
416  unsigned long memUsage = getMemoryUsage();
417  FILE* f = os::fopen(str.c_str(), "at");
418  if (f)
419  {
420  os::fprintf(f, "%u\t%lu\n", step, memUsage);
421  os::fclose(f);
422  }
424  "Memory usage:%.04f MiB", memUsage / (1024.0 * 1024.0));
425  }
426 
427  // Save the robot estimated pose for each step:
428  f_path.printf(
429  "%i %f %f %f\n", step, robotPose.x(), robotPose.y(),
430  robotPose.yaw());
431 
432  // Also keep the robot path as a vector, for the convenience of the app
433  // user:
434  out_estimated_path[cur_timestamp] = robotPose.asTPose();
435 
436  f_pathOdo.printf(
437  "%i %f %f %f\n", step, odoPose.x(), odoPose.y(), odoPose.phi());
438 
439  step++;
440  MRPT_LOG_INFO_FMT("------------- STEP %u ----------------", step);
441  };
442 
444  "----------- **END** (total time: %.03f sec) ---------",
445  tictacGlobal.Tac());
446 
447  // Save map:
448  mapBuilder.getCurrentlyBuiltMap(finalMap);
449 
450  {
451  auto str = format("%s/_finalmap_.simplemap", OUT_DIR);
453  "Dumping final map in binary format to: %s\n", str.c_str());
454  mapBuilder.saveCurrentMapToFile(str);
455  }
456 
457  {
458  const CMultiMetricMap* finalPointsMap =
459  mapBuilder.getCurrentlyBuiltMetricMap();
460  auto str = format("%s/_finalmaps_.txt", OUT_DIR);
461  MRPT_LOG_INFO_FMT("Dumping final metric maps to %s_XXX\n", str.c_str());
462  finalPointsMap->saveMetricMapRepresentationToFile(str);
463  }
464 
465  if (win3D) win3D->waitForKey();
466 
467  MRPT_END
468 }
469 
470 // ---------------------------------------
471 // ICP_SLAM_App_Rawlog
472 // ---------------------------------------
474 {
475  setLoggerName("ICP_SLAM_App_Rawlog");
476 }
477 
479 {
480  MRPT_START
481  // Rawlog file: from args. line or from config file:
482  if (argc == 3)
483  m_rawlogFileName = std::string(argv[2]);
484  else
485  m_rawlogFileName = params.read_string(
486  sect, "rawlog_file", std::string("log.rawlog"), true);
487 
488  m_rawlog_offset = params.read_int(sect, "rawlog_offset", 0, true);
489 
490  ASSERT_FILE_EXISTS_(m_rawlogFileName);
491 
492  MRPT_END
493 }
494 
495 // ---------------------------------------
496 // ICP_SLAM_App_Live
497 // ---------------------------------------
499 {
500  this->setLoggerName("ICP_SLAM_App_Live");
501 }
502 
504 
506 {
507  MRPT_START
508 
509  if (argc != 2)
510  {
511  THROW_EXCEPTION_FMT("Usage: %s", impl_get_usage().c_str());
512  }
513 
514  // Config file already loaded into "params".
515 
516  // Load sensor params from section: "LIDAR_SENSOR"
517  std::thread hSensorThread;
518  {
519  TThreadParams threParms;
520  threParms.cfgFile = &params;
521  threParms.section_name = "LIDAR_SENSOR";
522  MRPT_LOG_INFO("Launching LIDAR grabbing thread...");
523  hSensorThread =
524  std::thread(&ICP_SLAM_App_Live::SensorThread, this, threParms);
525 
526  mrpt::system::thread_name("icpLiveSensor", hSensorThread);
527  }
528  // Wait and check if the sensor is ready:
529  using namespace std::chrono_literals;
530  std::this_thread::sleep_for(2000ms);
531 
532  if (m_allThreadsMustExit)
533  throw std::runtime_error(
534  "\n\n==== ABORTING: It seems that we could not connect to the "
535  "LIDAR. See reported errors. ==== \n");
536 
537  MRPT_END
538 }
539 
541  [[maybe_unused]] mrpt::obs::CActionCollection::Ptr& action,
542  [[maybe_unused]] mrpt::obs::CSensoryFrame::Ptr& observations,
543  mrpt::obs::CObservation::Ptr& observation)
544 {
545  MRPT_START
546 
548 
549  // Check if we had any hardware failure:
550  if (m_allThreadsMustExit) return false;
551 
552  const auto t0 = mrpt::Clock::now();
553 
554  // Load sensor LIDAR data from live capture:
556  {
558  {
560  {
561  std::lock_guard<std::mutex> csl(m_cs_global_list_obs);
562  obs_copy = m_global_list_obs;
563  m_global_list_obs.clear();
564  }
565  // Keep the most recent laser scan:
566  for (auto it = obs_copy.rbegin(); !new_obs && it != obs_copy.rend();
567  ++it)
568  if (it->second &&
569  IS_CLASS(*it->second, CObservation2DRangeScan))
570  new_obs =
571  std::dynamic_pointer_cast<CObservation2DRangeScan>(
572  it->second);
573  }
574 
575  if (new_obs)
576  {
577  observation = std::move(new_obs);
578  return true;
579  }
580  else
581  {
582  using namespace std::chrono_literals;
583  std::this_thread::sleep_for(10ms);
584  }
585  }
586 
587  // timeout:
588  MRPT_LOG_ERROR("Timeout waiting for next lidar scan.");
589  return false;
590 
591  MRPT_END
592 }
593 
595 {
596  using namespace mrpt::hwdrivers;
597  using namespace mrpt::system;
598  try
599  {
600  std::string driver_name =
601  tp.cfgFile->read_string(tp.section_name, "driver", "", true);
602  CGenericSensor::Ptr sensor =
603  CGenericSensor::createSensorPtr(driver_name);
604  if (!sensor)
605  throw std::runtime_error(
606  std::string("***ERROR***: Class name not recognized: ") +
607  driver_name);
608 
609  // Load common & sensor specific parameters:
610  sensor->loadConfig(*tp.cfgFile, tp.section_name);
611  std::cout << mrpt::format(
612  "[thread_%s] Starting...", tp.section_name.c_str())
613  << " at " << sensor->getProcessRate() << " Hz" << std::endl;
614 
615  ASSERTMSG_(
616  sensor->getProcessRate() > 0,
617  "process_rate must be set to a valid value (>0 Hz).");
618 
619  mrpt::system::CRateTimer rate(sensor->getProcessRate());
620 
621  sensor->initialize(); // Init device:
622  while (!m_allThreadsMustExit)
623  {
624  sensor->doProcess(); // Process
625  // Get new observations
627  sensor->getObservations(lstObjs);
628  {
629  std::lock_guard<std::mutex> lock(m_cs_global_list_obs);
630  m_global_list_obs.insert(lstObjs.begin(), lstObjs.end());
631  }
632  lstObjs.clear();
633 
634  // wait for the process period:
635  rate.sleep();
636  }
637  sensor.reset();
638  printf("[thread_%s] Closing...", tp.section_name.c_str());
639  }
640  catch (const std::exception& e)
641  {
642  std::cerr << "[SensorThread] Closing due to exception:\n"
643  << mrpt::exception_to_str(e) << std::endl;
644  m_allThreadsMustExit = true;
645  }
646  catch (...)
647  {
648  std::cerr << "[SensorThread] Untyped exception! Closing." << std::endl;
649  m_allThreadsMustExit = true;
650  }
651 }
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:87
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:381
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:286
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:165
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
void thread_name(const std::string &name, std::thread &theThread)
Sets the name of the given thread; useful for debuggers.
Definition: thread_name.cpp:63
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:67
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:419
#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:403
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:268
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:198
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:76
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.5 Git: fc2d69e97 Fri Jul 10 18:20:57 2020 +0200 at vie jul 10 18:30:13 CEST 2020