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