MRPT  1.9.9
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-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 
22 #include <mrpt/obs/CRawlog.h>
23 #include <mrpt/opengl/CEllipsoid.h>
28 #include <mrpt/random.h>
29 #include <mrpt/system/filesystem.h> // ASSERT_FILE_EXISTS_()
30 #include <mrpt/system/memory.h> // getMemoryUsage()
31 
32 using namespace mrpt::apps;
33 
34 constexpr auto sect = "MappingApplication";
35 
36 // ---------------------------------------
37 // RBPF_SLAM_App_Base
38 // ---------------------------------------
40 {
41  // Set logger display name:
42  this->setLoggerName("RBPF_SLAM_App");
43 }
44 
45 void RBPF_SLAM_App_Base::initialize(int argc, const char** argv)
46 {
48 
50  " rbpf-slam - Part of the MRPT\n"
51  " MRPT C++ Library: %s - Sources timestamp: %s\n\n",
54 
55  // Process arguments:
56  if (argc < 2)
57  {
58  THROW_EXCEPTION_FMT("Usage: %s", impl_get_usage().c_str());
59  }
60 
61  // Config file:
62  const std::string configFile = std::string(argv[1]);
63 
64  ASSERT_FILE_EXISTS_(configFile);
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  int LOG_FREQUENCY = 5;
95  bool GENERATE_LOG_JOINT_H = false;
96  bool GENERATE_LOG_INFO = false;
97  bool SAVE_POSE_LOG = false;
98  bool SAVE_MAP_IMAGES = false;
99  bool SAVE_3D_SCENE = false;
100  bool CAMERA_3DSCENE_FOLLOWS_ROBOT = true;
101  bool SHOW_PROGRESS_IN_WINDOW = false;
102  int SHOW_PROGRESS_IN_WINDOW_DELAY_MS = 1;
103  std::string METRIC_MAP_CONTINUATION_GRIDMAP_FILE;
104  std::string SIMPLEMAP_CONTINUATION;
105  mrpt::math::TPose2D METRIC_MAP_CONTINUATION_START_POSE;
106  int PROGRESS_WINDOW_WIDTH = 600, PROGRESS_WINDOW_HEIGHT = 500;
107  int RANDOM_SEED = -1; // <0: randomize
108  const string OUT_DIR_STD =
109  params.read_string(sect, "logOutput_dir", "log_out", true);
110 
111  // This to allow using the shorter XXX_CS() macros to read params:
112  {
113  const auto& c = params;
114  const auto& s = sect;
115 
116  MRPT_LOAD_CONFIG_VAR_CS(LOG_FREQUENCY, int);
117  MRPT_LOAD_CONFIG_VAR_CS(GENERATE_LOG_JOINT_H, bool);
118  MRPT_LOAD_CONFIG_VAR_CS(GENERATE_LOG_INFO, bool);
119  MRPT_LOAD_CONFIG_VAR_CS(SAVE_POSE_LOG, bool);
120  MRPT_LOAD_CONFIG_VAR_CS(SAVE_MAP_IMAGES, bool);
121  MRPT_LOAD_CONFIG_VAR_CS(SAVE_3D_SCENE, bool);
122  MRPT_LOAD_CONFIG_VAR_CS(CAMERA_3DSCENE_FOLLOWS_ROBOT, bool);
123  MRPT_LOAD_CONFIG_VAR_CS(SHOW_PROGRESS_IN_WINDOW, bool);
124  MRPT_LOAD_CONFIG_VAR_CS(SHOW_PROGRESS_IN_WINDOW_DELAY_MS, int);
125  MRPT_LOAD_CONFIG_VAR_CS(METRIC_MAP_CONTINUATION_GRIDMAP_FILE, string);
126  MRPT_LOAD_CONFIG_VAR_CS(SIMPLEMAP_CONTINUATION, string);
127  METRIC_MAP_CONTINUATION_START_POSE.x =
128  c.read_double(s, "METRIC_MAP_CONTINUATION_START_POSE_X", .0);
129  METRIC_MAP_CONTINUATION_START_POSE.y =
130  c.read_double(s, "METRIC_MAP_CONTINUATION_START_POSE_Y", .0);
131  METRIC_MAP_CONTINUATION_START_POSE.phi = DEG2RAD(
132  c.read_double(s, "METRIC_MAP_CONTINUATION_START_POSE_PHI_DEG", .0));
133  MRPT_LOAD_CONFIG_VAR_CS(PROGRESS_WINDOW_WIDTH, int);
134  MRPT_LOAD_CONFIG_VAR_CS(PROGRESS_WINDOW_HEIGHT, int);
135  MRPT_LOAD_CONFIG_VAR_CS(RANDOM_SEED, int);
136  }
137  const char* OUT_DIR = OUT_DIR_STD.c_str();
138 
139  // Print params:
140  MRPT_LOG_INFO_FMT("Output directory: `%s`", OUT_DIR);
141 
142  CTicTac tictac, tictacGlobal, tictac_JH;
143  int step = 0;
144  CSimpleMap finalMap;
145  float t_exec;
147 
148  char strFil[1000];
149 
150  // ---------------------------------
151  // MapPDF opts
152  // ---------------------------------
154  rbpfMappingOptions.loadFromConfigFile(params, sect);
155 
156  // ---------------------------------
157  // Constructor
158  // ---------------------------------
159  mapBuilder = std::make_shared<CMetricMapBuilderRBPF>(rbpfMappingOptions);
160  mapBuilder->setVerbosityLevel(this->getMinLoggingLevel());
161 
162  {
163  std::stringstream ss;
164  rbpfMappingOptions.dumpToTextStream(ss);
165  MRPT_LOG_INFO(ss.str());
166  }
167 
168  // handle the case of metric map continuation
169  if (!METRIC_MAP_CONTINUATION_GRIDMAP_FILE.empty())
170  {
171  CSimpleMap dummySimpleMap;
172  CPosePDFGaussian startPose;
173 
174  startPose.mean.x(METRIC_MAP_CONTINUATION_START_POSE.x);
175  startPose.mean.y(METRIC_MAP_CONTINUATION_START_POSE.y);
176  startPose.mean.phi(METRIC_MAP_CONTINUATION_START_POSE.phi);
177  startPose.cov.setZero();
178 
180  {
182  METRIC_MAP_CONTINUATION_GRIDMAP_FILE);
183  mrpt::serialization::archiveFrom(f) >> gridmap;
184  }
185 
186  mapBuilder->initialize(dummySimpleMap, &startPose);
187 
188  for (auto& m_particle : mapBuilder->mapPDF.m_particles)
189  {
190  CRBPFParticleData* part_d = m_particle.d.get();
191  CMultiMetricMap& mmap = part_d->mapTillNow;
194  ASSERTMSG_(
195  it_grid,
196  "No gridmap in multimetric map definition, but metric map "
197  "continuation was set (!)");
198  it_grid->copyMapContentFrom(gridmap);
199  }
200  }
201  if (!SIMPLEMAP_CONTINUATION.empty())
202  {
203  mrpt::maps::CSimpleMap init_map;
204  mrpt::io::CFileGZInputStream f(SIMPLEMAP_CONTINUATION);
205  mrpt::serialization::archiveFrom(f) >> init_map;
206  mapBuilder->initialize(init_map);
207  }
208 
209  // ---------------------------------
210  // CMetricMapBuilder::TOptions
211  // ---------------------------------
212  // mapBuilder->setVerbosityLevel( mrpt::system::LVL_DEBUG ); // default
213  // value: as loaded from config file
214  mapBuilder->options.enableMapUpdating = true;
215  mapBuilder->options.debugForceInsertion = false;
216 
217  auto& rng = mrpt::random::getRandomGenerator();
218  if (RANDOM_SEED >= 0)
219  rng.randomize(RANDOM_SEED);
220  else
221  rng.randomize();
222 
223  // Prepare output directory:
224  // --------------------------------
225  deleteFilesInDirectory(OUT_DIR);
226  createDirectory(OUT_DIR);
227 
228  string OUT_DIR_MAPS = format("%s/maps", OUT_DIR);
229  string OUT_DIR_3D = format("%s/3D", OUT_DIR);
230 
231  deleteFilesInDirectory(OUT_DIR_MAPS);
232  createDirectory(OUT_DIR_MAPS);
233 
234  deleteFilesInDirectory(OUT_DIR_3D);
235  createDirectory(OUT_DIR_3D);
236 
237  // Open log files:
238  // ----------------------------------
239  CFileOutputStream f_log(format("%s/log_times.txt", OUT_DIR));
240  CFileOutputStream f_info(format("%s/log_info.txt", OUT_DIR));
241  CFileOutputStream f_jinfo(format("%s/log_jinfo.txt", OUT_DIR));
242  CFileOutputStream f_path(format("%s/log_estimated_path.txt", OUT_DIR));
243  CFileOutputStream f_pathOdo(format("%s/log_odometry_path.txt", OUT_DIR));
244  CFileOutputStream f_partStats(format("%s/log_ParticlesStats.txt", OUT_DIR));
245 
246  f_log.printf(
247  "%% time_step execution_time(ms) map_size(#frames) frame_inserted? "
248  "\n"
249  "%%-------------------------------------------------------------------"
250  "\n");
251 
252  f_info.printf(
253  "%% EMI H EMMI effecMappedArea effecMappedCells \n"
254  "%%-------------------------------------------------------\n");
255 
256  f_pathOdo.printf(
257  "%% time_step x y z yaw pitch roll timestamp \n"
258  "%%--------------------------------------------\n");
259 
260  f_pathOdo.printf(
261  "%% time_step x y z yaw pitch roll \n"
262  "%%----------------------------------\n");
263 
264  f_partStats.printf(
265  "%% time_step #particles ESS \n"
266  "%%------------------------------\n");
267 
268  // ----------------------------------------------------------
269  // Map Building
270  // ----------------------------------------------------------
271  CPose3D odoPose(0, 0, 0);
272 
273  CDisplayWindow3D::Ptr win3D;
274 
275  if (SHOW_PROGRESS_IN_WINDOW)
276  {
277  win3D = CDisplayWindow3D::Create(
278  "RBPF-SLAM @ MRPT C++ Library", PROGRESS_WINDOW_WIDTH,
279  PROGRESS_WINDOW_HEIGHT);
280  win3D->setCameraZoom(40);
281  win3D->setCameraAzimuthDeg(-50);
282  win3D->setCameraElevationDeg(70);
283  }
284 
285  tictacGlobal.Tic();
286  for (;;)
287  {
288  if (quits_with_esc_key && os::kbhit())
289  {
290  char c = os::getch();
291  if (c == 27) break;
292  }
293 
294  CActionCollection::Ptr action;
295  CSensoryFrame::Ptr observations;
296  CObservation::Ptr observation;
297 
298  // Load action/observation pair from the rawlog:
299  // --------------------------------------------------
300  if (!impl_get_next_observations(action, observations, observation))
301  break; // EOF
302 
303  // Update odometry:
304  {
306  action->getBestMovementEstimation();
307  if (act)
308  odoPose = odoPose + CPose3D(act->poseChange->getMeanVal());
309  else
310  {
312  action->getActionByClass<CActionRobotMovement3D>();
313  if (act3D) odoPose = odoPose + act3D->poseChange.mean;
314  }
315  }
316 
317  mrpt::system::TTimeStamp observations_timestamp;
318  if (observations && !observations->empty())
319  observations_timestamp = (*observations->begin())->timestamp;
320 
321  // Execute:
322  // ----------------------------------------
323  tictac.Tic();
324  mapBuilder->processActionObservation(*action, *observations);
325  t_exec = tictac.Tac();
326  MRPT_LOG_INFO_FMT("Map building executed in %.03fms", 1000.0f * t_exec);
327 
328  // Info log:
329  // -----------
330  f_log.printf(
331  "%u %f %i %i\n", static_cast<unsigned int>(step), 1000.0f * t_exec,
332  mapBuilder->getCurrentlyBuiltMapSize(),
333  mapBuilder->m_statsLastIteration.observationsInserted ? int(1)
334  : int(0));
335 
336  CPose3DPDF::Ptr curPDFptr = mapBuilder->getCurrentPoseEstimation();
337  CPose3DPDFParticles curPDF;
338 
339  if (IS_CLASS(*curPDFptr, CPose3DPDFParticles))
340  {
342  std::dynamic_pointer_cast<CPose3DPDFParticles>(curPDFptr);
343  curPDF = *pp;
344  }
345 
346  if (LOG_FREQUENCY > 0 && 0 == (step % LOG_FREQUENCY))
347  {
348  const CMultiMetricMap* mostLikMap =
349  mapBuilder->mapPDF.getCurrentMostLikelyMetricMap();
350 
351  if (GENERATE_LOG_INFO)
352  {
353  tictac_JH.Tic();
354 
355  const CMultiMetricMap* avrMap =
356  mapBuilder->mapPDF.getAveragedMetricMapEstimation();
358  avrMap->mapByClass<COccupancyGridMap2D>();
359  ASSERT_(grid);
360  grid->computeEntropy(entropy);
361 
362  grid->saveAsBitmapFile(
363  format("%s/EMMI_gridmap_%03u.png", OUT_DIR, step));
364 
365  f_info.printf(
366  "%f %f %f %f %lu\n", entropy.I, entropy.H, entropy.mean_I,
367  entropy.effectiveMappedArea, entropy.effectiveMappedCells);
369  "Log information saved. EMI = %.04f EMMI=%.04f (in "
370  "%.03fms)\n",
371  entropy.I, entropy.mean_I, 1000.0f * tictac_JH.Tac());
372  }
373 
374  // Pose log:
375  // -------------
376  if (SAVE_POSE_LOG)
377  {
378  curPDF.saveToTextFile(
379  format("%s/mapbuild_posepdf_%03u.txt", OUT_DIR, step));
380  }
381 
382  // Map images:
383  // -------------
384  if (SAVE_MAP_IMAGES)
385  {
386  MRPT_LOG_DEBUG("Saving map images to files...");
387 
388  // Most likely maps:
389  // ----------------------------------------
391  format("%s/mapbuilt_%05u_", OUT_DIR_MAPS.c_str(), step));
392 
393  if (mostLikMap->countMapsByClass<COccupancyGridMap2D>() > 0)
394  {
395  mrpt::img::CImage img;
396  mapBuilder->drawCurrentEstimationToImage(&img);
397  img.saveToFile(
398  format("%s/mapping_%05u.png", OUT_DIR, step));
399  }
400  }
401 
402  // Save a 3D scene view of the mapping process:
403  COpenGLScene::Ptr scene;
404  if (SAVE_3D_SCENE || SHOW_PROGRESS_IN_WINDOW)
405  {
406  scene = std::make_shared<COpenGLScene>();
407 
408  // The ground:
409  mrpt::opengl::CGridPlaneXY::Ptr groundPlane =
411  -200, 200, -200, 200, 0, 5);
412  groundPlane->setColor(0.4, 0.4, 0.4);
413  scene->insert(groundPlane);
414 
415  // The camera pointing to the current robot pose:
416  if (CAMERA_3DSCENE_FOLLOWS_ROBOT)
417  {
420  CPose3D robotPose;
421  curPDF.getMean(robotPose);
422 
423  objCam->setPointingAt(robotPose);
424  objCam->setAzimuthDegrees(-30);
425  objCam->setElevationDegrees(30);
426  scene->insert(objCam);
427  }
428  // Draw the map(s):
431  mostLikMap->getAs3DObject(objs);
432  scene->insert(objs);
433 
434  // Draw the robot particles:
435  size_t M = mapBuilder->mapPDF.particlesCount();
438  objLines->setColor(0, 1, 1);
439  for (size_t i = 0; i < M; i++)
440  {
441  std::deque<TPose3D> path;
442  mapBuilder->mapPDF.getPath(i, path);
443 
444  float x0 = 0, y0 = 0, z0 = 0;
445  for (auto& k : path)
446  {
447  objLines->appendLine(
448  x0, y0, z0 + 0.001, k.x, k.y, k.z + 0.001);
449  x0 = k.x;
450  y0 = k.y;
451  z0 = k.z;
452  }
453  }
454  scene->insert(objLines);
455 
456  // An ellipsoid:
457  CPose3D lastMeanPose;
458  float minDistBtwPoses = -1;
459  std::deque<TPose3D> dummyPath;
460  mapBuilder->mapPDF.getPath(0, dummyPath);
461  for (int k = (int)dummyPath.size() - 1; k >= 0; k--)
462  {
463  CPose3DPDFParticles poseParts;
464  mapBuilder->mapPDF.getEstimatedPosePDFAtTime(k, poseParts);
465 
466  const auto [COV, meanPose] =
467  poseParts.getCovarianceAndMean();
468 
469  if (meanPose.distanceTo(lastMeanPose) > minDistBtwPoses)
470  {
471  CMatrixDouble33 COV3 = COV.blockCopy<3, 3>(0, 0);
472 
473  minDistBtwPoses = 6 * sqrt(COV3(0, 0) + COV3(1, 1));
474 
475  opengl::CEllipsoid::Ptr objEllip =
476  std::make_shared<opengl::CEllipsoid>();
477  objEllip->setLocation(
478  meanPose.x(), meanPose.y(), meanPose.z() + 0.001);
479  objEllip->setCovMatrix(COV3, COV3(2, 2) == 0 ? 2 : 3);
480 
481  objEllip->setColor(0, 0, 1);
482  objEllip->enableDrawSolid3D(false);
483  scene->insert(objEllip);
484 
485  lastMeanPose = meanPose;
486  }
487  }
488  } // end if show or save 3D scene->
489 
490  if (SAVE_3D_SCENE)
491  { // Save as file:
493  "%s/buildingmap_%05u.3Dscene", OUT_DIR_3D.c_str(), step));
495  }
496 
497  if (SHOW_PROGRESS_IN_WINDOW)
498  {
499  COpenGLScene::Ptr& scenePtr = win3D->get3DSceneAndLock();
500  scenePtr = scene;
501  win3D->unlockAccess3DScene();
502 
503  win3D->forceRepaint();
504  int add_delay =
505  SHOW_PROGRESS_IN_WINDOW_DELAY_MS - t_exec * 1000;
506  if (add_delay > 0)
507  std::this_thread::sleep_for(
508  std::chrono::milliseconds(add_delay));
509  }
510 
511  // Save the weighted entropy of each map:
512  // ----------------------------------------
513  if (GENERATE_LOG_JOINT_H)
514  {
515  tictac_JH.Tic();
516 
517  double H_joint = mapBuilder->getCurrentJointEntropy();
518  double H_path = mapBuilder->mapPDF.getCurrentEntropyOfPaths();
519  f_jinfo.printf("%e %e\n", H_joint, H_path);
521  "Saving joing H info. joint-H=%f\t(in %.03fms)", H_joint,
522  1000.0f * tictac_JH.Tac());
523  }
524 
525  } // end of LOG_FREQ
526 
527  // Save the memory usage:
528  // ------------------------------------------------------------------
529  {
530  unsigned long memUsage = mrpt::system::getMemoryUsage();
531  FILE* f = os::fopen(
532  format("%s/log_MemoryUsage.txt", OUT_DIR).c_str(), "at");
533  if (f)
534  {
535  os::fprintf(f, "%u\t%lu\n", step, memUsage);
536  os::fclose(f);
537  }
539  "Saving memory usage: %.04f MiB", memUsage / (1024.0 * 1024.0));
540  }
541 
542  // Save the parts stats:
543  f_partStats.printf(
544  "%u %u %f\n", (unsigned int)step, (unsigned int)curPDF.size(),
545  curPDF.ESS());
546 
547  // Save the robot estimated pose for each step:
548  CPose3D meanPose;
549  mapBuilder->getCurrentPoseEstimation()->getMean(meanPose);
550 
551  f_path.printf(
552  "%u %f %f %f %f %f %f %f\n", (unsigned int)step, meanPose.x(),
553  meanPose.y(), meanPose.z(), meanPose.yaw(), meanPose.pitch(),
554  meanPose.roll(), mrpt::Clock::toDouble(observations_timestamp));
555 
556  // Also keep the robot path as a vector, for the convenience of the app
557  // user:
558  out_estimated_path[observations_timestamp] = meanPose.asTPose();
559 
560  f_pathOdo.printf(
561  "%i\t%f\t%f\t%f\t%f\t%f\t%f\n", step, odoPose.x(), odoPose.y(),
562  odoPose.z(), odoPose.yaw(), odoPose.pitch(), odoPose.roll());
563 
564  step++;
565  MRPT_LOG_INFO_FMT("------------- STEP %u ----------------", step);
566 
567  }; // end while
568 
570  "----------- **END** (total time: %.03f sec) ---------",
571  tictacGlobal.Tac());
572 
573  // Save map:
574  mapBuilder->getCurrentlyBuiltMap(finalMap);
575 
576  CFileOutputStream filOut(format("%s/_finalmap_.simplemap", OUT_DIR));
577  mrpt::serialization::archiveFrom(filOut) << finalMap;
578 
579  // Save gridmap extend (if exists):
580  const CMultiMetricMap* mostLikMap =
581  mapBuilder->mapPDF.getCurrentMostLikelyMetricMap();
583  format("%s/finalMap", OUT_DIR));
584 
585  // Save the most likely path of the particle set
586  FILE* f_pathPart;
587 
588  os::sprintf(strFil, 1000, "%s/most_likely_path.txt", OUT_DIR);
589  f_pathPart = os::fopen(strFil, "wt");
590 
591  ASSERT_(f_pathPart != nullptr);
592 
593  std::deque<TPose3D> outPath;
594  std::deque<TPose3D>::iterator itPath;
595 
596  mapBuilder->getCurrentMostLikelyPath(outPath);
597 
598  for (itPath = outPath.begin(); itPath != outPath.end(); itPath++)
599  os::fprintf(
600  f_pathPart, "%.3f %.3f %.3f\n", itPath->x, itPath->y, itPath->yaw);
601 
602  os::fclose(f_pathPart);
603 
604  // Close 3D window, if any:
605  if (win3D) win3D->waitForKey();
606 
607  MRPT_END
608 }
609 
610 // ---------------------------------------
611 // RBPF_SLAM_App_Rawlog
612 // ---------------------------------------
614 {
615  setLoggerName("RBPF_SLAM_App_Rawlog");
616 }
617 
619 {
620  MRPT_START
621  // Rawlog file: from args. line or from config file:
622  if (argc == 3)
623  m_rawlogFileName = std::string(argv[2]);
624  else
625  m_rawlogFileName = params.read_string(
626  sect, "rawlog_file", std::string("log.rawlog"), true);
627 
628  m_rawlog_offset = params.read_int(sect, "rawlog_offset", 0, true);
629 
630  ASSERT_FILE_EXISTS_(m_rawlogFileName);
631 
632  // Set relative path for externally-stored images in rawlogs:
633  std::string rawlog_images_path =
634  mrpt::system::extractFileDirectory(m_rawlogFileName);
635  rawlog_images_path += "/Images";
636  mrpt::img::CImage::setImagesPathBase(rawlog_images_path);
637 
638  MRPT_END
639 }
static double toDouble(const time_point t) noexcept
Converts a timestamp to a UNIX time_t-like number, with fractional part.
Definition: Clock.cpp:58
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:775
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:86
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.
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: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
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
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.
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:154
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.
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
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
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
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:410
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
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:15
bool kbhit() noexcept
An OS-independent version of kbhit, which returns true if a key has been pushed.
Definition: os.cpp:394
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:257
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:330
constexpr auto sect
std::string MRPT_getVersion()
Returns a string describing the MRPT version.
Definition: os.cpp:187
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
#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:77
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:31
#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:33
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...
Definition: os.cpp:191
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:147
virtual void impl_initialize(int argc, const char **argv)=0
#define MRPT_LOG_INFO(_STRING)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020