MRPT  2.0.0
MonteCarloLocalization_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 
23 #include <mrpt/maps/CSimpleMap.h>
25 #include <mrpt/math/data_utils.h>
27 #include <mrpt/math/ops_vectors.h> // << for vector<>
28 #include <mrpt/math/utils.h>
33 #include <mrpt/obs/CRawlog.h>
34 #include <mrpt/opengl/CDisk.h>
40 #include <mrpt/poses/CPose2D.h>
42 #include <mrpt/random.h>
46 #include <mrpt/system/CTicTac.h>
47 #include <mrpt/system/filesystem.h>
48 #include <mrpt/system/os.h>
50 #include <Eigen/Dense>
51 
52 using namespace mrpt::apps;
53 
54 // ---------------------------------------
55 // MonteCarloLocalization_Base
56 // ---------------------------------------
58 {
59  // Set logger display name:
60  this->setLoggerName("MonteCarloLocalization_Base");
61 }
62 
64 {
66 
68  " pf-localization - Part of the MRPT\n"
69  " MRPT C++ Library: %s - Sources timestamp: %s\n\n",
72 
73  // Process arguments:
74  if (argc < 2)
75  {
76  THROW_EXCEPTION_FMT("Usage: %s", impl_get_usage().c_str());
77  }
78 
79  // Config file:
80  const std::string configFile = std::string(argv[1]);
81 
82  ASSERT_FILE_EXISTS_(configFile);
84 
86 
87  MRPT_END
88 }
89 
90 // All these "using"s, to easily reuse old code from the pf-localization app:
91 using namespace mrpt;
92 using namespace mrpt::slam;
93 using namespace mrpt::maps;
94 using namespace mrpt::opengl;
95 using namespace mrpt::gui;
96 using namespace mrpt::math;
97 using namespace mrpt::system;
98 using namespace mrpt::random;
99 using namespace mrpt::poses;
100 using namespace mrpt::bayes;
101 using namespace mrpt::obs;
102 using namespace mrpt::io;
103 using namespace mrpt::img;
104 using namespace mrpt::config;
105 using namespace mrpt::serialization;
106 using namespace std;
107 
108 // Auxiliary trait classes for template implementations below:
109 template <class PDF>
111 {
112 };
113 
114 template <>
116 {
118  static constexpr bool PF_IS_3D = false;
119 
120  static void resetOnFreeSpace(
122  size_t PARTICLE_COUNT, const mrpt::math::TPose3D& init_min,
123  const mrpt::math::TPose3D& init_max)
124  {
126  metricMap.mapByClass<COccupancyGridMap2D>().get(), 0.7f,
127  PARTICLE_COUNT, init_min.x, init_max.x, init_min.y, init_max.y,
128  init_min.yaw, init_max.yaw);
129  }
130 
131  static void resetUniform(
132  CMonteCarloLocalization2D& pdf, size_t PARTICLE_COUNT,
133  const mrpt::math::TPose3D& init_min,
134  const mrpt::math::TPose3D& init_max)
135  {
136  pdf.resetUniform(
137  init_min.x, init_max.x, init_min.y, init_max.y, init_min.yaw,
138  init_max.yaw, PARTICLE_COUNT);
139  }
140 };
141 template <>
143 {
145  static constexpr bool PF_IS_3D = true;
146 
147  static void resetOnFreeSpace(
149  size_t PARTICLE_COUNT, const mrpt::math::TPose3D& init_min,
150  const mrpt::math::TPose3D& init_max)
151  {
152  THROW_EXCEPTION("init_PDF_mode=0 not supported for 3D particles");
153  }
154 
155  static void resetUniform(
156  CMonteCarloLocalization3D& pdf, size_t PARTICLE_COUNT,
157  const mrpt::math::TPose3D& init_min,
158  const mrpt::math::TPose3D& init_max)
159  {
160  pdf.resetUniform(init_min, init_max, PARTICLE_COUNT);
161  }
162 };
163 
164 template <class MONTECARLO_TYPE>
166 {
167  auto& cfg = this->params; // rename for reusing old code
168 
169  // Number of initial particles (if size>1, run the experiments N times)
170  std::vector<int> particles_count;
171 
172  // Load parameters:
173 
174  // Mandatory entries:
175  cfg.read_vector(
176  sect, "particles_count", std::vector<int>(1, 0), particles_count,
177  /*Fail if not found*/ true);
178  string OUT_DIR_PREFIX =
179  cfg.read_string(sect, "logOutput_dir", "", /*Fail if not found*/ true);
180 
181  // Non-mandatory entries:
182  string MAP_FILE = cfg.read_string(sect, "map_file", "");
183  size_t rawlog_offset = cfg.read_int(sect, "rawlog_offset", 0);
184  string GT_FILE = cfg.read_string(sect, "ground_truth_path_file", "");
185  const auto NUM_REPS = cfg.read_uint64_t(sect, "experimentRepetitions", 1);
186  int SCENE3D_FREQ = cfg.read_int(sect, "3DSceneFrequency", 10);
187  bool SCENE3D_FOLLOW = cfg.read_bool(sect, "3DSceneFollowRobot", true);
188  unsigned int testConvergenceAt =
189  cfg.read_int(sect, "experimentTestConvergenceAtStep", -1);
190 
191  bool SAVE_STATS_ONLY = cfg.read_bool(sect, "SAVE_STATS_ONLY", false);
192  bool DO_RELIABILITY_ESTIMATE = false;
193  bool DO_SCAN_LIKELIHOOD_DEBUG = false;
194  MRPT_LOAD_CONFIG_VAR(DO_RELIABILITY_ESTIMATE, bool, cfg, sect);
195  MRPT_LOAD_CONFIG_VAR(DO_SCAN_LIKELIHOOD_DEBUG, bool, cfg, sect);
196 
197  bool SHOW_PROGRESS_3D_REAL_TIME =
198  cfg.read_bool(sect, "SHOW_PROGRESS_3D_REAL_TIME", false);
199  int SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS =
200  cfg.read_int(sect, "SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS", 1);
201  double STATS_CONF_INTERVAL =
202  cfg.read_double(sect, "STATS_CONF_INTERVAL", 0.2);
203 
204  CPose2D initial_odo;
205  initial_odo.x(cfg.read_double(sect, "initial_odo_x", 0));
206  initial_odo.y(cfg.read_double(sect, "initial_odo_y", 0));
207  initial_odo.phi(cfg.read_double(sect, "initial_odo_phi", 0));
208 
209 #if !MRPT_HAS_WXWIDGETS
210  SHOW_PROGRESS_3D_REAL_TIME = false;
211 #endif
212 
213  // Default odometry uncertainty parameters in "actOdom2D_params" depending
214  // on how fast the robot moves, etc...
215  // Only used for observations-only rawlogs:
217  actOdom2D_params.modelSelection = CActionRobotMovement2D::mmGaussian;
218  actOdom2D_params.gaussianModel.minStdXY =
219  cfg.read_double("DummyOdometryParams", "minStdXY", 0.04);
220  actOdom2D_params.gaussianModel.minStdPHI =
221  DEG2RAD(cfg.read_double("DummyOdometryParams", "minStdPHI", 2.0));
222 
224  actOdom3D_params.mm6DOFModel.additional_std_XYZ =
225  cfg.read_double("DummyOdometryParams", "additional_std_XYZ", 0.01);
226  actOdom3D_params.mm6DOFModel.additional_std_angle = DEG2RAD(
227  cfg.read_double("DummyOdometryParams", "additional_std_angle", 0.1));
228 
229  // PF-algorithm Options:
230  // ---------------------------
232  pfOptions.loadFromConfigFile(cfg, "PF_options");
233 
234  // PDF Options:
235  // ------------------
236  TMonteCarloLocalizationParams pdfPredictionOptions;
237  pdfPredictionOptions.KLD_params.loadFromConfigFile(cfg, "KLD_options");
238 
239  // Metric map options:
240  // -----------------------------
242  mapList.loadFromConfigFile(cfg, "MetricMap");
243 
245  "" << endl
246  << "MAP_FILE : " << MAP_FILE << endl
247  << "GT_FILE : " << GT_FILE << endl
248  << "OUT_DIR_PREFIX : " << OUT_DIR_PREFIX << endl
249  << "particles_count :" << particles_count << endl);
250 
251  // --------------------------------------------------------------------
252  // EXPERIMENT PREPARATION
253  // --------------------------------------------------------------------
254  CTicTac tictac;
255  CSimpleMap simpleMap;
257 
258  // Load the set of metric maps to consider in the experiments:
259  CMultiMetricMap metricMap;
260  metricMap.setListOfMaps(mapList);
261 
262  {
263  std::stringstream ss;
264  pfOptions.dumpToTextStream(ss);
265  mapList.dumpToTextStream(ss);
266 
267  MRPT_LOG_INFO_STREAM(ss.str());
268  }
269 
271 
272  // Load the map (if any):
273  // -------------------------
274  if (MAP_FILE.size())
275  {
276  ASSERT_(fileExists(MAP_FILE));
277 
278  // Detect file extension:
279  // -----------------------------
280  string mapExt = lowerCase(extractFileExtension(
281  MAP_FILE, true)); // Ignore possible .gz extensions
282 
283  if (!mapExt.compare("simplemap"))
284  {
285  // It's a ".simplemap":
286  // -------------------------
287  MRPT_LOG_INFO_STREAM("Loading '.simplemap' file: " << MAP_FILE);
288  {
289  CFileGZInputStream f(MAP_FILE);
290  archiveFrom(f) >> simpleMap;
291  }
293  "Map loaded ok, with " << simpleMap.size() << " frames.");
294 
295  ASSERT_(simpleMap.size() > 0);
296 
297  // Build metric map:
298  // ------------------------------
299  MRPT_LOG_INFO("Building metric map(s) from '.simplemap'...");
300  metricMap.loadFromProbabilisticPosesAndObservations(simpleMap);
301  MRPT_LOG_INFO("Done.");
302  }
303  else if (!mapExt.compare("gridmap"))
304  {
305  // It's a ".gridmap":
306  // -------------------------
307  MRPT_LOG_INFO("Loading gridmap from '.gridmap'...");
308  auto grid = metricMap.mapByClass<COccupancyGridMap2D>();
309  ASSERT_(grid);
310  {
311  CFileGZInputStream f(MAP_FILE);
312  archiveFrom(f) >> (*grid);
313  }
314  MRPT_LOG_INFO("Done.");
315  }
316  else
317  {
319  "Map file has unknown extension: '%s'", mapExt.c_str());
320  }
321  }
322 
323  // Load the Ground Truth:
324  CMatrixDouble GT(0, 0);
325  if (fileExists(GT_FILE))
326  {
327  MRPT_LOG_INFO("Loading ground truth file...");
328  GT.loadFromTextFile(GT_FILE);
329  MRPT_LOG_INFO("Done.");
330 
331  prepareGT(GT);
332  }
333  else
334  MRPT_LOG_INFO("Ground truth file: not available.");
335 
336  // PDF initialization uniform distribution limits:
337  const auto init_min = mrpt::math::TPose3D(
338  cfg.read_double(sect, "init_PDF_min_x", 0),
339  cfg.read_double(sect, "init_PDF_min_y", 0),
340  cfg.read_double(sect, "init_PDF_min_z", 0),
341  mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_min_yaw_deg", -180.0)),
342  mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_min_pitch_deg", 0)),
343  mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_min_roll_deg", 0)));
344 
345  const auto init_max = mrpt::math::TPose3D(
346  cfg.read_double(sect, "init_PDF_max_x", 0),
347  cfg.read_double(sect, "init_PDF_max_y", 0),
348  cfg.read_double(sect, "init_PDF_max_z", 0),
349  mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_max_yaw_deg", +180.0)),
350  mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_max_pitch_deg", 0)),
351  mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_max_roll_deg", 0)));
352 
354  "Initial PDF limits:\n Min=" << init_min.asString()
355  << "\n Max=" << init_max.asString());
356 
357  // Gridmap / area of initial uncertainty:
359  if (auto grid = metricMap.mapByClass<COccupancyGridMap2D>(); grid)
360  {
361  grid->computeEntropy(gridInfo);
363  "The gridmap has %.04fm2 observed area, %u observed cells.",
364  gridInfo.effectiveMappedArea,
365  (unsigned)gridInfo.effectiveMappedCells);
366  }
367  else
368  {
369  gridInfo.effectiveMappedArea =
370  (init_max.x - init_min.x) * (init_max.y - init_min.y);
371  }
372 
373  for (int PARTICLE_COUNT : particles_count)
374  {
376  "Initial PDF: %f particles/m2",
377  PARTICLE_COUNT / gridInfo.effectiveMappedArea);
378 
379  // Global stats for all the experiment loops:
380  int nConvergenceTests = 0, nConvergenceOK = 0;
381  CVectorDouble convergenceErrors;
382  std::mutex convergenceErrors_mtx;
383 
384  // --------------------------------------------------------------------
385  // EXPERIMENT REPETITIONS LOOP
386  // --------------------------------------------------------------------
387 
388  // Generate list of repetition indices:
389  std::vector<unsigned int> rep_indices(NUM_REPS);
390  std::iota(rep_indices.begin(), rep_indices.end(), 0);
391 
392  // Run each repetition in parallel:
393  // GCC 7 still has not implemented C++17 for_each(std::execution::par
394  // So: home-made parallel loops:
395 
396  // std::for_each(std::execution::par, rep_indices.begin(),
397  // rep_indices.end(),
398  auto run_localization_code = [&](const size_t repetition) {
399  CVectorDouble indivConvergenceErrors, executionTimes, odoError;
401  "====== RUNNING FOR " << PARTICLE_COUNT
402  << " INITIAL PARTICLES - Repetition "
403  << 1 + repetition << " / " << NUM_REPS);
404 
405  // Create 3D window if requested:
406  CDisplayWindow3D::Ptr win3D;
407  if (SHOW_PROGRESS_3D_REAL_TIME)
408  {
409  win3D = std::make_shared<CDisplayWindow3D>(
410  "pf-localization - The MRPT project", 1000, 600);
411  win3D->setCameraAzimuthDeg(-45);
412  }
413 
414  // Create the 3D scene and get the map only once, later we'll modify
415  // only the particles, etc..
416  COpenGLScene scene;
417  if (SCENE3D_FREQ > 0 || SHOW_PROGRESS_3D_REAL_TIME)
418  {
419  mrpt::math::TPoint3D bbox_max(50, 50, 0), bbox_min(-50, -50, 0);
420  if (auto pts = metricMap.getAsSimplePointsMap(); pts)
421  {
422  pts->boundingBox(bbox_min, bbox_max);
423  }
424 
426  bbox_min.x, bbox_max.x, bbox_min.y, bbox_max.y, 0, 5));
427 
428  if (win3D)
429  win3D->setCameraZoom(
430  2 *
431  std::max(
432  bbox_max.x - bbox_min.x, bbox_max.y - bbox_min.y));
433 
434  CSetOfObjects::Ptr gl_obj = std::make_shared<CSetOfObjects>();
435  metricMap.getAs3DObject(gl_obj);
436  scene.insert(gl_obj);
437  }
438 
439  // The experiment directory is:
440  string sOUT_DIR_PARTS, sOUT_DIR_3D;
441  const auto sOUT_DIR = format(
442  "%s_%03u_%07i", OUT_DIR_PREFIX.c_str(), repetition,
443  PARTICLE_COUNT);
444  MRPT_LOG_INFO_FMT("Creating directory: %s", sOUT_DIR.c_str());
445  deleteFilesInDirectory(sOUT_DIR);
446  createDirectory(sOUT_DIR);
447  ASSERT_DIRECTORY_EXISTS_(sOUT_DIR);
448 
449  if (!SAVE_STATS_ONLY)
450  {
451  sOUT_DIR_PARTS = format("%s/particles", sOUT_DIR.c_str());
452  sOUT_DIR_3D = format("%s/3D", sOUT_DIR.c_str());
453 
455  "Creating directory: %s", sOUT_DIR_PARTS.c_str());
456  deleteFilesInDirectory(sOUT_DIR_PARTS);
457  createDirectory(sOUT_DIR_PARTS);
458  ASSERT_DIRECTORY_EXISTS_(sOUT_DIR_PARTS);
459 
461  "Creating directory: %s", sOUT_DIR_3D.c_str());
462  deleteFilesInDirectory(sOUT_DIR_3D);
463  createDirectory(sOUT_DIR_3D);
464  ASSERT_DIRECTORY_EXISTS_(sOUT_DIR_3D);
465 
466  using namespace std::string_literals;
467  metricMap.saveMetricMapRepresentationToFile(sOUT_DIR + "/map"s);
468  }
469 
470  int M = PARTICLE_COUNT;
471 
472  MONTECARLO_TYPE pdf;
473 
474  // PDF Options:
475  pdf.options = pdfPredictionOptions;
476 
477  pdf.options.metricMap = &metricMap;
478 
479  // Create the PF object:
480  CParticleFilter PF;
481  PF.m_options = pfOptions;
482 
483  size_t step = 0;
484  size_t rawlogEntry = 0;
485 
486  // Initialize the PDF:
487  // -----------------------------
488  tictac.Tic();
489  if (!cfg.read_bool(
490  sect, "init_PDF_mode", false,
491  /*Fail if not found*/ true))
492  {
493  // Reset uniform on free space:
495  pdf, metricMap, PARTICLE_COUNT, init_min, init_max);
496  }
497  else
498  {
499  // Reset uniform:
501  pdf, PARTICLE_COUNT, init_min, init_max);
502  }
503 
505  "PDF of %u particles initialized in %.03fms", M,
506  1000 * tictac.Tac());
507 
508  pdf.saveToTextFile(
509  format("%s/particles_0_initial.txt", sOUT_DIR_PARTS.c_str()));
510 
511  // -----------------------------
512  // Particle filter
513  // -----------------------------
514  CPose2D odometryEstimation = initial_odo;
515 
516  using PDF_MEAN_TYPE = typename MONTECARLO_TYPE::type_value;
517 
518  PDF_MEAN_TYPE pdfEstimation;
519 
521  bool end = false;
522 
523  CFileOutputStream f_cov_est, f_pf_stats, f_odo_est;
524 
525  if (!SAVE_STATS_ONLY)
526  {
527  f_cov_est.open(sOUT_DIR.c_str() + string("/cov_est.txt"));
528  f_pf_stats.open(sOUT_DIR.c_str() + string("/PF_stats.txt"));
529  f_odo_est.open(sOUT_DIR.c_str() + string("/odo_est.txt"));
530  }
531 
532  Clock::time_point cur_obs_timestamp;
533  CPose2D last_used_abs_odo(0, 0, 0),
534  pending_most_recent_odo(0, 0, 0);
535 
536  while (!end)
537  {
538  // Finish if ESC is pushed:
539  if (allow_quit_on_esc_key && os::kbhit())
540  if (os::getch() == 27)
541  {
542  end = true;
543  break;
544  }
545 
546  // Load pose change from the rawlog:
547  // ----------------------------------------
548  CActionCollection::Ptr action;
549  CSensoryFrame::Ptr observations;
550  CObservation::Ptr obs;
551 
552  if (!impl_get_next_observations(action, observations, obs))
553  {
554  // EOF
555  end = true;
556  break;
557  }
558 
559  // Determine if we are reading a Act-SF or an Obs-only
560  // rawlog:
561  if (obs)
562  {
563  // It's an observation-only rawlog: build an auxiliary
564  // pair of action-SF, since
565  // montecarlo-localization only accepts those pairs as
566  // input:
567 
568  // If it's an odometry reading, don't feed it to the PF.
569  // Instead,
570  // store its value for use as an "action" together with
571  // the next actual observation:
572  if (IS_CLASS(*obs, CObservationOdometry))
573  {
574  auto obs_odo =
575  std::dynamic_pointer_cast<CObservationOdometry>(
576  obs);
577  pending_most_recent_odo = obs_odo->odometry;
578  static bool is_1st_odo = true;
579  if (is_1st_odo)
580  {
581  is_1st_odo = false;
582  last_used_abs_odo = pending_most_recent_odo;
583  }
584  continue;
585  }
586  else
587  {
588  // SF: Just one observation:
589  // ------------------------------------------------------
590  observations = std::make_shared<CSensoryFrame>();
591  observations->insert(obs);
592 
593  // ActionCollection: Just one action with a dummy
594  // odometry
595  // ------------------------------------------------------
596  action = std::make_shared<CActionCollection>();
597 
599  {
600  CActionRobotMovement3D actOdom3D;
601 
602  const CPose3D odo_incr = CPose3D(
603  pending_most_recent_odo - last_used_abs_odo);
604  last_used_abs_odo = pending_most_recent_odo;
605 
606  actOdom3D.computeFromOdometry(
607  odo_incr, actOdom3D_params);
608  action->insert(actOdom3D);
609  }
610  else
611  {
612  CActionRobotMovement2D actOdom2D;
613 
614  const CPose2D odo_incr =
615  pending_most_recent_odo - last_used_abs_odo;
616  last_used_abs_odo = pending_most_recent_odo;
617 
618  actOdom2D.computeFromOdometry(
619  odo_incr, actOdom2D_params);
620  action->insert(actOdom2D);
621  }
622  }
623  }
624  else
625  {
626  // Already in Act-SF format, nothing else to do!
627  }
628 
629  CPose2D expectedPose; // Ground truth
630 
631  if (observations->size() > 0)
632  cur_obs_timestamp =
633  observations->getObservationByIndex(0)->timestamp;
634  else if (obs)
635  cur_obs_timestamp = obs->timestamp;
636 
637  if (step >= rawlog_offset)
638  {
639  // Do not execute the PF at "step=0", to let the initial
640  // PDF to be reflected in the logs.
641  if (step > rawlog_offset)
642  {
643  // Show 3D?
644  if (SHOW_PROGRESS_3D_REAL_TIME)
645  {
646  const auto [cov, meanPose] =
647  pdf.getCovarianceAndMean();
648 
649  if (rawlogEntry >= 2)
650  getGroundTruth(
651  expectedPose, rawlogEntry - 2, GT,
652  cur_obs_timestamp);
653 
654  // The particles' cov:
655  {
656  CRenderizable::Ptr ellip =
657  scene.getByName("parts_cov");
658  if (!ellip)
659  {
661  {
662  auto el = CEllipsoid3D::Create();
663  ellip =
665  el);
666  el->setLineWidth(2);
667  el->setQuantiles(3);
668  el->enableDrawSolid3D(false);
669  }
670  else
671  {
672  auto el = CEllipsoid2D::Create();
673  ellip =
675  el);
676  el->setLineWidth(2);
677  el->setQuantiles(3);
678  el->enableDrawSolid3D(false);
679  }
680  ellip->setName("parts_cov");
681  ellip->setColor(1, 0, 0, 0.6);
682  scene.insert(ellip);
683  }
684  else
685  {
687  {
689  ellip)
690  ->setCovMatrix(
691  cov.template blockCopy<3, 3>());
692  }
693  else
694  {
696  ellip)
697  ->setCovMatrix(
698  cov.template blockCopy<2, 2>());
699  }
700  }
701  double ellipse_z =
702  mrpt::poses::CPose3D(meanPose).z() + 0.01;
703 
704  ellip->setLocation(
705  meanPose.x(), meanPose.y(), ellipse_z);
706  }
707 
708  COpenGLScene::Ptr ptrSceneWin =
709  win3D->get3DSceneAndLock();
710 
711  win3D->setCameraPointingToPoint(
712  meanPose.x(), meanPose.y(), 0);
713 
714  win3D->addTextMessage(
715  10, 10,
716  mrpt::format(
717  "timestamp: %s",
719  cur_obs_timestamp)
720  .c_str()),
721  6001);
722 
723  win3D->addTextMessage(
724  10, 33,
725  mrpt::format(
726  "#particles= %7u",
727  static_cast<unsigned int>(pdf.size())),
728  6002);
729 
730  win3D->addTextMessage(
731  10, 55,
732  mrpt::format(
733  "mean pose (x y phi_deg)= %s",
734  meanPose.asString().c_str()),
735  6003);
736 
737  *ptrSceneWin = scene;
738  win3D->unlockAccess3DScene();
739 
740  // Update:
741  win3D->forceRepaint();
742 
743  std::this_thread::sleep_for(
744  std::chrono::milliseconds(
745  SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS));
746  } // end show 3D real-time
747 
748  // ----------------------------------------
749  // RUN ONE STEP OF THE PARTICLE FILTER:
750  // ----------------------------------------
751  tictac.Tic();
752  if (!SAVE_STATS_ONLY)
753  {
755  "Step "
756  << step << ". Executing ParticleFilter on "
757  << pdf.particlesCount() << " particles...");
758  }
759 
760  PF.executeOn(
761  pdf,
762  action.get(), // Action
763  observations.get(), // Obs.
764  &PF_stats // Output statistics
765  );
766 
767  double run_time = tictac.Tac();
768  executionTimes.push_back(run_time);
769  if (!SAVE_STATS_ONLY)
770  {
772  "Done in %.03fms, ESS=%f", 1e3 * run_time,
773  pdf.ESS());
774  }
775  }
776 
777  // Avrg. error:
778  // ----------------------------------------
779  CActionRobotMovement2D::Ptr best_mov_estim =
780  action->getBestMovementEstimation();
781  if (best_mov_estim)
782  {
783  odometryEstimation =
784  odometryEstimation +
785  best_mov_estim->poseChange->getMeanVal();
786  }
787 
788  pdf.getMean(pdfEstimation);
789 
790  // save estimated mean in history:
791  if (cur_obs_timestamp != INVALID_TIMESTAMP &&
792  fill_out_estimated_path)
793  {
794  out_estimated_path.insert(
795  cur_obs_timestamp,
796  mrpt::math::TPose3D(pdfEstimation.asTPose()));
797  }
798 
799  getGroundTruth(
800  expectedPose, rawlogEntry, GT, cur_obs_timestamp);
801 
802  if (expectedPose.x() != 0 || expectedPose.y() != 0 ||
803  expectedPose.phi() != 0)
804  { // Averaged error to GT
805  double sumW = 0;
806  double locErr = 0;
807  for (size_t k = 0; k < pdf.size(); k++)
808  sumW += exp(pdf.getW(k));
809  for (size_t k = 0; k < pdf.size(); k++)
810  {
811  const auto pk = pdf.getParticlePose(k);
812  locErr += mrpt::hypot_fast(
813  expectedPose.x() - pk.x,
814  expectedPose.y() - pk.y) *
815  exp(pdf.getW(k)) / sumW;
816  }
817  convergenceErrors_mtx.lock();
818  convergenceErrors.push_back(locErr);
819  convergenceErrors_mtx.unlock();
820 
821  indivConvergenceErrors.push_back(locErr);
822  odoError.push_back(
823  expectedPose.distanceTo(odometryEstimation));
824  }
825 
826  const auto [C, M] = pdf.getCovarianceAndMean();
827  const auto current_pdf_gaussian =
828  typename pf2gauss_t<MONTECARLO_TYPE>::type(M, C);
829 
830  // Text output:
831  // ----------------------------------------
832  if (!SAVE_STATS_ONLY)
833  {
835  "Odometry estimation: " << odometryEstimation);
837  "PDF estimation: "
838  << pdfEstimation << ", ESS (B.R.)= "
839  << PF_stats.ESS_beforeResample << " tr(cov): "
840  << std::sqrt(current_pdf_gaussian.cov.trace()));
841 
842  if (GT.rows() > 0)
844  "Ground truth: " << expectedPose);
845  }
846 
847  // Evaluate the "reliability" of the pose estimation
848  // (for now, only for 2D laser scans + grid maps)
849  double obs_reliability_estim = .0;
850  if (DO_RELIABILITY_ESTIMATE)
851  {
852  // We need: a gridmap & a 2D LIDAR:
854  if (observations)
855  obs_scan = observations->getObservationByClass<
856  CObservation2DRangeScan>(0); // Get the 0'th
857  // scan, if
858  // several are
859  // present.
860  COccupancyGridMap2D::Ptr gridmap =
861  metricMap.mapByClass<COccupancyGridMap2D>();
862  if (obs_scan && gridmap) // We have both, go on:
863  {
864  // Simulate scan + uncertainty:
866  ssu_params;
868  ssu_out;
869  ssu_params.method =
870  COccupancyGridMap2D::sumUnscented;
871  // ssu_params.UT_alpha = 0.99;
872  // obs_scan->stdError = 0.07;
873  // obs_scan->maxRange = 10.0;
874 
875  ssu_params.robotPose =
876  CPosePDFGaussian(current_pdf_gaussian);
877  ssu_params.aperture = obs_scan->aperture;
878  ssu_params.rangeNoiseStd = obs_scan->stdError;
879  ssu_params.nRays = obs_scan->getScanSize();
880  ssu_params.rightToLeft = obs_scan->rightToLeft;
881  ssu_params.sensorPose = obs_scan->sensorPose;
882  ssu_params.maxRange = obs_scan->maxRange;
883 
884  gridmap->laserScanSimulatorWithUncertainty(
885  ssu_params, ssu_out);
886 
887  // Evaluate reliability:
889  evalParams;
890  // evalParams.prob_outliers = 0.40;
891  // evalParams.max_prediction_std_dev = 1.0;
892  obs_reliability_estim =
894  *obs_scan, evalParams);
895 
896  if (DO_SCAN_LIKELIHOOD_DEBUG)
897  {
899 
900  std::vector<float> ranges_mean, ranges_obs;
901  for (size_t i = 0; i < obs_scan->getScanSize();
902  i++)
903  {
904  ranges_mean.push_back(
905  ssu_out.scanWithUncert.rangeScan
906  .getScanRange(i));
907  ranges_obs.push_back(
908  obs_scan->getScanRange(i));
909  }
910 
911  win.plot(ranges_mean, "3k-", "mean");
912  win.plot(ranges_obs, "r-", "obs");
913 
914  Eigen::VectorXd ci1 =
915  ssu_out.scanWithUncert.rangesMean
916  .asEigen() +
917  3 * ssu_out.scanWithUncert.rangesCovar
918  .asEigen()
919  .diagonal()
920  .array()
921  .sqrt()
922  .matrix();
923  Eigen::VectorXd ci2 =
924  ssu_out.scanWithUncert.rangesMean
925  .asEigen() -
926  3 * ssu_out.scanWithUncert.rangesCovar
927  .asEigen()
928  .diagonal()
929  .array()
930  .sqrt()
931  .matrix();
932  win.plot(ci1, "k-", "CI+");
933  win.plot(ci2, "k-", "CI-");
934 
935  win.setWindowTitle(mrpt::format(
936  "obs_reliability_estim: %f",
937  obs_reliability_estim));
938  win.axis_fit();
939  }
940  }
942  "Reliability measure [0-1]: "
943  << obs_reliability_estim);
944  }
945 
946  if (!SAVE_STATS_ONLY)
947  {
948  f_cov_est.printf("%e\n", sqrt(cov.det()));
949  f_pf_stats.printf(
950  "%u %e %e %f %f\n", (unsigned int)pdf.size(),
951  PF_stats.ESS_beforeResample,
953  obs_reliability_estim,
954  sqrt(current_pdf_gaussian.cov.det()));
955  f_odo_est.printf(
956  "%f %f %f\n", odometryEstimation.x(),
957  odometryEstimation.y(), odometryEstimation.phi());
958  }
959 
960  const auto [cov, meanPose] = pdf.getCovarianceAndMean();
961 
962  if ((!SAVE_STATS_ONLY && SCENE3D_FREQ > 0) ||
963  SHOW_PROGRESS_3D_REAL_TIME)
964  {
965  // Generate 3D scene:
966  // ------------------------------
967 
968  // The Ground Truth (GT):
969  if (GT.rows() > 0)
970  {
971  CRenderizable::Ptr GTpt = scene.getByName("GT");
972  if (!GTpt)
973  {
974  GTpt = std::make_shared<CDisk>();
975  GTpt = std::make_shared<CDisk>();
976  GTpt->setName("GT");
977  GTpt->setColor(0, 0, 0, 0.9);
978 
980  ->setDiskRadius(0.04f);
981  scene.insert(GTpt);
982  }
983 
984  GTpt->setPose(expectedPose);
985  }
986 
987  // The particles:
988  {
989  CRenderizable::Ptr parts =
990  scene.getByName("particles");
991  if (parts) scene.removeObject(parts);
992 
993  auto p = pdf.template getAs3DObject<
995  p->setName("particles");
996  scene.insert(p);
997  }
998 
999  // The laser scan:
1000  {
1001  CRenderizable::Ptr scanPts =
1002  scene.getByName("scan");
1003  if (!scanPts)
1004  {
1005  scanPts = std::make_shared<CPointCloud>();
1006  scanPts->setName("scan");
1007  scanPts->setColor(1, 0, 0, 0.9);
1009  ->enableColorFromZ(false);
1011  ->setPointSize(4);
1012  scene.insert(scanPts);
1013  }
1014 
1015  CSimplePointsMap map;
1016 
1017  CPose3D robotPose3D(meanPose);
1018 
1019  map.clear();
1020  observations->insertObservationsInto(&map);
1021 
1023  ->loadFromPointsMap(&map);
1024  mrpt::ptr_cast<CPointCloud>::from(scanPts)->setPose(
1025  robotPose3D);
1026  }
1027 
1028  // The camera:
1029  scene.enableFollowCamera(SCENE3D_FOLLOW);
1030 
1031  // Views:
1032  COpenGLViewport::Ptr view1 = scene.getViewport("main");
1033  {
1034  CCamera& cam = view1->getCamera();
1035  cam.setAzimuthDegrees(-90);
1036  cam.setElevationDegrees(90);
1037  cam.setPointingAt(meanPose);
1038  cam.setZoomDistance(5);
1039  cam.setOrthogonal();
1040  }
1041  }
1042 
1043  if (!SAVE_STATS_ONLY && SCENE3D_FREQ != -1 &&
1044  ((step + 1) % SCENE3D_FREQ) == 0)
1045  {
1046  // Save 3D scene:
1048  "%s/progress_%05u.3Dscene", sOUT_DIR_3D.c_str(),
1049  (unsigned)step));
1050  archiveFrom(f) << scene;
1051 
1052  // Generate text files for matlab:
1053  // ------------------------------------
1054  pdf.saveToTextFile(format(
1055  "%s/particles_%05u.txt", sOUT_DIR_PARTS.c_str(),
1056  (unsigned)step));
1057  }
1058 
1059  } // end if rawlog_offset
1060 
1061  step++;
1062 
1063  // Test for end condition if we are testing convergence:
1064  if (step == testConvergenceAt)
1065  {
1066  nConvergenceTests++;
1067 
1068  // Convergence??
1069  if (sqrt(cov.det()) < 2)
1070  {
1071  if (pdfEstimation.distanceTo(expectedPose) < 2)
1072  nConvergenceOK++;
1073  }
1074  end = true;
1075  }
1076  }; // while rawlogEntries
1077 
1078  indivConvergenceErrors.saveToTextFile(sOUT_DIR + "/GT_error.txt");
1079  odoError.saveToTextFile(sOUT_DIR + "/ODO_error.txt");
1080  executionTimes.saveToTextFile(sOUT_DIR + "/exec_times.txt");
1081 
1082  if (win3D && NUM_REPS == 1) mrpt::system::pause();
1083  }; // for repetitions
1084 
1085  CTicTac tictacGlobal;
1086  tictacGlobal.Tic();
1087 
1088  const auto max_num_threads = std::thread::hardware_concurrency();
1089  size_t runs_per_thread = NUM_REPS;
1090  if (max_num_threads > 1)
1091  runs_per_thread = static_cast<size_t>(
1092  std::ceil((NUM_REPS) / static_cast<double>(max_num_threads)));
1093 
1095  "Running " << NUM_REPS << " repetitions, on max " << max_num_threads
1096  << " parallel threads: " << runs_per_thread
1097  << " runs/thread.");
1098 
1099  std::vector<std::thread> running_tasks;
1100  for (size_t r = 0; r < NUM_REPS; r += runs_per_thread)
1101  {
1102  auto runner = [&](size_t i_start, size_t i_end) {
1103  if (i_end > NUM_REPS) i_end = NUM_REPS; // sanity check
1104  for (size_t i = i_start; i < i_end; i++)
1105  run_localization_code(i);
1106  };
1107 
1108  running_tasks.emplace_back(runner, r, r + runs_per_thread);
1109  std::this_thread::sleep_for(std::chrono::milliseconds(100));
1110  }
1111 
1112  // Wait for all threads to end:
1113  for (auto& t : running_tasks)
1114  if (t.joinable()) t.join();
1115 
1116  double repetitionTime = tictacGlobal.Tac();
1117 
1118  // Avr. error:
1119  double covergenceErrorMean = 0, convergenceErrorsMin = 0,
1120  convergenceErrorsMax = 0;
1121  if (!convergenceErrors.empty())
1123  convergenceErrors, covergenceErrorMean, convergenceErrorsMin,
1124  convergenceErrorsMax, STATS_CONF_INTERVAL);
1125 
1126  // Save overall results:
1127  {
1129  format("%s_SUMMARY.txt", OUT_DIR_PREFIX.c_str()),
1130  true /* append */);
1131 
1132  f.printf(
1133  "%% Ratio_covergence_success #particles "
1134  "average_time_per_execution convergence_mean_error "
1135  "convergence_error_conf_int_inf "
1136  "convergence_error_conf_int_sup "
1137  "\n");
1138  if (!nConvergenceTests) nConvergenceTests = 1;
1139  f.printf(
1140  "%f %u %f %f %f %f\n",
1141  ((double)nConvergenceOK) / nConvergenceTests, PARTICLE_COUNT,
1142  repetitionTime / NUM_REPS, covergenceErrorMean,
1143  convergenceErrorsMin, convergenceErrorsMax);
1144  }
1145 
1146  MRPT_LOG_INFO_FMT("Total execution time: %.06f sec", repetitionTime);
1147 
1148  } // end of loop for different # of particles
1149 }
1150 
1152  mrpt::poses::CPose2D& expectedPose, size_t rawlogEntry,
1153  const mrpt::math::CMatrixDouble& GT, const Clock::time_point& cur_time)
1154 {
1155  // Either:
1156  // - time x y phi
1157  // or
1158  // - time x y z yaw pitch roll
1159 
1160  if (GT.cols() == 4 || GT.cols() == 7)
1161  {
1162  bool GT_index_is_time;
1163 
1164  // First column can be: timestamps, or rawlogentries:
1165  // Auto-figure it out:
1166  if (GT.rows() > 2)
1167  {
1168  GT_index_is_time =
1169  floor(GT(0, 0)) != GT(0, 0) && floor(GT(1, 0)) != GT(1, 0);
1170  }
1171  else
1172  {
1173  GT_index_is_time = false;
1174  }
1175 
1176  if (GT_index_is_time)
1177  {
1178  // Look for the timestamp:
1179  using namespace std::chrono_literals;
1180 
1181  bool interp_ok = false;
1182  GT_path.interpolate(cur_time, expectedPose, interp_ok);
1183  if (!interp_ok)
1184  {
1185  /*
1186  cerr << format(
1187  "GT time not found: %f\n",
1188  mrpt::system::timestampTotime_t(cur_time));
1189  */
1190  }
1191  }
1192  else
1193  {
1194  // Look for the rawlogEntry:
1195  size_t k, N = GT.rows();
1196  for (k = 0; k < N; k++)
1197  {
1198  if (GT(k, 0) == rawlogEntry) break;
1199  }
1200 
1201  if (k < N)
1202  {
1203  expectedPose.x(GT(k, 1));
1204  expectedPose.y(GT(k, 2));
1205  expectedPose.phi(GT(k, 3));
1206  }
1207  }
1208  }
1209  else if (GT.cols() == 3)
1210  {
1211  if ((int)rawlogEntry < GT.rows())
1212  {
1213  expectedPose.x(GT(rawlogEntry, 0));
1214  expectedPose.y(GT(rawlogEntry, 1));
1215  expectedPose.phi(GT(rawlogEntry, 2));
1216  }
1217  }
1218  else if (GT.cols() > 0)
1219  THROW_EXCEPTION("Unexpected number of columns in ground truth file");
1220 }
1221 
1223 {
1224  // Either:
1225  // - time x y phi
1226  // or
1227  // - time x y z yaw pitch roll
1228  if (GT.cols() == 4 || GT.cols() == 7)
1229  {
1230  const bool GT_is_3D = (GT.cols() == 7);
1231  bool GT_index_is_time = false;
1232 
1233  // First column can be: timestamps, or rawlogentries:
1234  // Auto-figure it out:
1235  if (GT.rows() > 2)
1236  {
1237  GT_index_is_time =
1238  floor(GT(0, 0)) != GT(0, 0) && floor(GT(1, 0)) != GT(1, 0);
1239  }
1240 
1241  if (GT_index_is_time)
1242  {
1243  // Look for the timestamp:
1244  using namespace std::chrono_literals;
1245  GT_path.setMaxTimeInterpolation(200ms);
1246 
1247  for (int i = 0; i < GT.rows(); i++)
1248  {
1249  GT_path.insert(
1250  mrpt::Clock::fromDouble(GT(i, 0)),
1251  TPose2D(GT(i, 1), GT(i, 2), GT(i, GT_is_3D ? 4 : 3)));
1252  }
1253  }
1254  }
1255 }
1256 
1258 {
1259  MRPT_START
1260 
1261  // Detect 2D vs 3D particle filter?
1262  const bool is_3D = params.read_bool(sect, "use_3D_poses", false);
1263 
1264  if (is_3D)
1265  {
1266  MRPT_LOG_INFO("Running for: CMonteCarloLocalization3D");
1267  do_pf_localization<CMonteCarloLocalization3D>();
1268  }
1269  else
1270  {
1271  MRPT_LOG_INFO("Running for: CMonteCarloLocalization2D");
1272  do_pf_localization<CMonteCarloLocalization2D>();
1273  }
1274 
1275  MRPT_END
1276 }
1277 
1278 // ---------------------------------------
1279 // MonteCarloLocalization_Rawlog
1280 // ---------------------------------------
1282 {
1283  setLoggerName("MonteCarloLocalization_Rawlog");
1284 }
1285 
1287 {
1288  MRPT_START
1289  // Rawlog file: from args. line or from config file:
1290  if (argc == 3)
1291  m_rawlogFileName = std::string(argv[2]);
1292  else
1293  m_rawlogFileName = params.read_string(
1294  sect, "rawlog_file", std::string("log.rawlog"), true);
1295 
1296  m_rawlog_offset = params.read_int(sect, "rawlog_offset", 0);
1297 
1298  ASSERT_FILE_EXISTS_(m_rawlogFileName);
1299 
1300  MRPT_END
1301 }
mrpt::poses::CPosePDFGaussian robotPose
The robot pose Gaussian, in map coordinates.
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
A namespace of pseudo-random numbers generators of diferent distributions.
void resetUniform(const double x_min, const double x_max, const double y_min, const double y_max, const double phi_min=-M_PI, const double phi_max=M_PI, const int particlesCount=-1)
Reset the PDF to an uniformly distributed one, inside of the defined 2D area [x_min,x_max]x[y_min,y_max] (in meters) and for orientations [phi_min, phi_max] (in radians).
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:30
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:161
#define MRPT_START
Definition: exceptions.h:241
static time_point fromDouble(const double t) noexcept
Create a timestamp from its double representation.
Definition: Clock.cpp:99
The struct for passing extra simulation parameters to the prediction stage when running a particle fi...
void prepareGT(const mrpt::math::CMatrixDouble &GT)
mrpt::config::CConfigFileMemory params
Populated in initialize().
void run()
Runs with the current parameter set.
void resetUniformFreeSpace(mrpt::maps::COccupancyGridMap2D *theMap, const double freeCellsThreshold=0.7, const int particlesCount=-1, const double x_min=-1e10f, const double x_max=1e10f, const double y_min=-1e10f, const double y_max=1e10f, const double phi_min=-M_PI, const double phi_max=M_PI)
Reset the PDF to an uniformly distributed one, but only in the free-space of a given 2D occupancy-gri...
void setOrthogonal(bool v=true)
Enable/Disable orthogonal mode (vs.
Definition: CCamera.h:72
std::chrono::time_point< Clock > time_point
Definition: Clock.h:25
void loadFromProbabilisticPosesAndObservations(const mrpt::maps::CSimpleMap &Map)
Load the map contents from a CSimpleMap object, erasing all previous content of the map...
Definition: CMetricMap.cpp:36
mrpt::obs::CObservation2DRangeScanWithUncertainty scanWithUncert
The scan + its uncertainty.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
Create a GUI window and display plots with MATLAB-like interfaces and commands.
int getch() noexcept
An OS-independent version of getch, which waits until a key is pushed.
Definition: os.cpp:370
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
mrpt::poses::CPose3D sensorPose
(Default: at origin) The 6D pose of the sensor on the robot at the moment of starting the scan...
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
Output params for laserScanSimulatorWithUncertainty()
float rangeNoiseStd
(Default: 0) The standard deviation of measurement noise.
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
double x
X,Y,Z, coords.
Definition: TPose3D.h:32
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
A high-performance stopwatch, with typical resolution of nanoseconds.
static void resetUniform(CMonteCarloLocalization3D &pdf, size_t PARTICLE_COUNT, const mrpt::math::TPose3D &init_min, const mrpt::math::TPose3D &init_max)
static void resetOnFreeSpace(CMonteCarloLocalization3D &pdf, mrpt::maps::CMultiMetricMap &metricMap, size_t PARTICLE_COUNT, const mrpt::math::TPose3D &init_min, const mrpt::math::TPose3D &init_max)
mrpt::vision::TStereoCalibParams params
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
void computeFromOdometry(const mrpt::poses::CPose2D &odometryIncrement, const TMotionModelOptions &options)
Computes the PDF of the pose increment from an odometry reading and according to the given motion mod...
static void resetOnFreeSpace(CMonteCarloLocalization2D &pdf, mrpt::maps::CMultiMetricMap &metricMap, size_t PARTICLE_COUNT, const mrpt::math::TPose3D &init_min, const mrpt::math::TPose3D &init_max)
STL namespace.
std::string MRPT_getCompilationDate()
Returns the MRPT source code timestamp, according to the Reproducible-Builds specifications: https://...
Definition: os.cpp:154
CObservation2DRangeScan rangeScan
The observation with the mean ranges in the scan field.
The parameter to be passed to "computeFromOdometry".
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 ...
float additional_std_XYZ
An additional noise added to the 6DOF model (std.
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: TPose3D.h:34
bool rightToLeft
(Default: true) The scanning direction: true=counterclockwise; false=clockwise
Statistics for being returned from the "execute" method.
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
mrpt::maps::CMetricMap * metricMap
[update stage] Must be set to a metric map used to estimate the likelihood of observations ...
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Saves the vector/matrix to a file compatible with MATLAB/Octave text format.
virtual std::string impl_get_usage() const =0
void push_back(const T &val)
void setZoomDistance(float z)
Definition: CCamera.h:63
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:214
void computeFromOdometry(const mrpt::poses::CPose3D &odometryIncrement, const TMotionModelOptions &options)
Computes the PDF of the pose increment from an odometry reading and according to the given motion mod...
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
void setAzimuthDegrees(float ang)
Definition: CCamera.h:67
The parameter to be passed to "computeFromOdometry".
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
Represents a probabilistic 3D (6D) movement.
Represents a probabilistic 2D movement of the robot mobile base.
mrpt::math::CVectorDouble rangesMean
The same ranges than in rangeScan.getScanRange(), for convenience as a math vector container...
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 confidenceIntervals(const CONTAINER &data, T &out_mean, T &out_lower_conf_interval, T &out_upper_conf_interval, const double confidenceInterval=0.1, const size_t histogramNumBins=1000)
Return the mean and the 10%-90% confidence points (or with any other confidence value) of a set of sa...
std::string lowerCase(const std::string &str)
Returns an lower-case version of a string.
static void resetUniform(CMonteCarloLocalization2D &pdf, size_t PARTICLE_COUNT, const mrpt::math::TPose3D &init_min, const mrpt::math::TPose3D &init_max)
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &sectionName) override
Loads the configuration for the set of internal maps from a textual definition in an INI-like file...
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 enableFollowCamera(bool enabled)
If disabled (default), the SceneViewer application will ignore the camera of the "main" viewport and ...
Definition: COpenGLScene.h:134
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: TKLDParams.cpp:48
void initialize(int argc, const char **argv)
Initializes the application from CLI parameters.
float aperture
(Default: M_PI) The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180...
void setLoggerName(const std::string &name)
Set the name of the COutputLogger instance.
This namespace contains representation of robot actions and observations.
Scalar det() const
Determinant of matrix.
void dumpToTextStream(std::ostream &out) const override
This method dumps the options of the multi-metric map AND those of every internal map...
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
constexpr auto sect
double evaluateScanLikelihood(const CObservation2DRangeScan &otherScan, const TEvalParams &params) const
Returns a measure of the likelihood of a given scan, compared to this scan variances.
std::string extractFileExtension(const std::string &filePath, bool ignore_gz=false)
Extract the extension of a filename.
Definition: filesystem.cpp:98
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
T hypot_fast(const T x, const T y)
Faster version of std::hypot(), to use when overflow is not an issue and we prefer fast code...
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:53
This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilt...
CMatrixDouble cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:149
This CStream derived class allow using a file as a write-only, binary stream.
mrpt::gui::CDisplayWindow3D::Ptr win
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
TDrawSampleMotionModel modelSelection
The model to be used.
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
void pause(const std::string &msg=std::string("Press any key to continue...")) noexcept
Shows the message "Press any key to continue" (or other custom message) to the current standard outpu...
Definition: os.cpp:430
bool open(const std::string &fileName, bool append=false)
Open the given file for write.
Declares a class that represents a Probability Density Function (PDF) over a 3D pose (x...
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
A class for storing an occupancy grid map.
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
const_iterator end() const
Definition: ts_hash_map.h:246
#define ASSERT_DIRECTORY_EXISTS_(DIR)
Definition: filesystem.h:27
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void executeOn(CParticleFilterCapable &obj, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, TParticleFilterStats *stats=nullptr)
Executes a complete prediction + update step of the selected particle filtering algorithm.
const char * argv[]
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
void setElevationDegrees(float ang)
Definition: CCamera.h:68
COpenGLViewport::Ptr getViewport(const std::string &viewportName=std::string("main")) const
Returns the viewport with the given name, or nullptr if it does not exist; note that the default view...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const override
If the map is a simple points map or it&#39;s a multi-metric map that contains EXACTLY one simple points ...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
Input params for laserScanSimulatorWithUncertainty()
CRenderizable::Ptr getByName(const std::string &str, const std::string &viewportName=std::string("main"))
Returns the first object with a given name, or nullptr (an empty smart pointer) if not found...
void setContent(const std::vector< std::string > &stringList)
Changes the contents of the virtual "config file".
The configuration of a particle filter.
const float & getScanRange(const size_t i) const
The range values of the scan, in meters.
mrpt::math::CMatrixDouble rangesCovar
The covariance matrix for all the ranges in rangeScan.getScanRange()
#define MRPT_END
Definition: exceptions.h:245
TLaserSimulUncertaintyMethod method
(Default: sumMonteCarlo) Select the method to do the uncertainty propagation
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
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:392
std::string file_get_contents(const std::string &fileName)
Loads an entire text file and return its contents as a single std::string.
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:56
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
static CAST_TO::Ptr from(const CAST_FROM_PTR &ptr)
Definition: CObject.h:356
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.
void resetUniform(const mrpt::math::TPose3D &corner_min, const mrpt::math::TPose3D &corner_max, const int particlesCount=-1)
Reset the PDF to an uniformly distributed one, inside of the defined "cube".
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:14
const int argc
float maxRange
(Default: 80) The maximum range allowed by the device, in meters (e.g.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
std::string MRPT_getVersion()
Returns a string describing the MRPT version.
Definition: os.cpp:187
An observation of the current (cumulative) odometry for a wheeled robot.
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.
std::string dateTimeLocalToString(const mrpt::system::TTimeStamp t)
Convert a timestamp into this textual form (in local time): YEAR/MONTH/DAY,HH:MM:SS.MMM.
Definition: datetime.cpp:176
#define ASSERT_FILE_EXISTS_(FIL)
Definition: filesystem.h:22
A camera: if added to a scene, the viewpoint defined by this camera will be used instead of the camer...
Definition: CCamera.h:33
void impl_initialize(int argc, const char **argv) override
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
CParticleFilter::TParticleFilterOptions m_options
The options to be used in the PF, must be set before executing any step of the particle filter...
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
void insert(const CRenderizable::Ptr &newObject, const std::string &viewportName=std::string("main"))
Insert a new object into the scene, in the given viewport (by default, into the "main" viewport)...
Used for returning entropy related information.
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:31
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
void removeObject(const CRenderizable::Ptr &obj, const std::string &viewportName=std::string("main"))
Removes the given object from the scene (it also deletes the object to free its memory).
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
TKLDParams KLD_params
Parameters for dynamic sample size, KLD method.
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers &init)
Sets the list of internal map according to the passed list of map initializers (current maps will be ...
virtual void impl_initialize(int argc, const char **argv)=0
#define MRPT_LOG_INFO(_STRING)
void setPointingAt(float x, float y, float z)
Definition: CCamera.h:41
void loadFromTextFile(std::istream &f)
Loads a vector/matrix from a text file, compatible with MATLAB text format.
void getGroundTruth(mrpt::poses::CPose2D &expectedPose, size_t rawlogEntry, const mrpt::math::CMatrixDouble &GT, const Clock::time_point &cur_time)



Page generated by Doxygen 1.8.14 for MRPT 2.0.0 Git: b38439d21 Tue Mar 31 19:58:06 2020 +0200 at mar mar 31 20:00:11 CEST 2020