MRPT  2.0.4
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 }
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:403
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:286
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
mrpt::apps::ICP_SLAM_App_Rawlog::ICP_SLAM_App_Rawlog
ICP_SLAM_App_Rawlog()
Definition: ICP_SLAM_App.cpp:473
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
thread_name.h
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:594
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:540
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:71
sect
constexpr auto sect
Definition: ICP_SLAM_App.cpp:33
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
vector_loadsave.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:87
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::system::thread_name
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
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: vector_loadsave.cpp:72
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:44
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:30
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:76
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:165
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:498
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:478
ICP_SLAM_App.h
mrpt::apps::ICP_SLAM_App_Base::ICP_SLAM_App_Base
ICP_SLAM_App_Base()
Definition: ICP_SLAM_App.cpp:38
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:419
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:505
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:381
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:268
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:198
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.4 at Sat Jun 27 14:00:59 UTC 2020