MRPT  2.0.4
RBPF_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 
22 #include <mrpt/obs/CRawlog.h>
29 #include <mrpt/random.h>
30 #include <mrpt/system/filesystem.h> // ASSERT_FILE_EXISTS_()
31 #include <mrpt/system/memory.h> // getMemoryUsage()
32 
33 using namespace mrpt::apps;
34 
35 constexpr auto sect = "MappingApplication";
36 
37 // ---------------------------------------
38 // RBPF_SLAM_App_Base
39 // ---------------------------------------
41 {
42  // Set logger display name:
43  this->setLoggerName("RBPF_SLAM_App");
44 }
45 
46 void RBPF_SLAM_App_Base::initialize(int argc, const char** argv)
47 {
49 
51  " rbpf-slam - Part of the MRPT\n"
52  " MRPT C++ Library: %s - Sources timestamp: %s\n\n",
55 
56  // Process arguments:
57  if (argc < 2)
58  {
59  THROW_EXCEPTION_FMT("Usage: %s", impl_get_usage().c_str());
60  }
61 
62  // Config file:
63  const std::string configFile = std::string(argv[1]);
64 
65  ASSERT_FILE_EXISTS_(configFile);
67 
69 
70  MRPT_END
71 }
72 
74 {
76 
77  using namespace mrpt;
78  using namespace mrpt::slam;
79  using namespace mrpt::obs;
80  using namespace mrpt::maps;
81  using namespace mrpt::opengl;
82  using namespace mrpt::gui;
83  using namespace mrpt::io;
84  using namespace mrpt::gui;
85  using namespace mrpt::config;
86  using namespace mrpt::system;
87  using namespace mrpt::math;
88  using namespace mrpt::poses;
89  using namespace std;
90 
91  // ------------------------------------------
92  // Load config from file:
93  // ------------------------------------------
94 
95  int LOG_FREQUENCY = 5;
96  bool GENERATE_LOG_JOINT_H = false;
97  bool GENERATE_LOG_INFO = false;
98  bool SAVE_POSE_LOG = false;
99  bool SAVE_MAP_IMAGES = false;
100  bool SAVE_3D_SCENE = false;
101  bool CAMERA_3DSCENE_FOLLOWS_ROBOT = true;
102  bool SHOW_PROGRESS_IN_WINDOW = false;
103  int SHOW_PROGRESS_IN_WINDOW_DELAY_MS = 1;
104  std::string METRIC_MAP_CONTINUATION_GRIDMAP_FILE;
105  std::string SIMPLEMAP_CONTINUATION;
106  mrpt::math::TPose2D METRIC_MAP_CONTINUATION_START_POSE;
107  int PROGRESS_WINDOW_WIDTH = 600, PROGRESS_WINDOW_HEIGHT = 500;
108  int RANDOM_SEED = -1; // <0: randomize
109  const string OUT_DIR_STD =
110  params.read_string(sect, "logOutput_dir", "log_out", true);
111 
112  if (params.keyExists(sect, "verbosity"))
115 
116  // This to allow using the shorter XXX_CS() macros to read params:
117  {
118  const auto& c = params;
119  const auto& s = sect;
120 
121  MRPT_LOAD_CONFIG_VAR_CS(LOG_FREQUENCY, int);
122  MRPT_LOAD_CONFIG_VAR_CS(GENERATE_LOG_JOINT_H, bool);
123  MRPT_LOAD_CONFIG_VAR_CS(GENERATE_LOG_INFO, bool);
124  MRPT_LOAD_CONFIG_VAR_CS(SAVE_POSE_LOG, bool);
125  MRPT_LOAD_CONFIG_VAR_CS(SAVE_MAP_IMAGES, bool);
126  MRPT_LOAD_CONFIG_VAR_CS(SAVE_3D_SCENE, bool);
127  MRPT_LOAD_CONFIG_VAR_CS(CAMERA_3DSCENE_FOLLOWS_ROBOT, bool);
128  MRPT_LOAD_CONFIG_VAR_CS(SHOW_PROGRESS_IN_WINDOW, bool);
129  MRPT_LOAD_CONFIG_VAR_CS(SHOW_PROGRESS_IN_WINDOW_DELAY_MS, int);
130  MRPT_LOAD_CONFIG_VAR_CS(METRIC_MAP_CONTINUATION_GRIDMAP_FILE, string);
131  MRPT_LOAD_CONFIG_VAR_CS(SIMPLEMAP_CONTINUATION, string);
132  METRIC_MAP_CONTINUATION_START_POSE.x =
133  c.read_double(s, "METRIC_MAP_CONTINUATION_START_POSE_X", .0);
134  METRIC_MAP_CONTINUATION_START_POSE.y =
135  c.read_double(s, "METRIC_MAP_CONTINUATION_START_POSE_Y", .0);
136  METRIC_MAP_CONTINUATION_START_POSE.phi = DEG2RAD(
137  c.read_double(s, "METRIC_MAP_CONTINUATION_START_POSE_PHI_DEG", .0));
138  MRPT_LOAD_CONFIG_VAR_CS(PROGRESS_WINDOW_WIDTH, int);
139  MRPT_LOAD_CONFIG_VAR_CS(PROGRESS_WINDOW_HEIGHT, int);
140  MRPT_LOAD_CONFIG_VAR_CS(RANDOM_SEED, int);
141  }
142  const char* OUT_DIR = OUT_DIR_STD.c_str();
143 
144  // Print params:
145  MRPT_LOG_INFO_FMT("Output directory: `%s`", OUT_DIR);
146 
147  CTicTac tictac, tictacGlobal, tictac_JH;
148  int step = 0;
149  CSimpleMap finalMap;
150  float t_exec;
152 
153  char strFil[1000];
154 
155  // ---------------------------------
156  // MapPDF opts
157  // ---------------------------------
159  rbpfMappingOptions.loadFromConfigFile(params, sect);
160 
161  // ---------------------------------
162  // Constructor
163  // ---------------------------------
164  mapBuilder = std::make_shared<CMetricMapBuilderRBPF>(rbpfMappingOptions);
165  mapBuilder->setVerbosityLevel(this->getMinLoggingLevel());
166 
167  {
168  std::stringstream ss;
169  rbpfMappingOptions.dumpToTextStream(ss);
170  MRPT_LOG_INFO(ss.str());
171  }
172 
173  // handle the case of metric map continuation
174  if (!METRIC_MAP_CONTINUATION_GRIDMAP_FILE.empty())
175  {
176  CSimpleMap dummySimpleMap;
177  CPosePDFGaussian startPose;
178 
179  startPose.mean.x(METRIC_MAP_CONTINUATION_START_POSE.x);
180  startPose.mean.y(METRIC_MAP_CONTINUATION_START_POSE.y);
181  startPose.mean.phi(METRIC_MAP_CONTINUATION_START_POSE.phi);
182  startPose.cov.setZero();
183 
185  {
187  METRIC_MAP_CONTINUATION_GRIDMAP_FILE);
188  mrpt::serialization::archiveFrom(f) >> gridmap;
189  }
190 
191  mapBuilder->initialize(dummySimpleMap, &startPose);
192 
193  for (auto& m_particle : mapBuilder->mapPDF.m_particles)
194  {
195  CRBPFParticleData* part_d = m_particle.d.get();
196  CMultiMetricMap& mmap = part_d->mapTillNow;
199  ASSERTMSG_(
200  it_grid,
201  "No gridmap in multimetric map definition, but metric map "
202  "continuation was set (!)");
203  it_grid->copyMapContentFrom(gridmap);
204  }
205  }
206  if (!SIMPLEMAP_CONTINUATION.empty())
207  {
208  mrpt::maps::CSimpleMap init_map;
209  mrpt::io::CFileGZInputStream f(SIMPLEMAP_CONTINUATION);
210  mrpt::serialization::archiveFrom(f) >> init_map;
211  mapBuilder->initialize(init_map);
212  }
213 
214  // ---------------------------------
215  // CMetricMapBuilder::TOptions
216  // ---------------------------------
217  // mapBuilder->setVerbosityLevel( mrpt::system::LVL_DEBUG ); // default
218  // value: as loaded from config file
219  mapBuilder->options.enableMapUpdating = true;
220  mapBuilder->options.debugForceInsertion = false;
221 
222  auto& rng = mrpt::random::getRandomGenerator();
223  if (RANDOM_SEED >= 0)
224  rng.randomize(RANDOM_SEED);
225  else
226  rng.randomize();
227 
228  // Prepare output directory:
229  // --------------------------------
230  deleteFilesInDirectory(OUT_DIR);
231  createDirectory(OUT_DIR);
232 
233  string OUT_DIR_MAPS = format("%s/maps", OUT_DIR);
234  string OUT_DIR_3D = format("%s/3D", OUT_DIR);
235 
236  deleteFilesInDirectory(OUT_DIR_MAPS);
237  createDirectory(OUT_DIR_MAPS);
238 
239  deleteFilesInDirectory(OUT_DIR_3D);
240  createDirectory(OUT_DIR_3D);
241 
242  // Open log files:
243  // ----------------------------------
244  CFileOutputStream f_log(format("%s/log_times.txt", OUT_DIR));
245  CFileOutputStream f_info(format("%s/log_info.txt", OUT_DIR));
246  CFileOutputStream f_jinfo(format("%s/log_jinfo.txt", OUT_DIR));
247  CFileOutputStream f_path(format("%s/log_estimated_path.txt", OUT_DIR));
248  CFileOutputStream f_pathOdo(format("%s/log_odometry_path.txt", OUT_DIR));
249  CFileOutputStream f_partStats(format("%s/log_ParticlesStats.txt", OUT_DIR));
250 
251  f_log.printf(
252  "%% time_step execution_time(ms) map_size(#frames) frame_inserted? "
253  "\n"
254  "%%-------------------------------------------------------------------"
255  "\n");
256 
257  f_info.printf(
258  "%% EMI H EMMI effecMappedArea effecMappedCells \n"
259  "%%-------------------------------------------------------\n");
260 
261  f_pathOdo.printf(
262  "%% time_step x y z yaw pitch roll timestamp \n"
263  "%%--------------------------------------------\n");
264 
265  f_pathOdo.printf(
266  "%% time_step x y z yaw pitch roll \n"
267  "%%----------------------------------\n");
268 
269  f_partStats.printf(
270  "%% time_step #particles ESS \n"
271  "%%------------------------------\n");
272 
273  // ----------------------------------------------------------
274  // Map Building
275  // ----------------------------------------------------------
276  CPose3D odoPose(0, 0, 0);
277 
278  CDisplayWindow3D::Ptr win3D;
279 
280  if (SHOW_PROGRESS_IN_WINDOW)
281  {
282  win3D = CDisplayWindow3D::Create(
283  "RBPF-SLAM @ MRPT C++ Library", PROGRESS_WINDOW_WIDTH,
284  PROGRESS_WINDOW_HEIGHT);
285  win3D->setCameraZoom(40);
286  win3D->setCameraAzimuthDeg(-50);
287  win3D->setCameraElevationDeg(70);
288  }
289 
290  tictacGlobal.Tic();
291  for (;;)
292  {
293  if (quits_with_esc_key && os::kbhit())
294  {
295  char c = os::getch();
296  if (c == 27) break;
297  }
298 
299  CActionCollection::Ptr action;
300  CSensoryFrame::Ptr observations;
301  CObservation::Ptr observation;
302 
303  // Load action/observation pair from the rawlog:
304  // --------------------------------------------------
305  if (!impl_get_next_observations(action, observations, observation))
306  break; // EOF
307 
308  // Update odometry:
309  {
311  action->getBestMovementEstimation();
312  if (act)
313  odoPose = odoPose + CPose3D(act->poseChange->getMeanVal());
314  else
315  {
317  action->getActionByClass<CActionRobotMovement3D>();
318  if (act3D) odoPose = odoPose + act3D->poseChange.mean;
319  }
320  }
321 
322  mrpt::system::TTimeStamp observations_timestamp;
323  if (observations && !observations->empty())
324  observations_timestamp = (*observations->begin())->timestamp;
325 
326  // Execute:
327  // ----------------------------------------
328  tictac.Tic();
329  mapBuilder->processActionObservation(*action, *observations);
330  t_exec = tictac.Tac();
331  MRPT_LOG_INFO_FMT("Map building executed in %.03fms", 1000.0f * t_exec);
332 
333  // Info log:
334  // -----------
335  f_log.printf(
336  "%u %f %i %i\n", static_cast<unsigned int>(step), 1000.0f * t_exec,
337  mapBuilder->getCurrentlyBuiltMapSize(),
338  mapBuilder->m_statsLastIteration.observationsInserted ? int(1)
339  : int(0));
340 
341  CPose3DPDF::Ptr curPDFptr = mapBuilder->getCurrentPoseEstimation();
342  CPose3DPDFParticles curPDF;
343 
344  if (IS_CLASS(*curPDFptr, CPose3DPDFParticles))
345  {
347  std::dynamic_pointer_cast<CPose3DPDFParticles>(curPDFptr);
348  curPDF = *pp;
349  }
350 
351  if (LOG_FREQUENCY > 0 && 0 == (step % LOG_FREQUENCY))
352  {
353  const CMultiMetricMap* mostLikMap =
354  mapBuilder->mapPDF.getCurrentMostLikelyMetricMap();
355 
356  if (GENERATE_LOG_INFO)
357  {
358  tictac_JH.Tic();
359 
360  const CMultiMetricMap* avrMap =
361  mapBuilder->mapPDF.getAveragedMetricMapEstimation();
363  avrMap->mapByClass<COccupancyGridMap2D>();
364  ASSERT_(grid);
365  grid->computeEntropy(entropy);
366 
367  grid->saveAsBitmapFile(
368  format("%s/EMMI_gridmap_%03u.png", OUT_DIR, step));
369 
370  f_info.printf(
371  "%f %f %f %f %lu\n", entropy.I, entropy.H, entropy.mean_I,
372  entropy.effectiveMappedArea, entropy.effectiveMappedCells);
374  "Log information saved. EMI = %.04f EMMI=%.04f (in "
375  "%.03fms)\n",
376  entropy.I, entropy.mean_I, 1000.0f * tictac_JH.Tac());
377  }
378 
379  // Pose log:
380  // -------------
381  if (SAVE_POSE_LOG)
382  {
383  curPDF.saveToTextFile(
384  format("%s/mapbuild_posepdf_%03u.txt", OUT_DIR, step));
385  }
386 
387  // Map images:
388  // -------------
389  if (SAVE_MAP_IMAGES)
390  {
391  MRPT_LOG_DEBUG("Saving map images to files...");
392 
393  // Most likely maps:
394  // ----------------------------------------
396  format("%s/mapbuilt_%05u_", OUT_DIR_MAPS.c_str(), step));
397 
398  if (mostLikMap->countMapsByClass<COccupancyGridMap2D>() > 0)
399  {
400  mrpt::img::CImage img;
401  mapBuilder->drawCurrentEstimationToImage(&img);
402  img.saveToFile(
403  format("%s/mapping_%05u.png", OUT_DIR, step));
404  }
405  }
406 
407  // Save a 3D scene view of the mapping process:
408  COpenGLScene::Ptr scene;
409  if (SAVE_3D_SCENE || SHOW_PROGRESS_IN_WINDOW)
410  {
411  scene = std::make_shared<COpenGLScene>();
412 
413  // The ground:
414  mrpt::opengl::CGridPlaneXY::Ptr groundPlane =
416  -200, 200, -200, 200, 0, 5);
417  groundPlane->setColor(0.4f, 0.4f, 0.4f);
418  scene->insert(groundPlane);
419 
420  // The camera pointing to the current robot pose:
421  if (CAMERA_3DSCENE_FOLLOWS_ROBOT)
422  {
425  CPose3D robotPose;
426  curPDF.getMean(robotPose);
427 
428  objCam->setPointingAt(robotPose);
429  objCam->setAzimuthDegrees(-30);
430  objCam->setElevationDegrees(30);
431  scene->insert(objCam);
432  }
433  // Draw the map(s):
436  mostLikMap->getAs3DObject(objs);
437  scene->insert(objs);
438 
439  // Draw the robot particles:
440  size_t M = mapBuilder->mapPDF.particlesCount();
443  objLines->setColor(0, 1, 1);
444  for (size_t i = 0; i < M; i++)
445  {
446  std::deque<TPose3D> path;
447  mapBuilder->mapPDF.getPath(i, path);
448 
449  float x0 = 0, y0 = 0, z0 = 0;
450  for (auto& k : path)
451  {
452  objLines->appendLine(
453  x0, y0, z0 + 0.001, k.x, k.y, k.z + 0.001);
454  x0 = k.x;
455  y0 = k.y;
456  z0 = k.z;
457  }
458  }
459  scene->insert(objLines);
460 
461  // An ellipsoid:
462  CPose3D lastMeanPose;
463  float minDistBtwPoses = -1;
464  std::deque<TPose3D> dummyPath;
465  mapBuilder->mapPDF.getPath(0, dummyPath);
466  for (int k = (int)dummyPath.size() - 1; k >= 0; k--)
467  {
468  CPose3DPDFParticles poseParts;
469  mapBuilder->mapPDF.getEstimatedPosePDFAtTime(k, poseParts);
470 
471  const auto [COV, meanPose] =
472  poseParts.getCovarianceAndMean();
473 
474  if (meanPose.distanceTo(lastMeanPose) > minDistBtwPoses)
475  {
476  CMatrixDouble33 COV3 = COV.blockCopy<3, 3>(0, 0);
477 
478  minDistBtwPoses = 6 * sqrt(COV3(0, 0) + COV3(1, 1));
479 
480  if (COV3(2, 2) == 0)
481  {
482  auto objEllip = opengl::CEllipsoid2D::Create();
483  objEllip->setLocation(
484  meanPose.x(), meanPose.y(),
485  meanPose.z() + 0.001);
486  objEllip->setCovMatrix(COV3.blockCopy<2, 2>());
487  objEllip->setColor(0, 0, 1);
488  objEllip->enableDrawSolid3D(false);
489  scene->insert(objEllip);
490  }
491  else
492  {
493  auto objEllip = opengl::CEllipsoid3D::Create();
494  objEllip->setLocation(
495  meanPose.x(), meanPose.y(),
496  meanPose.z() + 0.001);
497  objEllip->setCovMatrix(COV3);
498  objEllip->setColor(0, 0, 1);
499  objEllip->enableDrawSolid3D(false);
500  scene->insert(objEllip);
501  }
502 
503  lastMeanPose = meanPose;
504  }
505  }
506  } // end if show or save 3D scene->
507 
508  if (SAVE_3D_SCENE)
509  { // Save as file:
511  "%s/buildingmap_%05u.3Dscene", OUT_DIR_3D.c_str(), step));
513  }
514 
515  if (SHOW_PROGRESS_IN_WINDOW)
516  {
517  COpenGLScene::Ptr& scenePtr = win3D->get3DSceneAndLock();
518  scenePtr = scene;
519  win3D->unlockAccess3DScene();
520 
521  win3D->forceRepaint();
522  int add_delay =
523  SHOW_PROGRESS_IN_WINDOW_DELAY_MS - t_exec * 1000;
524  if (add_delay > 0)
525  std::this_thread::sleep_for(
526  std::chrono::milliseconds(add_delay));
527  }
528 
529  // Save the weighted entropy of each map:
530  // ----------------------------------------
531  if (GENERATE_LOG_JOINT_H)
532  {
533  tictac_JH.Tic();
534 
535  double H_joint = mapBuilder->getCurrentJointEntropy();
536  double H_path = mapBuilder->mapPDF.getCurrentEntropyOfPaths();
537  f_jinfo.printf("%e %e\n", H_joint, H_path);
539  "Saving joing H info. joint-H=%f\t(in %.03fms)", H_joint,
540  1000.0f * tictac_JH.Tac());
541  }
542 
543  } // end of LOG_FREQ
544 
545  // Save the memory usage:
546  // ------------------------------------------------------------------
547  {
548  unsigned long memUsage = mrpt::system::getMemoryUsage();
549  FILE* f = os::fopen(
550  format("%s/log_MemoryUsage.txt", OUT_DIR).c_str(), "at");
551  if (f)
552  {
553  os::fprintf(f, "%u\t%lu\n", step, memUsage);
554  os::fclose(f);
555  }
557  "Saving memory usage: %.04f MiB", memUsage / (1024.0 * 1024.0));
558  }
559 
560  // Save the parts stats:
561  f_partStats.printf(
562  "%u %u %f\n", (unsigned int)step, (unsigned int)curPDF.size(),
563  curPDF.ESS());
564 
565  // Save the robot estimated pose for each step:
566  CPose3D meanPose;
567  mapBuilder->getCurrentPoseEstimation()->getMean(meanPose);
568 
569  f_path.printf(
570  "%u %f %f %f %f %f %f %f\n", (unsigned int)step, meanPose.x(),
571  meanPose.y(), meanPose.z(), meanPose.yaw(), meanPose.pitch(),
572  meanPose.roll(), mrpt::Clock::toDouble(observations_timestamp));
573 
574  // Also keep the robot path as a vector, for the convenience of the app
575  // user:
576  out_estimated_path[observations_timestamp] = meanPose.asTPose();
577 
578  f_pathOdo.printf(
579  "%i\t%f\t%f\t%f\t%f\t%f\t%f\n", step, odoPose.x(), odoPose.y(),
580  odoPose.z(), odoPose.yaw(), odoPose.pitch(), odoPose.roll());
581 
582  step++;
583  MRPT_LOG_INFO_FMT("------------- STEP %u ----------------", step);
584 
585  }; // end while
586 
588  "----------- **END** (total time: %.03f sec) ---------",
589  tictacGlobal.Tac());
590 
591  // Save map:
592  mapBuilder->getCurrentlyBuiltMap(finalMap);
593 
594  CFileOutputStream filOut(format("%s/_finalmap_.simplemap", OUT_DIR));
595  mrpt::serialization::archiveFrom(filOut) << finalMap;
596 
597  // Save gridmap extend (if exists):
598  const CMultiMetricMap* mostLikMap =
599  mapBuilder->mapPDF.getCurrentMostLikelyMetricMap();
601  format("%s/finalMap", OUT_DIR));
602 
603  // Save the most likely path of the particle set
604  FILE* f_pathPart;
605 
606  os::sprintf(strFil, 1000, "%s/most_likely_path.txt", OUT_DIR);
607  f_pathPart = os::fopen(strFil, "wt");
608 
609  ASSERT_(f_pathPart != nullptr);
610 
611  std::deque<TPose3D> outPath;
612  std::deque<TPose3D>::iterator itPath;
613 
614  mapBuilder->getCurrentMostLikelyPath(outPath);
615 
616  for (itPath = outPath.begin(); itPath != outPath.end(); itPath++)
617  os::fprintf(
618  f_pathPart, "%.3f %.3f %.3f\n", itPath->x, itPath->y, itPath->yaw);
619 
620  os::fclose(f_pathPart);
621 
622  // Close 3D window, if any:
623  if (win3D) win3D->waitForKey();
624 
625  MRPT_END
626 }
627 
628 // ---------------------------------------
629 // RBPF_SLAM_App_Rawlog
630 // ---------------------------------------
632 {
633  setLoggerName("RBPF_SLAM_App_Rawlog");
634 }
635 
637 {
638  MRPT_START
639  // Rawlog file: from args. line or from config file:
640  if (argc == 3)
641  m_rawlogFileName = std::string(argv[2]);
642  else
643  m_rawlogFileName = params.read_string(
644  sect, "rawlog_file", std::string("log.rawlog"), true);
645 
646  m_rawlog_offset = params.read_int(sect, "rawlog_offset", 0, true);
647 
648  ASSERT_FILE_EXISTS_(m_rawlogFileName);
649 
650  // Set relative path for externally-stored images in rawlogs:
651  std::string rawlog_images_path =
652  mrpt::system::extractFileDirectory(m_rawlogFileName);
653  rawlog_images_path += "/Images";
654  mrpt::img::CImage::setImagesPathBase(rawlog_images_path);
655 
656  MRPT_END
657 }
CSetOfLines.h
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
mrpt::obs::CObservation::Ptr
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:45
filesystem.h
apps-precomp.h
mrpt::apps::RBPF_SLAM_App_Base::run
void run()
Runs with the current parameter set.
Definition: RBPF_SLAM_App.cpp:73
mrpt::poses::CPose3DPDFParticles::saveToTextFile
bool saveToTextFile(const std::string &file) const override
Save PDF's m_particles to a text file.
Definition: CPose3DPDFParticles.cpp:223
mrpt::opengl::CCamera::Create
static Ptr Create(Args &&... args)
Definition: CCamera.h:36
MRPT_LOG_INFO_FMT
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
Definition: system/COutputLogger.h:463
COccupancyGridMap2D.h
mrpt::apps::RBPF_SLAM_App_Rawlog::impl_initialize
void impl_initialize(int argc, const char **argv) override
Definition: RBPF_SLAM_App.cpp:636
mrpt::system::os::fclose
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:286
mrpt::img::CImage::setImagesPathBase
static void setImagesPathBase(const std::string &path)
Definition: CImage.cpp:80
mrpt::system::extractFileDirectory
std::string extractFileDirectory(const std::string &filePath)
Extract the whole path (the directory) of a filename from a complete path plus name plus extension.
Definition: filesystem.cpp:78
mrpt::opengl::CSetOfObjects::Create
static Ptr Create(Args &&... args)
Definition: CSetOfObjects.h:28
mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions::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: CMetricMapBuilderRBPF.cpp:553
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::opengl::CSetOfObjects::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
Definition: CSetOfObjects.h:28
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::maps::CRBPFParticleData
Auxiliary class used in mrpt::maps::CMultiMetricMapPDF.
Definition: CMultiMetricMapPDF.h:35
mrpt::poses::CPose3D::pitch
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:552
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_LOG_DEBUG
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
Definition: system/COutputLogger.h:427
CObservationGasSensors.h
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
Definition: CSensoryFrame.h:53
mrpt::math::TPose2D::phi
double phi
Orientation (rads)
Definition: TPose2D.h:32
mrpt::poses::CPose2D::phi
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
CFileOutputStream.h
RBPF_SLAM_App.h
mrpt::maps::COccupancyGridMap2D::TEntropyInfo::mean_I
double mean_I
The target variable for mean information, defined as information per cell: mean_I(map) = I(map) / (ce...
Definition: COccupancyGridMap2D.h:455
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::poses::CPose3DPDFParticles::size
size_t size() const
Get the m_particles count (equivalent to "particlesCount")
Definition: CPose3DPDFParticles.h:92
mrpt::poses::CPosePDFGaussian::cov
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Definition: CPosePDFGaussian.h:46
CFileGZOutputStream.h
mrpt::obs::CActionRobotMovement3D
Represents a probabilistic 3D (6D) movement.
Definition: CActionRobotMovement3D.h:26
mrpt::poses::CPose3DPDFParticles::Ptr
std::shared_ptr< mrpt::poses ::CPose3DPDFParticles > Ptr
Definition: CPose3DPDFParticles.h:39
CObservationWirelessPower.h
mrpt::math::TPose2D::y
double y
Definition: TPose2D.h:30
mrpt::obs::CActionCollection::Ptr
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
Definition: CActionCollection.h:28
mrpt::opengl::CCamera::Ptr
std::shared_ptr< mrpt::opengl ::CCamera > Ptr
Definition: CCamera.h:36
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::obs::CActionRobotMovement3D::Ptr
std::shared_ptr< mrpt::obs ::CActionRobotMovement3D > Ptr
Definition: CActionRobotMovement3D.h:28
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
sect
constexpr auto sect
Definition: RBPF_SLAM_App.cpp:35
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::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
random.h
mrpt::opengl::CSetOfLines::Create
static Ptr Create(Args &&... args)
Definition: CSetOfLines.h:35
mrpt::poses::CPosePDFGaussian::mean
CPose2D mean
The mean value.
Definition: CPosePDFGaussian.h:44
mrpt::system::CTicTac::Tac
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:87
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::system::os::sprintf
int sprintf(char *buf, size_t bufSize, const char *format,...) noexcept MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
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
CActionRobotMovement2D.h
mrpt::apps::RBPF_SLAM_App_Base::RBPF_SLAM_App_Base
RBPF_SLAM_App_Base()
Definition: RBPF_SLAM_App.cpp:40
mrpt::Clock::toDouble
static double toDouble(const time_point t) noexcept
Converts a timestamp to a UNIX time_t-like number, with fractional part.
Definition: Clock.cpp:106
mrpt::math::CMatrixFixed< double, 3, 3 >
CEllipsoid3D.h
mrpt::io::CFileGZInputStream
Transparently opens a compressed "gz" file and reads uncompressed data from it.
Definition: io/CFileGZInputStream.h:26
mrpt::maps::COccupancyGridMap2D::TEntropyInfo::H
double H
The target variable for absolute entropy, computed as:
Definition: COccupancyGridMap2D.h:446
MRPT_LOAD_CONFIG_VAR_CS
#define MRPT_LOAD_CONFIG_VAR_CS(variableName, variableType)
Shortcut for MRPT_LOAD_CONFIG_VAR() for config file object named c and section string named s
Definition: config/CConfigFileBase.h:315
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
mrpt::io::CFileGZOutputStream
Saves data to a file and transparently compress the data using the given compression level.
Definition: io/CFileGZOutputStream.h:26
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: CMultiMetricMap.h:120
mrpt::apps::RBPF_SLAM_App_Base::params
mrpt::config::CConfigFileMemory params
Populated in initialize().
Definition: RBPF_SLAM_App.h:62
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::opengl::CSetOfLines::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfLines > Ptr
Definition: CSetOfLines.h:35
mrpt::poses::CPose3D::asTPose
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:759
CActionRobotMovement3D.h
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
mrpt::apps::RBPF_SLAM_App_Base::initialize
void initialize(int argc, const char **argv)
Initializes the application from CLI parameters.
Definition: RBPF_SLAM_App.cpp:46
mrpt::poses::CPose3D::roll
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
mrpt::gui::CDisplayWindow3D::Ptr
std::shared_ptr< CDisplayWindow3D > Ptr
Definition: CDisplayWindow3D.h:120
mrpt::maps::COccupancyGridMap2D::TEntropyInfo::effectiveMappedCells
unsigned long effectiveMappedCells
The mapped area in cells.
Definition: COccupancyGridMap2D.h:460
mrpt::opengl::CEllipsoid2D::Create
static Ptr Create(Args &&... args)
Definition: CEllipsoid2D.h:43
mrpt::maps::COccupancyGridMap2D::TEntropyInfo::I
double I
The target variable for absolute "information", defining I(x) = 1 - H(x)
Definition: COccupancyGridMap2D.h:449
mrpt::poses::CPose3DPDFParticles::getCovarianceAndMean
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once.
Definition: CPose3DPDFParticles.cpp:70
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: TPose2D.h:22
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
mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CMetricMapBuilderRBPF.cpp:517
argv
const char * argv[]
Definition: RawlogGrabberApp_unittest.cpp:50
mrpt::poses::CPose3DPDFParticles
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
Definition: CPose3DPDFParticles.h:29
mrpt::system::CTicTac::Tic
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:76
mrpt::apps::RBPF_SLAM_App_Base::mapBuilder
std::shared_ptr< mrpt::slam::CMetricMapBuilderRBPF > mapBuilder
Definition: RBPF_SLAM_App.h:72
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::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
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::io::CFileOutputStream
This CStream derived class allow using a file as a write-only, binary stream.
Definition: io/CFileOutputStream.h:22
mrpt::img::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
mrpt::maps::CRBPFParticleData::mapTillNow
CMultiMetricMap mapTillNow
Definition: CMultiMetricMapPDF.h:45
mrpt::img::CImage::saveToFile
bool saveToFile(const std::string &fileName, int jpeg_quality=95) const
Save the image to a file, whose format is determined from the extension (internally uses OpenCV).
Definition: CImage.cpp:329
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::maps::COccupancyGridMap2D::TEntropyInfo
Used for returning entropy related information.
Definition: COccupancyGridMap2D.h:441
mrpt::math::MatrixBase::blockCopy
CMatrixFixed< Scalar, BLOCK_ROWS, BLOCK_COLS > blockCopy(int start_row=0, int start_col=0) const
const blockCopy(): Returns a copy of the given block
Definition: MatrixBase.h:233
mrpt::opengl::COpenGLScene::Ptr
std::shared_ptr< mrpt::opengl ::COpenGLScene > Ptr
Definition: COpenGLScene.h:58
mrpt::maps::COccupancyGridMap2D::TEntropyInfo::effectiveMappedArea
double effectiveMappedArea
The target variable for the area of cells with information, i.e.
Definition: COccupancyGridMap2D.h:458
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::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::apps::RBPF_SLAM_App_Base::out_estimated_path
std::map< mrpt::system::TTimeStamp, mrpt::math::TPose3D > out_estimated_path
Definition: RBPF_SLAM_App.h:74
CEllipsoid2D.h
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
mrpt::maps::CMultiMetricMap::countMapsByClass
std::size_t countMapsByClass() const
Count how many maps exist of the given class (or derived class)
Definition: CMultiMetricMap.h:169
CPosePDFGaussian.h
argc
const int argc
Definition: RawlogGrabberApp_unittest.cpp:51
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
mrpt::math::MatrixVectorBase::setZero
void setZero()
Definition: MatrixVectorBase.h:112
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::apps::RBPF_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: RBPF_SLAM_App.h:66
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::poses::CPose3DPDF::Ptr
std::shared_ptr< CPose3DPDF > Ptr
Definition: CPose3DPDF.h:42
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: COccupancyGridMap2D.h:53
mrpt::poses::CPose3DPDFParticles::getMean
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF),...
Definition: CPose3DPDFParticles.cpp:52
mrpt::system::os::fopen
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:268
mrpt::maps::COccupancyGridMap2D::Ptr
std::shared_ptr< mrpt::maps ::COccupancyGridMap2D > Ptr
Definition: COccupancyGridMap2D.h:57
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::poses::CPosePDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
Definition: CPosePDFGaussian.h:28
mrpt::opengl::CEllipsoid3D::Create
static Ptr Create(Args &&... args)
Definition: CEllipsoid3D.h:42
mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions
Options for building a CMetricMapBuilderRBPF object, passed to the constructor.
Definition: CMetricMapBuilderRBPF.h:82
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
ASSERT_FILE_EXISTS_
#define ASSERT_FILE_EXISTS_(FIL)
Definition: filesystem.h:22
CRawlog.h
CGridPlaneXY.h
mrpt::slam
Definition: CMultiMetricMapPDF.h:26
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
mrpt::bayes::CParticleFilterDataImpl::ESS
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
Definition: CParticleFilterData.h:85
memory.h
mrpt::apps::RBPF_SLAM_App_Rawlog::RBPF_SLAM_App_Rawlog
RBPF_SLAM_App_Rawlog()
Definition: RBPF_SLAM_App.cpp:631
mrpt::math::TPose2D::x
double x
X,Y coordinates.
Definition: TPose2D.h:30
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



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 17:54:30 UTC 2020