MRPT  2.0.3
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 }
mrpt::apps::ICP_SLAM_App_Live::~ICP_SLAM_App_Live
virtual ~ICP_SLAM_App_Live() override
mrpt::slam::CMetricMapBuilderICP::getCurrentPoseEstimation
mrpt::poses::CPose3DPDF::Ptr getCurrentPoseEstimation() const override
Returns a copy of the current best pose estimation as a pose PDF.
Definition: CMetricMapBuilderICP.cpp:508
mrpt::system::os::kbhit
bool kbhit() noexcept
An OS-independent version of kbhit, which returns true if a key has been pushed.
Definition: os.cpp:392
mrpt::config
Definition: config/CConfigFile.h:14
mrpt::system::getMemoryUsage
unsigned long getMemoryUsage()
Returns the memory occupied by this process, in bytes.
Definition: memory.cpp:114
os.h
mrpt::obs::CObservation::Ptr
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:45
mrpt::apps::ICP_SLAM_App_Live::TThreadParams::section_name
std::string section_name
Definition: ICP_SLAM_App.h:116
filesystem.h
apps-precomp.h
mrpt::hwdrivers::CGenericSensor::TListObservations
std::multimap< mrpt::system::TTimeStamp, mrpt::serialization::CSerializable::Ptr > TListObservations
Definition: CGenericSensor.h:77
mrpt::system::timeDifference
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
mrpt::hwdrivers::CGenericSensor::Ptr
std::shared_ptr< CGenericSensor > Ptr
Definition: CGenericSensor.h:73
mrpt::slam::CMetricMapBuilderICP::processActionObservation
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...
Definition: CMetricMapBuilderICP.cpp:468
MRPT_LOG_INFO_FMT
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
Definition: system/COutputLogger.h:463
COccupancyGridMap2D.h
mrpt::slam::CMetricMapBuilder::options
TOptions options
Definition: CMetricMapBuilder.h:141
mrpt::slam::CMetricMapBuilderICP::initialize
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...
Definition: CMetricMapBuilderICP.cpp:522
mrpt::opengl::CCamera::setAzimuthDegrees
void setAzimuthDegrees(float ang)
Definition: CCamera.h:67
mrpt::system::os::fclose
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
mrpt::opengl::CSetOfObjects::Create
static Ptr Create(Args &&... args)
Definition: CSetOfObjects.h:28
INVALID_TIMESTAMP
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
mrpt::io
Definition: img/CImage.h:24
mrpt::maps::CMultiMetricMap::mapByClass
T::Ptr mapByClass(size_t ith=0) const
Returns the i'th map of a given class (or of a derived class), or empty smart pointer if there is no ...
Definition: CMultiMetricMap.h:156
MRPT_LOG_INFO
#define MRPT_LOG_INFO(_STRING)
Definition: system/COutputLogger.h:429
mrpt::apps::ICP_SLAM_App_Live::TThreadParams::cfgFile
mrpt::config::CConfigFileBase * cfgFile
Definition: ICP_SLAM_App.h:115
mrpt::config::CConfigFileBase::keyExists
bool keyExists(const std::string &section, const std::string &key) const
Checks if a given key exists inside a section (case insensitive)
Definition: CConfigFileBase.cpp:215
mrpt::slam::CMetricMapBuilderICP::processObservation
void processObservation(const mrpt::obs::CObservation::Ptr &obs)
The main method of this class: Process one odometry or sensor observation.
Definition: CMetricMapBuilderICP.cpp:134
vector_loadsave.h
mrpt::apps::ICP_SLAM_App_Rawlog::ICP_SLAM_App_Rawlog
ICP_SLAM_App_Rawlog()
Definition: ICP_SLAM_App.cpp:472
mrpt::system::COutputLogger::setVerbosityLevel
void setVerbosityLevel(const VerbosityLevel level)
alias of setMinLoggingLevel()
Definition: COutputLogger.cpp:149
mrpt::Clock::now
static time_point now() noexcept
Returns the current time using the currently selected Clock source.
Definition: Clock.cpp:94
mrpt::opengl::CGridPlaneXY::Ptr
std::shared_ptr< mrpt::opengl ::CGridPlaneXY > Ptr
Definition: CGridPlaneXY.h:31
mrpt::system::CTicTac
A high-performance stopwatch, with typical resolution of nanoseconds.
Definition: system/CTicTac.h:17
MRPT_LOAD_CONFIG_VAR
#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...
Definition: config/CConfigFileBase.h:306
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
Definition: CSensoryFrame.h:53
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:54
mrpt::poses::CPose2D::phi
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
mrpt::apps::ICP_SLAM_App_Live::SensorThread
void SensorThread(TThreadParams params)
Definition: ICP_SLAM_App.cpp:591
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
mrpt::slam::CMetricMapBuilderICP::ICP_params
CICP::TConfigParams ICP_params
Options for the ICP algorithm itself.
Definition: CMetricMapBuilderICP.h:88
CFileOutputStream.h
IS_CLASS
#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
mrpt::apps::ICP_SLAM_App_Live::impl_get_next_observations
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.
Definition: ICP_SLAM_App.cpp:537
mrpt::slam::CMetricMapBuilderICP
A class for very simple 2D SLAM based on ICP.
Definition: CMetricMapBuilderICP.h:25
CFileGZOutputStream.h
mrpt::apps::ICP_SLAM_App_Base::run
void run()
Runs with the current parameter set.
Definition: ICP_SLAM_App.cpp:70
sect
constexpr auto sect
Definition: ICP_SLAM_App.cpp:32
mrpt::maps::CPointsMap
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
mrpt::config::CConfigFileBase::read_bool
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:155
mrpt::hwdrivers
Contains classes for various device interfaces.
Definition: C2DRangeFinderAbstract.h:19
mrpt::obs::CActionCollection::Ptr
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
Definition: CActionCollection.h:28
mrpt::opengl::CRenderizable::Ptr
std::shared_ptr< CRenderizable > Ptr
Definition: CRenderizable.h:50
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::opengl::CCamera::setPointingAt
void setPointingAt(float x, float y, float z)
Definition: CCamera.h:41
mrpt::system::LVL_INFO
@ LVL_INFO
Definition: system/COutputLogger.h:31
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
stock_objects.h
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::obs::CActionRobotMovement2D::Ptr
std::shared_ptr< mrpt::obs ::CActionRobotMovement2D > Ptr
Definition: CActionRobotMovement2D.h:32
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
CDisplayWindow3D.h
mrpt::apps::ICP_SLAM_App_Base::out_estimated_path
std::map< mrpt::system::TTimeStamp, mrpt::math::TPose3D > out_estimated_path
Definition: ICP_SLAM_App.h:73
mrpt::slam::CMetricMapBuilderICP::getCurrentlyBuiltMap
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.
Definition: CMetricMapBuilderICP.cpp:584
mrpt::config::CConfigFileBase::read_string
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:171
mrpt::apps::ICP_SLAM_App_Live::TThreadParams
Definition: ICP_SLAM_App.h:113
mrpt::obs::CObservation2DRangeScan::Ptr
std::shared_ptr< mrpt::obs ::CObservation2DRangeScan > Ptr
Definition: CObservation2DRangeScan.h:56
mrpt::system::CTicTac::Tac
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
mrpt::obs::CObservationOdometry
An observation of the current (cumulative) odometry for a wheeled robot.
Definition: CObservationOdometry.h:29
mrpt::slam::CMetricMapBuilderICP::TConfigParams::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CMetricMapBuilderICP.cpp:100
mrpt::config::CConfigFileBase::read_int
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:130
mrpt::apps::BaseAppInitializableCLI::impl_initialize
virtual void impl_initialize(int argc, const char **argv)=0
mrpt::io::file_get_contents
std::string file_get_contents(const std::string &fileName)
Loads an entire text file and return its contents as a single std::string.
Definition: system/src/vector_loadsave.cpp:70
mrpt::slam::CMetricMapBuilderICP::getCurrentlyBuiltMetricMap
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const override
Returns the map built so far.
Definition: CMetricMapBuilderICP.cpp:589
mrpt::apps::ICP_SLAM_App_Base::initialize
void initialize(int argc, const char **argv)
Initializes the application from CLI parameters.
Definition: ICP_SLAM_App.cpp:43
mrpt::apps::ICP_SLAM_App_Base::quits_with_esc_key
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
CMetricMapBuilderICP.h
mrpt::apps::ICP_SLAM_App_Base::params
mrpt::config::CConfigFileMemory params
Populated in initialize().
Definition: ICP_SLAM_App.h:62
mrpt::system::CRateTimer::sleep
bool sleep()
Sleeps for some time, such as the return of this method is 1/rate (seconds) after the return of the p...
Definition: CRateTimer.cpp:25
mrpt::opengl::COpenGLViewport::Ptr
std::shared_ptr< mrpt::opengl ::COpenGLViewport > Ptr
Definition: COpenGLViewport.h:65
mrpt::system::VerbosityLevel
VerbosityLevel
Enumeration of available verbosity levels.
Definition: system/COutputLogger.h:28
mrpt::opengl::CGridPlaneXY::Create
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:31
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
COpenGLScene.h
mrpt::slam::CMetricMapBuilderICP::TConfigParams::loadFromConfigFile
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: CMetricMapBuilderICP.cpp:84
mrpt::rtti::CListOfClasses::fromString
void fromString(const std::string &s)
Builds from a string representation of the list, for example: "CPose2D, CObservation,...
Definition: CListOfClasses.cpp:62
mrpt::io::CFileGZOutputStream
Saves data to a file and transparently compress the data using the given compression level.
Definition: io/CFileGZOutputStream.h:26
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: CMultiMetricMap.h:120
MRPT_LOG_ERROR
#define MRPT_LOG_ERROR(_STRING)
Definition: system/COutputLogger.h:433
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::config::CConfigFileMemory::setContent
void setContent(const std::vector< std::string > &stringList)
Changes the contents of the virtual "config file".
Definition: CConfigFileMemory.cpp:52
mrpt::poses::CPose3D::asTPose
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:759
mrpt::system::CRateTimer
A class for calling sleep() in a loop, such that the amount of sleep time will be computed to make th...
Definition: system/CRateTimer.h:21
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
mrpt::opengl::CCamera
A camera: if added to a scene, the viewpoint defined by this camera will be used instead of the camer...
Definition: CCamera.h:33
mrpt::gui::CDisplayWindow3D::Ptr
std::shared_ptr< CDisplayWindow3D > Ptr
Definition: CDisplayWindow3D.h:120
mrpt::system::COutputLogger::setMinLoggingLevel
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account.
Definition: COutputLogger.cpp:144
mrpt::system::TTimeStamp
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
mrpt::apps
Definition: BaseAppDataSource.h:15
argv
const char * argv[]
Definition: RawlogGrabberApp_unittest.cpp:40
mrpt::opengl::CCamera::setZoomDistance
void setZoomDistance(float z)
Definition: CCamera.h:63
mrpt::system::CTicTac::Tic
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
mrpt::slam::CMetricMapBuilderICP::getCurrentlyBuiltMapSize
unsigned int getCurrentlyBuiltMapSize() override
Returns just how many sensory-frames are stored in the currently build map.
Definition: CMetricMapBuilderICP.cpp:597
mrpt::config::CConfigFileBase::read_enum
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...
Definition: config/CConfigFileBase.h:269
mrpt::system::MRPT_getCompilationDate
std::string MRPT_getCompilationDate()
Returns the MRPT source code timestamp, according to the Reproducible-Builds specifications: https://...
Definition: os.cpp:154
mrpt::gui
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:14
mrpt::apps::BaseAppInitializableCLI::impl_get_usage
virtual std::string impl_get_usage() const =0
mrpt::system::deleteFilesInDirectory
bool deleteFilesInDirectory(const std::string &s, bool deleteDirectoryAsWell=false)
Delete all the files in a given directory (nothing done if directory does not exists,...
Definition: filesystem.cpp:218
mrpt::apps::ICP_SLAM_App_Live::ICP_SLAM_App_Live
ICP_SLAM_App_Live()
Definition: ICP_SLAM_App.cpp:497
mrpt::io::CFileOutputStream
This CStream derived class allow using a file as a write-only, binary stream.
Definition: io/CFileOutputStream.h:22
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
mrpt::opengl::COpenGLScene::Create
static Ptr Create(Args &&... args)
Definition: COpenGLScene.h:58
mrpt::apps::ICP_SLAM_App_Rawlog::impl_initialize
void impl_initialize(int argc, const char **argv) override
Definition: ICP_SLAM_App.cpp:477
ICP_SLAM_App.h
mrpt::apps::ICP_SLAM_App_Base::ICP_SLAM_App_Base
ICP_SLAM_App_Base()
Definition: ICP_SLAM_App.cpp:37
mrpt::apps::BaseAppDataSource::impl_get_next_observations
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.
mrpt::opengl::COpenGLScene::Ptr
std::shared_ptr< mrpt::opengl ::COpenGLScene > Ptr
Definition: COpenGLScene.h:58
mrpt::system::os::fprintf
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:408
mrpt::maps::CMultiMetricMap::saveMetricMapRepresentationToFile
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >,...
Definition: CMultiMetricMap.cpp:239
mrpt::opengl::stock_objects::RobotPioneer
CSetOfObjects::Ptr RobotPioneer()
Returns a representation of a Pioneer II mobile base.
Definition: StockObjects.cpp:30
mrpt::serialization::archiveFrom
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream,...
Definition: CArchive.h:592
mrpt::opengl::CPlanarLaserScan::Create
static Ptr Create(Args &&... args)
Definition: CPlanarLaserScan.h:61
mrpt::apps::ICP_SLAM_App_Live::impl_initialize
void impl_initialize(int argc, const char **argv) override
Definition: ICP_SLAM_App.cpp:504
argc
const int argc
Definition: RawlogGrabberApp_unittest.cpp:41
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
mrpt::opengl::CCamera::setElevationDegrees
void setElevationDegrees(float ang)
Definition: CCamera.h:68
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::maps::CSimpleMap
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
mrpt::system::os::getch
int getch() noexcept
An OS-independent version of getch, which waits until a key is pushed.
Definition: os.cpp:370
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::system::os::fopen
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
CRateTimer.h
mrpt::system::COutputLogger::getMinLoggingLevel
VerbosityLevel getMinLoggingLevel() const
Definition: system/COutputLogger.h:201
mrpt::io::CStream::printf
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
mrpt::maps
Definition: CBeacon.h:21
mrpt::maps::CMultiMetricMap::getAs3DObject
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CMultiMetricMap.cpp:261
mrpt::slam::CMetricMapBuilderICP::ICP_options
TConfigParams ICP_options
Options for the ICP-SLAM application.
Definition: CMetricMapBuilderICP.h:86
CPlanarLaserScan.h
mrpt::slam::CICP::TConfigParams::loadFromConfigFile
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
CArchive.h
mrpt::exception_to_str
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
mrpt::system::COutputLogger::setLoggerName
void setLoggerName(const std::string &name)
Set the name of the COutputLogger instance.
Definition: COutputLogger.cpp:138
mrpt::system::createDirectory
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:161
CObservationOdometry.h
ASSERT_FILE_EXISTS_
#define ASSERT_FILE_EXISTS_(FIL)
Definition: filesystem.h:22
CGridPlaneXY.h
mrpt::slam
Definition: CMultiMetricMapPDF.h:26
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
mrpt::slam::CMetricMapBuilder::saveCurrentMapToFile
void saveCurrentMapToFile(const std::string &fileName, bool compressGZ=true) const
Save map (mrpt::maps::CSimpleMap) to a ".simplemap" file.
Definition: CMetricMapBuilder.cpp:88
memory.h
mrpt::config::CLoadableOptions::dumpToTextStream
virtual void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form,...
Definition: CLoadableOptions.cpp:76
mrpt::system::MRPT_getVersion
std::string MRPT_getVersion()
Returns a string describing the MRPT version.
Definition: os.cpp:187
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::system
Definition: backtrace.h:14
mrpt::slam::CMetricMapBuilder::TOptions::alwaysInsertByClass
mrpt::rtti::CListOfClasses alwaysInsertByClass
A list of observation classes (derived from mrpt::obs::CObservation) which will be always inserted in...
Definition: CMetricMapBuilder.h:138



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020