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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020