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 }
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::TPose3D asTPose() const
Definition: CPose3D.cpp:759
std::size_t countMapsByClass() const
Count how many maps exist of the given class (or derived class)
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
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
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:87
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:161
#define MRPT_START
Definition: exceptions.h:241
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
CPose2D mean
The mean value.
VerbosityLevel
Enumeration of available verbosity levels.
void run()
Runs with the current parameter set.
bool saveToTextFile(const std::string &file) const override
Save PDF&#39;s m_particles to a text file.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
double x
X,Y coordinates.
Definition: TPose2D.h:30
int getch() noexcept
An OS-independent version of getch, which waits until a key is pushed.
Definition: os.cpp:381
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
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:286
double effectiveMappedArea
The target variable for the area of cells with information, i.e.
#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 ...
A high-performance stopwatch, with typical resolution of nanoseconds.
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account...
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:552
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:165
T::Ptr mapByClass(size_t ith=0) const
Returns the i&#39;th map of a given class (or of a derived class), or empty smart pointer if there is no ...
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
static Ptr Create(Args &&... args)
Definition: CEllipsoid2D.h:43
virtual std::string impl_get_usage() const =0
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
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...
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:592
VerbosityLevel getMinLoggingLevel() const
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
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
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
Represents a probabilistic 3D (6D) movement.
double H
The target variable for absolute entropy, computed as: H(map)=Sumx,y{ -p(x,y)*ln(p(x,y)) -(1-p(x,y))*ln(1-p(x,y)) }
This base provides a set of functions for maths stuff.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
void initialize(int argc, const char **argv)
Initializes the application from CLI parameters.
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
constexpr double DEG2RAD(const double x)
Degrees to radians.
void setLoggerName(const std::string &name)
Set the name of the COutputLogger instance.
double I
The target variable for absolute "information", defining I(x) = 1 - H(x)
This namespace contains representation of robot actions and observations.
size_t size() const
Get the m_particles count (equivalent to "particlesCount")
#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
#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
This CStream derived class allow using a file as a write-only, binary stream.
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...
std::shared_ptr< mrpt::slam::CMetricMapBuilderRBPF > mapBuilder
Definition: RBPF_SLAM_App.h:72
bool keyExists(const std::string &section, const std::string &key) const
Checks if a given key exists inside a section (case insensitive)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
Auxiliary class used in mrpt::maps::CMultiMetricMapPDF.
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.
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:419
A class for storing an occupancy grid map.
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.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const char * argv[]
unsigned long getMemoryUsage()
Returns the memory occupied by this process, in bytes.
Definition: memory.cpp:114
unsigned long effectiveMappedCells
The mapped area in cells.
mrpt::config::CConfigFileMemory params
Populated in initialize().
Definition: RBPF_SLAM_App.h:62
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
static Ptr Create(Args &&... args)
Definition: CEllipsoid3D.h:42
void setContent(const std::vector< std::string > &stringList)
Changes the contents of the virtual "config file".
#define MRPT_END
Definition: exceptions.h:245
Lightweight 2D pose.
Definition: TPose2D.h:22
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
bool kbhit() noexcept
An OS-independent version of kbhit, which returns true if a key has been pushed.
Definition: os.cpp:403
std::string file_get_contents(const std::string &fileName)
Loads an entire text file and return its contents as a single std::string.
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
Transparently opens a compressed "gz" file and reads uncompressed data from it.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:268
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:14
const int argc
void impl_initialize(int argc, const char **argv) override
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
constexpr auto sect
std::string MRPT_getVersion()
Returns a string describing the MRPT version.
Definition: os.cpp:198
Saves data to a file and transparently compress the data using the given compression level...
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:76
This class stores any customizable set of metric maps.
#define ASSERT_FILE_EXISTS_(FIL)
Definition: filesystem.h:22
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
double mean_I
The target variable for mean information, defined as information per cell: mean_I(map) = I(map) / (ce...
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
std::map< mrpt::system::TTimeStamp, mrpt::math::TPose3D > out_estimated_path
Definition: RBPF_SLAM_App.h:74
Used for returning entropy related information.
static void setImagesPathBase(const std::string &path)
Definition: CImage.cpp:80
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:31
double phi
Orientation (rads)
Definition: TPose2D.h:32
Options for building a CMetricMapBuilderRBPF object, passed to the constructor.
static Ptr Create(Args &&... args)
Definition: CCamera.h:36
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
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
static Ptr Create(Args &&... args)
Definition: CSetOfLines.h:35
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
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...
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
virtual void impl_initialize(int argc, const char **argv)=0
#define MRPT_LOG_INFO(_STRING)



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020