MRPT  2.0.5
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>
49 #include <Eigen/Dense>
50 
51 using namespace mrpt::apps;
52 
53 // ---------------------------------------
54 // MonteCarloLocalization_Base
55 // ---------------------------------------
57 {
58  // Set logger display name:
59  this->setLoggerName("MonteCarloLocalization_Base");
60 }
61 
63 {
65 
67  " pf-localization - Part of the MRPT\n"
68  " MRPT C++ Library: %s - Sources timestamp: %s\n\n",
71 
72  // Process arguments:
73  if (argc < 2)
74  {
75  THROW_EXCEPTION_FMT("Usage: %s", impl_get_usage().c_str());
76  }
77 
78  // Config file:
79  const std::string configFile = std::string(argv[1]);
80 
81  ASSERT_FILE_EXISTS_(configFile);
83 
85 
86  MRPT_END
87 }
88 
89 // All these "using"s, to easily reuse old code from the pf-localization app:
90 using namespace mrpt;
91 using namespace mrpt::slam;
92 using namespace mrpt::maps;
93 using namespace mrpt::opengl;
94 using namespace mrpt::gui;
95 using namespace mrpt::math;
96 using namespace mrpt::system;
97 using namespace mrpt::random;
98 using namespace mrpt::poses;
99 using namespace mrpt::bayes;
100 using namespace mrpt::obs;
101 using namespace mrpt::io;
102 using namespace mrpt::img;
103 using namespace mrpt::config;
104 using namespace mrpt::serialization;
105 using namespace std;
106 
107 // Auxiliary trait classes for template implementations below:
108 template <class PDF>
110 {
111 };
112 
113 template <>
115 {
117  static constexpr bool PF_IS_3D = false;
118 
119  static void resetOnFreeSpace(
121  size_t PARTICLE_COUNT, const mrpt::math::TPose3D& init_min,
122  const mrpt::math::TPose3D& init_max)
123  {
125  metricMap.mapByClass<COccupancyGridMap2D>().get(), 0.7f,
126  PARTICLE_COUNT, init_min.x, init_max.x, init_min.y, init_max.y,
127  init_min.yaw, init_max.yaw);
128  }
129 
130  static void resetUniform(
131  CMonteCarloLocalization2D& pdf, size_t PARTICLE_COUNT,
132  const mrpt::math::TPose3D& init_min,
133  const mrpt::math::TPose3D& init_max)
134  {
135  pdf.resetUniform(
136  init_min.x, init_max.x, init_min.y, init_max.y, init_min.yaw,
137  init_max.yaw, PARTICLE_COUNT);
138  }
139 };
140 template <>
142 {
144  static constexpr bool PF_IS_3D = true;
145 
146  static void resetOnFreeSpace(
147  [[maybe_unused]] CMonteCarloLocalization3D& pdf,
148  [[maybe_unused]] mrpt::maps::CMultiMetricMap& metricMap,
149  [[maybe_unused]] size_t PARTICLE_COUNT,
150  [[maybe_unused]] const mrpt::math::TPose3D& init_min,
151  [[maybe_unused]] const mrpt::math::TPose3D& init_max)
152  {
153  THROW_EXCEPTION("init_PDF_mode=0 not supported for 3D particles");
154  }
155 
156  static void resetUniform(
157  CMonteCarloLocalization3D& pdf, size_t PARTICLE_COUNT,
158  const mrpt::math::TPose3D& init_min,
159  const mrpt::math::TPose3D& init_max)
160  {
161  pdf.resetUniform(init_min, init_max, PARTICLE_COUNT);
162  }
163 };
164 
165 template <class MONTECARLO_TYPE>
167 {
168  auto& cfg = this->params; // rename for reusing old code
169 
170  // Number of initial particles (if size>1, run the experiments N times)
171  std::vector<int> particles_count;
172 
173  // Load parameters:
174 
175  // Mandatory entries:
176  cfg.read_vector(
177  sect, "particles_count", std::vector<int>(1, 0), particles_count,
178  /*Fail if not found*/ true);
179  string OUT_DIR_PREFIX =
180  cfg.read_string(sect, "logOutput_dir", "", /*Fail if not found*/ true);
181 
182  // Non-mandatory entries:
183  string MAP_FILE = cfg.read_string(sect, "map_file", "");
184  size_t rawlog_offset = cfg.read_int(sect, "rawlog_offset", 0);
185  string GT_FILE = cfg.read_string(sect, "ground_truth_path_file", "");
186  const auto NUM_REPS = cfg.read_uint64_t(sect, "experimentRepetitions", 1);
187  int SCENE3D_FREQ = cfg.read_int(sect, "3DSceneFrequency", 10);
188  bool SCENE3D_FOLLOW = cfg.read_bool(sect, "3DSceneFollowRobot", true);
189  unsigned int testConvergenceAt =
190  cfg.read_int(sect, "experimentTestConvergenceAtStep", -1);
191 
192  bool SAVE_STATS_ONLY = cfg.read_bool(sect, "SAVE_STATS_ONLY", false);
193  bool DO_RELIABILITY_ESTIMATE = false;
194  bool DO_SCAN_LIKELIHOOD_DEBUG = false;
195  MRPT_LOAD_CONFIG_VAR(DO_RELIABILITY_ESTIMATE, bool, cfg, sect);
196  MRPT_LOAD_CONFIG_VAR(DO_SCAN_LIKELIHOOD_DEBUG, bool, cfg, sect);
197 
198  bool SHOW_PROGRESS_3D_REAL_TIME =
199  cfg.read_bool(sect, "SHOW_PROGRESS_3D_REAL_TIME", false);
200  int SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS =
201  cfg.read_int(sect, "SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS", 1);
202  double STATS_CONF_INTERVAL =
203  cfg.read_double(sect, "STATS_CONF_INTERVAL", 0.2);
204 
205  CPose2D initial_odo;
206  initial_odo.x(cfg.read_double(sect, "initial_odo_x", 0));
207  initial_odo.y(cfg.read_double(sect, "initial_odo_y", 0));
208  initial_odo.phi(cfg.read_double(sect, "initial_odo_phi", 0));
209 
210 #if !MRPT_HAS_WXWIDGETS
211  SHOW_PROGRESS_3D_REAL_TIME = false;
212 #endif
213 
214  // Default odometry uncertainty parameters in "actOdom2D_params" depending
215  // on how fast the robot moves, etc...
216  // Only used for observations-only rawlogs:
218  actOdom2D_params.modelSelection = CActionRobotMovement2D::mmGaussian;
219  actOdom2D_params.gaussianModel.minStdXY =
220  cfg.read_double("DummyOdometryParams", "minStdXY", 0.04);
221  actOdom2D_params.gaussianModel.minStdPHI =
222  DEG2RAD(cfg.read_double("DummyOdometryParams", "minStdPHI", 2.0));
223 
225  actOdom3D_params.mm6DOFModel.additional_std_XYZ =
226  cfg.read_double("DummyOdometryParams", "additional_std_XYZ", 0.01);
227  actOdom3D_params.mm6DOFModel.additional_std_angle = DEG2RAD(
228  cfg.read_double("DummyOdometryParams", "additional_std_angle", 0.1));
229 
230  // PF-algorithm Options:
231  // ---------------------------
233  pfOptions.loadFromConfigFile(cfg, "PF_options");
234 
235  // PDF Options:
236  // ------------------
237  TMonteCarloLocalizationParams pdfPredictionOptions;
238  pdfPredictionOptions.KLD_params.loadFromConfigFile(cfg, "KLD_options");
239 
240  // Metric map options:
241  // -----------------------------
243  mapList.loadFromConfigFile(cfg, "MetricMap");
244 
246  "" << endl
247  << "MAP_FILE : " << MAP_FILE << endl
248  << "GT_FILE : " << GT_FILE << endl
249  << "OUT_DIR_PREFIX : " << OUT_DIR_PREFIX << endl
250  << "particles_count :" << particles_count << endl);
251 
252  // --------------------------------------------------------------------
253  // EXPERIMENT PREPARATION
254  // --------------------------------------------------------------------
255  CTicTac tictac;
256  CSimpleMap simpleMap;
258 
259  // Load the set of metric maps to consider in the experiments:
260  CMultiMetricMap metricMap;
261  metricMap.setListOfMaps(mapList);
262 
263  {
264  std::stringstream ss;
265  pfOptions.dumpToTextStream(ss);
266  mapList.dumpToTextStream(ss);
267 
268  MRPT_LOG_INFO_STREAM(ss.str());
269  }
270 
272 
273  // Load the map (if any):
274  // -------------------------
275  if (MAP_FILE.size())
276  {
277  ASSERT_(fileExists(MAP_FILE));
278 
279  // Detect file extension:
280  // -----------------------------
281  string mapExt = lowerCase(extractFileExtension(
282  MAP_FILE, true)); // Ignore possible .gz extensions
283 
284  if (!mapExt.compare("simplemap"))
285  {
286  // It's a ".simplemap":
287  // -------------------------
288  MRPT_LOG_INFO_STREAM("Loading '.simplemap' file: " << MAP_FILE);
289  {
290  CFileGZInputStream f(MAP_FILE);
291  archiveFrom(f) >> simpleMap;
292  }
294  "Map loaded ok, with " << simpleMap.size() << " frames.");
295 
296  ASSERT_(simpleMap.size() > 0);
297 
298  // Build metric map:
299  // ------------------------------
300  MRPT_LOG_INFO("Building metric map(s) from '.simplemap'...");
301  metricMap.loadFromProbabilisticPosesAndObservations(simpleMap);
302  MRPT_LOG_INFO("Done.");
303  }
304  else if (!mapExt.compare("gridmap"))
305  {
306  // It's a ".gridmap":
307  // -------------------------
308  MRPT_LOG_INFO("Loading gridmap from '.gridmap'...");
309  auto grid = metricMap.mapByClass<COccupancyGridMap2D>();
310  ASSERT_(grid);
311  {
312  CFileGZInputStream f(MAP_FILE);
313  archiveFrom(f) >> (*grid);
314  }
315  MRPT_LOG_INFO("Done.");
316  }
317  else
318  {
320  "Map file has unknown extension: '%s'", mapExt.c_str());
321  }
322  }
323 
324  // Load the Ground Truth:
325  CMatrixDouble GT(0, 0);
326  if (fileExists(GT_FILE))
327  {
328  MRPT_LOG_INFO("Loading ground truth file...");
329  GT.loadFromTextFile(GT_FILE);
330  MRPT_LOG_INFO("Done.");
331 
332  prepareGT(GT);
333  }
334  else
335  MRPT_LOG_INFO("Ground truth file: not available.");
336 
337  // PDF initialization uniform distribution limits:
338  const auto init_min = mrpt::math::TPose3D(
339  cfg.read_double(sect, "init_PDF_min_x", 0),
340  cfg.read_double(sect, "init_PDF_min_y", 0),
341  cfg.read_double(sect, "init_PDF_min_z", 0),
342  mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_min_yaw_deg", -180.0)),
343  mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_min_pitch_deg", 0)),
344  mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_min_roll_deg", 0)));
345 
346  const auto init_max = mrpt::math::TPose3D(
347  cfg.read_double(sect, "init_PDF_max_x", 0),
348  cfg.read_double(sect, "init_PDF_max_y", 0),
349  cfg.read_double(sect, "init_PDF_max_z", 0),
350  mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_max_yaw_deg", +180.0)),
351  mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_max_pitch_deg", 0)),
352  mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_max_roll_deg", 0)));
353 
355  "Initial PDF limits:\n Min=" << init_min.asString()
356  << "\n Max=" << init_max.asString());
357 
358  // Gridmap / area of initial uncertainty:
360  if (auto grid = metricMap.mapByClass<COccupancyGridMap2D>(); grid)
361  {
362  grid->computeEntropy(gridInfo);
364  "The gridmap has %.04fm2 observed area, %u observed cells.",
365  gridInfo.effectiveMappedArea,
366  (unsigned)gridInfo.effectiveMappedCells);
367  }
368  else
369  {
370  gridInfo.effectiveMappedArea =
371  (init_max.x - init_min.x) * (init_max.y - init_min.y);
372  }
373 
374  for (int PARTICLE_COUNT : particles_count)
375  {
377  "Initial PDF: %f particles/m2",
378  PARTICLE_COUNT / gridInfo.effectiveMappedArea);
379 
380  // Global stats for all the experiment loops:
381  int nConvergenceTests = 0, nConvergenceOK = 0;
382  CVectorDouble convergenceErrors;
383  std::mutex convergenceErrors_mtx;
384 
385  // --------------------------------------------------------------------
386  // EXPERIMENT REPETITIONS LOOP
387  // --------------------------------------------------------------------
388 
389  // Generate list of repetition indices:
390  std::vector<unsigned int> rep_indices(NUM_REPS);
391  std::iota(rep_indices.begin(), rep_indices.end(), 0);
392 
393  // Run each repetition in parallel:
394  // GCC 7 still has not implemented C++17 for_each(std::execution::par
395  // So: home-made parallel loops:
396 
397  // std::for_each(std::execution::par, rep_indices.begin(),
398  // rep_indices.end(),
399  auto run_localization_code = [&](const size_t repetition) {
400  CVectorDouble indivConvergenceErrors, executionTimes, odoError;
402  "====== RUNNING FOR " << PARTICLE_COUNT
403  << " INITIAL PARTICLES - Repetition "
404  << 1 + repetition << " / " << NUM_REPS);
405 
406  // Create 3D window if requested:
407  CDisplayWindow3D::Ptr win3D;
408  if (SHOW_PROGRESS_3D_REAL_TIME)
409  {
410  win3D = std::make_shared<CDisplayWindow3D>(
411  "pf-localization - The MRPT project", 1000, 600);
412  win3D->setCameraAzimuthDeg(-45);
413  }
414 
415  // Create the 3D scene and get the map only once, later we'll modify
416  // only the particles, etc..
417  COpenGLScene scene;
418  if (SCENE3D_FREQ > 0 || SHOW_PROGRESS_3D_REAL_TIME)
419  {
420  mrpt::math::TPoint3D bbox_max(50, 50, 0), bbox_min(-50, -50, 0);
421  if (auto pts = metricMap.getAsSimplePointsMap(); pts)
422  {
423  pts->boundingBox(bbox_min, bbox_max);
424  }
425 
427  bbox_min.x, bbox_max.x, bbox_min.y, bbox_max.y, 0, 5));
428 
429  if (win3D)
430  win3D->setCameraZoom(
431  2 *
432  std::max(
433  bbox_max.x - bbox_min.x, bbox_max.y - bbox_min.y));
434 
435  CSetOfObjects::Ptr gl_obj = std::make_shared<CSetOfObjects>();
436  metricMap.getAs3DObject(gl_obj);
437  scene.insert(gl_obj);
438  }
439 
440  // The experiment directory is:
441  string sOUT_DIR_PARTS, sOUT_DIR_3D;
442  const auto sOUT_DIR = format(
443  "%s_%03u_%07i", OUT_DIR_PREFIX.c_str(), repetition,
444  PARTICLE_COUNT);
445  MRPT_LOG_INFO_FMT("Creating directory: %s", sOUT_DIR.c_str());
446  deleteFilesInDirectory(sOUT_DIR);
447  createDirectory(sOUT_DIR);
448  ASSERT_DIRECTORY_EXISTS_(sOUT_DIR);
449 
450  if (!SAVE_STATS_ONLY)
451  {
452  sOUT_DIR_PARTS = format("%s/particles", sOUT_DIR.c_str());
453  sOUT_DIR_3D = format("%s/3D", sOUT_DIR.c_str());
454 
456  "Creating directory: %s", sOUT_DIR_PARTS.c_str());
457  deleteFilesInDirectory(sOUT_DIR_PARTS);
458  createDirectory(sOUT_DIR_PARTS);
459  ASSERT_DIRECTORY_EXISTS_(sOUT_DIR_PARTS);
460 
462  "Creating directory: %s", sOUT_DIR_3D.c_str());
463  deleteFilesInDirectory(sOUT_DIR_3D);
464  createDirectory(sOUT_DIR_3D);
465  ASSERT_DIRECTORY_EXISTS_(sOUT_DIR_3D);
466 
467  using namespace std::string_literals;
468  metricMap.saveMetricMapRepresentationToFile(sOUT_DIR + "/map"s);
469  }
470 
471  MONTECARLO_TYPE pdf;
472 
473  // PDF Options:
474  pdf.options = pdfPredictionOptions;
475 
476  pdf.options.metricMap = &metricMap;
477 
478  // Create the PF object:
479  CParticleFilter PF;
480  PF.m_options = pfOptions;
481 
482  size_t step = 0;
483  size_t rawlogEntry = 0;
484 
485  // Initialize the PDF:
486  // -----------------------------
487  tictac.Tic();
488  if (!cfg.read_bool(
489  sect, "init_PDF_mode", false,
490  /*Fail if not found*/ true))
491  {
492  // Reset uniform on free space:
494  pdf, metricMap, PARTICLE_COUNT, init_min, init_max);
495  }
496  else
497  {
498  // Reset uniform:
500  pdf, PARTICLE_COUNT, init_min, init_max);
501  }
502 
504  "PDF of %u particles initialized in %.03fms", PARTICLE_COUNT,
505  1000 * tictac.Tac());
506 
507  pdf.saveToTextFile(
508  format("%s/particles_0_initial.txt", sOUT_DIR_PARTS.c_str()));
509 
510  // -----------------------------
511  // Particle filter
512  // -----------------------------
513  CPose2D odometryEstimation = initial_odo;
514 
515  using PDF_MEAN_TYPE = typename MONTECARLO_TYPE::type_value;
516 
517  PDF_MEAN_TYPE pdfEstimation;
518 
519  bool end = false;
520 
521  CFileOutputStream f_cov_est, f_pf_stats, f_odo_est;
522 
523  if (!SAVE_STATS_ONLY)
524  {
525  f_cov_est.open(sOUT_DIR.c_str() + string("/cov_est.txt"));
526  f_pf_stats.open(sOUT_DIR.c_str() + string("/PF_stats.txt"));
527  f_odo_est.open(sOUT_DIR.c_str() + string("/odo_est.txt"));
528  }
529 
530  Clock::time_point cur_obs_timestamp;
531  CPose2D last_used_abs_odo(0, 0, 0),
532  pending_most_recent_odo(0, 0, 0);
533 
534  for (; !end; step++)
535  {
536  // Finish if ESC is pushed:
537  if (allow_quit_on_esc_key && os::kbhit())
538  if (os::getch() == 27)
539  {
540  end = true;
541  break;
542  }
543 
544  // Load pose change from the rawlog:
545  // ----------------------------------------
546  CActionCollection::Ptr action;
547  CSensoryFrame::Ptr observations;
548  CObservation::Ptr obs;
549 
550  if (!impl_get_next_observations(action, observations, obs))
551  {
552  // EOF
553  end = true;
554  break;
555  }
556 
557  // Determine if we are reading a Act-SF or an Obs-only
558  // rawlog:
559  if (obs)
560  {
561  // It's an observation-only rawlog: build an auxiliary
562  // pair of action-SF, since
563  // montecarlo-localization only accepts those pairs as
564  // input:
565 
566  // If it's an odometry reading, don't feed it to the PF.
567  // Instead,
568  // store its value for use as an "action" together with
569  // the next actual observation:
570  if (IS_CLASS(*obs, CObservationOdometry))
571  {
572  auto obs_odo =
573  std::dynamic_pointer_cast<CObservationOdometry>(
574  obs);
575  pending_most_recent_odo = obs_odo->odometry;
576  static bool is_1st_odo = true;
577  if (is_1st_odo)
578  {
579  is_1st_odo = false;
580  last_used_abs_odo = pending_most_recent_odo;
581  }
582  continue;
583  }
584  else
585  {
586  // SF: Just one observation:
587  // ------------------------------------------------------
588  observations = std::make_shared<CSensoryFrame>();
589  observations->insert(obs);
590 
591  // ActionCollection: Just one action with a dummy
592  // odometry
593  // ------------------------------------------------------
594  action = std::make_shared<CActionCollection>();
595 
597  {
598  CActionRobotMovement3D actOdom3D;
599 
600  const CPose3D odo_incr = CPose3D(
601  pending_most_recent_odo - last_used_abs_odo);
602  last_used_abs_odo = pending_most_recent_odo;
603 
604  actOdom3D.computeFromOdometry(
605  odo_incr, actOdom3D_params);
606  action->insert(actOdom3D);
607  }
608  else
609  {
610  CActionRobotMovement2D actOdom2D;
611 
612  const CPose2D odo_incr =
613  pending_most_recent_odo - last_used_abs_odo;
614  last_used_abs_odo = pending_most_recent_odo;
615 
616  actOdom2D.computeFromOdometry(
617  odo_incr, actOdom2D_params);
618  action->insert(actOdom2D);
619  }
620  }
621  }
622  else
623  {
624  // Already in Act-SF format, nothing else to do!
625  }
626 
627  CPose2D expectedPose; // Ground truth
628 
629  if (observations->size() > 0)
630  cur_obs_timestamp =
631  observations->getObservationByIndex(0)->timestamp;
632  else if (obs)
633  cur_obs_timestamp = obs->timestamp;
634 
635  // Test for end condition if we are testing convergence:
636  if (step == testConvergenceAt)
637  {
638  nConvergenceTests++;
639 
640  // Convergence??
641  if (sqrt(pdf.getCovariance().det()) < 2)
642  {
643  if (pdfEstimation.distanceTo(expectedPose) < 2)
644  nConvergenceOK++;
645  }
646  end = true;
647  }
648 
649  if (step < rawlog_offset) continue;
650 
651  // Do not execute the PF at "step=0", to let the initial
652  // PDF to be reflected in the logs.
653  if (step > rawlog_offset)
654  {
655  // Show 3D?
656  if (SHOW_PROGRESS_3D_REAL_TIME)
657  {
658  const auto [cov, meanPose] = pdf.getCovarianceAndMean();
659 
660  if (rawlogEntry >= 2)
661  getGroundTruth(
662  expectedPose, rawlogEntry - 2, GT,
663  cur_obs_timestamp);
664 
665  // The particles' cov:
666  {
667  CRenderizable::Ptr ellip =
668  scene.getByName("parts_cov");
669  if (!ellip)
670  {
672  {
673  auto el = CEllipsoid3D::Create();
674  ellip =
676  el->setLineWidth(2);
677  el->setQuantiles(3);
678  el->enableDrawSolid3D(false);
679  }
680  else
681  {
682  auto el = CEllipsoid2D::Create();
683  ellip =
685  el->setLineWidth(2);
686  el->setQuantiles(3);
687  el->enableDrawSolid3D(false);
688  }
689  ellip->setName("parts_cov");
690  ellip->setColor(1, 0, 0, 0.6);
691  scene.insert(ellip);
692  }
693  else
694  {
696  {
698  ->setCovMatrix(
699  cov.template blockCopy<3, 3>());
700  }
701  else
702  {
704  ->setCovMatrix(
705  cov.template blockCopy<2, 2>());
706  }
707  }
708  double ellipse_z =
709  mrpt::poses::CPose3D(meanPose).z() + 0.01;
710 
711  ellip->setLocation(
712  meanPose.x(), meanPose.y(), ellipse_z);
713  }
714 
715  COpenGLScene::Ptr ptrSceneWin =
716  win3D->get3DSceneAndLock();
717 
718  win3D->setCameraPointingToPoint(
719  meanPose.x(), meanPose.y(), 0);
720 
721  win3D->addTextMessage(
722  10, 10,
723  mrpt::format(
724  "timestamp: %s",
726  cur_obs_timestamp)
727  .c_str()),
728  6001);
729 
730  win3D->addTextMessage(
731  10, 33,
732  mrpt::format(
733  "#particles= %7u",
734  static_cast<unsigned int>(pdf.size())),
735  6002);
736 
737  win3D->addTextMessage(
738  10, 55,
739  mrpt::format(
740  "mean pose (x y phi_deg)= %s",
741  meanPose.asString().c_str()),
742  6003);
743 
744  *ptrSceneWin = scene;
745  win3D->unlockAccess3DScene();
746 
747  // Update:
748  win3D->forceRepaint();
749 
750  std::this_thread::sleep_for(std::chrono::milliseconds(
751  SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS));
752  } // end show 3D real-time
753 
754  // ----------------------------------------
755  // RUN ONE STEP OF THE PARTICLE FILTER:
756  // ----------------------------------------
757  tictac.Tic();
758  if (!SAVE_STATS_ONLY)
759  {
761  "Step " << step << ". Executing ParticleFilter on "
762  << pdf.particlesCount() << " particles...");
763  }
764 
765  PF.executeOn(
766  pdf,
767  action.get(), // Action
768  observations.get(), // Obs.
769  &PF_stats // Output statistics
770  );
771 
772  double run_time = tictac.Tac();
773  executionTimes.push_back(run_time);
774  if (!SAVE_STATS_ONLY)
775  {
777  "Done in %.03fms, ESS=%f", 1e3 * run_time,
778  pdf.ESS());
779  }
780  }
781 
782  // Avrg. error:
783  // ----------------------------------------
784  CActionRobotMovement2D::Ptr best_mov_estim =
785  action->getBestMovementEstimation();
786  if (best_mov_estim)
787  {
788  odometryEstimation =
789  odometryEstimation +
790  best_mov_estim->poseChange->getMeanVal();
791  }
792 
793  pdf.getMean(pdfEstimation);
794 
795  // save estimated mean in history:
796  if (cur_obs_timestamp != INVALID_TIMESTAMP &&
797  fill_out_estimated_path)
798  {
799  out_estimated_path.insert(
800  cur_obs_timestamp,
801  mrpt::math::TPose3D(pdfEstimation.asTPose()));
802  }
803 
804  getGroundTruth(
805  expectedPose, rawlogEntry, GT, cur_obs_timestamp);
806 
807  if (expectedPose.x() != 0 || expectedPose.y() != 0 ||
808  expectedPose.phi() != 0)
809  { // Averaged error to GT
810  double sumW = 0;
811  double locErr = 0;
812  for (size_t k = 0; k < pdf.size(); k++)
813  sumW += exp(pdf.getW(k));
814  for (size_t k = 0; k < pdf.size(); k++)
815  {
816  const auto pk = pdf.getParticlePose(k);
817  locErr += mrpt::hypot_fast(
818  expectedPose.x() - pk.x,
819  expectedPose.y() - pk.y) *
820  exp(pdf.getW(k)) / sumW;
821  }
822  convergenceErrors_mtx.lock();
823  convergenceErrors.push_back(locErr);
824  convergenceErrors_mtx.unlock();
825 
826  indivConvergenceErrors.push_back(locErr);
827  odoError.push_back(
828  expectedPose.distanceTo(odometryEstimation));
829  }
830 
831  const auto [cov, meanPose] = pdf.getCovarianceAndMean();
832  const auto current_pdf_gaussian =
833  typename pf2gauss_t<MONTECARLO_TYPE>::type(meanPose, cov);
834 
835  // Text output:
836  // ----------------------------------------
837  if (!SAVE_STATS_ONLY)
838  {
840  "Odometry estimation: " << odometryEstimation);
842  "PDF estimation: "
843  << pdfEstimation << ", ESS (B.R.)= "
844  << PF_stats.ESS_beforeResample << " tr(cov): "
845  << std::sqrt(current_pdf_gaussian.cov.trace()));
846 
847  if (GT.rows() > 0)
848  MRPT_LOG_INFO_STREAM("Ground truth: " << expectedPose);
849  }
850 
851  // Evaluate the "reliability" of the pose estimation
852  // (for now, only for 2D laser scans + grid maps)
853  double obs_reliability_estim = .0;
854  if (DO_RELIABILITY_ESTIMATE)
855  {
856  // We need: a gridmap & a 2D LIDAR:
858  if (observations)
859  obs_scan = observations->getObservationByClass<
860  CObservation2DRangeScan>(0); // Get the 0'th
861  // scan, if
862  // several are
863  // present.
864  COccupancyGridMap2D::Ptr gridmap =
865  metricMap.mapByClass<COccupancyGridMap2D>();
866  if (obs_scan && gridmap) // We have both, go on:
867  {
868  // Simulate scan + uncertainty:
870  ssu_params;
872  ssu_out;
873  ssu_params.method = COccupancyGridMap2D::sumUnscented;
874  // ssu_params.UT_alpha = 0.99;
875  // obs_scan->stdError = 0.07;
876  // obs_scan->maxRange = 10.0;
877 
878  ssu_params.robotPose =
879  CPosePDFGaussian(current_pdf_gaussian);
880  ssu_params.aperture = obs_scan->aperture;
881  ssu_params.rangeNoiseStd = obs_scan->stdError;
882  ssu_params.nRays = obs_scan->getScanSize();
883  ssu_params.rightToLeft = obs_scan->rightToLeft;
884  ssu_params.sensorPose = obs_scan->sensorPose;
885  ssu_params.maxRange = obs_scan->maxRange;
886 
887  gridmap->laserScanSimulatorWithUncertainty(
888  ssu_params, ssu_out);
889 
890  // Evaluate reliability:
892  evalParams;
893  // evalParams.prob_outliers = 0.40;
894  // evalParams.max_prediction_std_dev = 1.0;
895  obs_reliability_estim =
897  *obs_scan, evalParams);
898 
899  if (DO_SCAN_LIKELIHOOD_DEBUG)
900  {
902 
903  std::vector<float> ranges_mean, ranges_obs;
904  for (size_t i = 0; i < obs_scan->getScanSize(); i++)
905  {
906  ranges_mean.push_back(
907  ssu_out.scanWithUncert.rangeScan
908  .getScanRange(i));
909  ranges_obs.push_back(obs_scan->getScanRange(i));
910  }
911 
912  win.plot(ranges_mean, "3k-", "mean");
913  win.plot(ranges_obs, "r-", "obs");
914 
915  Eigen::VectorXd ci1 =
916  ssu_out.scanWithUncert.rangesMean.asEigen() +
917  3 * ssu_out.scanWithUncert.rangesCovar.asEigen()
918  .diagonal()
919  .array()
920  .sqrt()
921  .matrix();
922  Eigen::VectorXd ci2 =
923  ssu_out.scanWithUncert.rangesMean.asEigen() -
924  3 * ssu_out.scanWithUncert.rangesCovar.asEigen()
925  .diagonal()
926  .array()
927  .sqrt()
928  .matrix();
929  win.plot(ci1, "k-", "CI+");
930  win.plot(ci2, "k-", "CI-");
931 
932  win.setWindowTitle(mrpt::format(
933  "obs_reliability_estim: %f",
934  obs_reliability_estim));
935  win.axis_fit();
936  }
937  }
939  "Reliability measure [0-1]: " << obs_reliability_estim);
940  }
941 
942  if (!SAVE_STATS_ONLY)
943  {
944  f_cov_est.printf("%e\n", sqrt(cov.det()));
945  f_pf_stats.printf(
946  "%u %e %e %f %f\n", (unsigned int)pdf.size(),
947  PF_stats.ESS_beforeResample,
949  obs_reliability_estim,
950  sqrt(current_pdf_gaussian.cov.det()));
951  f_odo_est.printf(
952  "%f %f %f\n", odometryEstimation.x(),
953  odometryEstimation.y(), odometryEstimation.phi());
954  }
955 
956  if ((!SAVE_STATS_ONLY && SCENE3D_FREQ > 0) ||
957  SHOW_PROGRESS_3D_REAL_TIME)
958  {
959  // Generate 3D scene:
960  // ------------------------------
961 
962  // The Ground Truth (GT):
963  if (GT.rows() > 0)
964  {
965  CRenderizable::Ptr GTpt = scene.getByName("GT");
966  if (!GTpt)
967  {
968  GTpt = std::make_shared<CDisk>();
969  GTpt = std::make_shared<CDisk>();
970  GTpt->setName("GT");
971  GTpt->setColor(0, 0, 0, 0.9);
972 
973  mrpt::ptr_cast<CDisk>::from(GTpt)->setDiskRadius(
974  0.04f);
975  scene.insert(GTpt);
976  }
977 
978  GTpt->setPose(expectedPose);
979  }
980 
981  // The particles:
982  {
983  CRenderizable::Ptr parts = scene.getByName("particles");
984  if (parts) scene.removeObject(parts);
985 
986  auto p =
987  pdf.template getAs3DObject<CSetOfObjects::Ptr>();
988  p->setName("particles");
989  scene.insert(p);
990  }
991 
992  // The laser scan:
993  {
994  CRenderizable::Ptr scanPts = scene.getByName("scan");
995  if (!scanPts)
996  {
997  scanPts = std::make_shared<CPointCloud>();
998  scanPts->setName("scan");
999  scanPts->setColor(1, 0, 0, 0.9);
1001  ->enableColorFromZ(false);
1003  ->setPointSize(4);
1004  scene.insert(scanPts);
1005  }
1006 
1007  CSimplePointsMap map;
1008 
1009  CPose3D robotPose3D(meanPose);
1010 
1011  map.clear();
1012  observations->insertObservationsInto(&map);
1013 
1015  ->loadFromPointsMap(&map);
1016  mrpt::ptr_cast<CPointCloud>::from(scanPts)->setPose(
1017  robotPose3D);
1018  }
1019 
1020  // The camera:
1021  scene.enableFollowCamera(SCENE3D_FOLLOW);
1022 
1023  // Views:
1024  COpenGLViewport::Ptr view1 = scene.getViewport("main");
1025  {
1026  CCamera& cam = view1->getCamera();
1027  cam.setAzimuthDegrees(-90);
1028  cam.setElevationDegrees(90);
1029  cam.setPointingAt(meanPose);
1030  cam.setZoomDistance(5);
1031  cam.setOrthogonal();
1032  }
1033  }
1034 
1035  if (!SAVE_STATS_ONLY && SCENE3D_FREQ != -1 &&
1036  ((step + 1) % SCENE3D_FREQ) == 0)
1037  {
1038  // Save 3D scene:
1040  "%s/progress_%05u.3Dscene", sOUT_DIR_3D.c_str(),
1041  (unsigned)step));
1042  archiveFrom(f) << scene;
1043 
1044  // Generate text files for matlab:
1045  // ------------------------------------
1046  pdf.saveToTextFile(format(
1047  "%s/particles_%05u.txt", sOUT_DIR_PARTS.c_str(),
1048  (unsigned)step));
1049  }
1050 
1051  }; // while rawlogEntries
1052 
1053  indivConvergenceErrors.saveToTextFile(sOUT_DIR + "/GT_error.txt");
1054  odoError.saveToTextFile(sOUT_DIR + "/ODO_error.txt");
1055  executionTimes.saveToTextFile(sOUT_DIR + "/exec_times.txt");
1056 
1057  if (win3D && NUM_REPS == 1) mrpt::system::pause();
1058  }; // for repetitions
1059 
1060  CTicTac tictacGlobal;
1061  tictacGlobal.Tic();
1062 
1063  const auto max_num_threads = std::thread::hardware_concurrency();
1064  size_t runs_per_thread = NUM_REPS;
1065  if (max_num_threads > 1)
1066  runs_per_thread = static_cast<size_t>(
1067  std::ceil((NUM_REPS) / static_cast<double>(max_num_threads)));
1068 
1070  "Running " << NUM_REPS << " repetitions, on max " << max_num_threads
1071  << " parallel threads: " << runs_per_thread
1072  << " runs/thread.");
1073 
1074  std::vector<std::thread> running_tasks;
1075  for (size_t r = 0; r < NUM_REPS; r += runs_per_thread)
1076  {
1077  auto runner = [&](size_t i_start, size_t i_end) {
1078  if (i_end > NUM_REPS) i_end = NUM_REPS; // sanity check
1079  for (size_t i = i_start; i < i_end; i++)
1080  run_localization_code(i);
1081  };
1082 
1083  running_tasks.emplace_back(runner, r, r + runs_per_thread);
1084  std::this_thread::sleep_for(std::chrono::milliseconds(100));
1085  }
1086 
1087  // Wait for all threads to end:
1088  for (auto& t : running_tasks)
1089  if (t.joinable()) t.join();
1090 
1091  double repetitionTime = tictacGlobal.Tac();
1092 
1093  // Avr. error:
1094  double covergenceErrorMean = 0, convergenceErrorsMin = 0,
1095  convergenceErrorsMax = 0;
1096  if (!convergenceErrors.empty())
1098  convergenceErrors, covergenceErrorMean, convergenceErrorsMin,
1099  convergenceErrorsMax, STATS_CONF_INTERVAL);
1100 
1101  // Save overall results:
1102  {
1104  format("%s_SUMMARY.txt", OUT_DIR_PREFIX.c_str()),
1105  true /* append */);
1106 
1107  f.printf(
1108  "%% Ratio_covergence_success #particles "
1109  "average_time_per_execution convergence_mean_error "
1110  "convergence_error_conf_int_inf "
1111  "convergence_error_conf_int_sup "
1112  "\n");
1113  if (!nConvergenceTests) nConvergenceTests = 1;
1114  f.printf(
1115  "%f %u %f %f %f %f\n",
1116  ((double)nConvergenceOK) / nConvergenceTests, PARTICLE_COUNT,
1117  repetitionTime / NUM_REPS, covergenceErrorMean,
1118  convergenceErrorsMin, convergenceErrorsMax);
1119  }
1120 
1121  MRPT_LOG_INFO_FMT("Total execution time: %.06f sec", repetitionTime);
1122 
1123  } // end of loop for different # of particles
1124 }
1125 
1127  mrpt::poses::CPose2D& expectedPose, size_t rawlogEntry,
1128  const mrpt::math::CMatrixDouble& GT, const Clock::time_point& cur_time)
1129 {
1130  // Either:
1131  // - time x y phi
1132  // or
1133  // - time x y z yaw pitch roll
1134 
1135  if (GT.cols() == 4 || GT.cols() == 7)
1136  {
1137  bool GT_index_is_time;
1138 
1139  // First column can be: timestamps, or rawlogentries:
1140  // Auto-figure it out:
1141  if (GT.rows() > 2)
1142  {
1143  GT_index_is_time =
1144  floor(GT(0, 0)) != GT(0, 0) && floor(GT(1, 0)) != GT(1, 0);
1145  }
1146  else
1147  {
1148  GT_index_is_time = false;
1149  }
1150 
1151  if (GT_index_is_time)
1152  {
1153  // Look for the timestamp:
1154  using namespace std::chrono_literals;
1155 
1156  bool interp_ok = false;
1157  GT_path.interpolate(cur_time, expectedPose, interp_ok);
1158  if (!interp_ok)
1159  {
1160  /*
1161  cerr << format(
1162  "GT time not found: %f\n",
1163  mrpt::system::timestampTotime_t(cur_time));
1164  */
1165  }
1166  }
1167  else
1168  {
1169  // Look for the rawlogEntry:
1170  size_t k, N = GT.rows();
1171  for (k = 0; k < N; k++)
1172  {
1173  if (GT(k, 0) == rawlogEntry) break;
1174  }
1175 
1176  if (k < N)
1177  {
1178  expectedPose.x(GT(k, 1));
1179  expectedPose.y(GT(k, 2));
1180  expectedPose.phi(GT(k, 3));
1181  }
1182  }
1183  }
1184  else if (GT.cols() == 3)
1185  {
1186  if ((int)rawlogEntry < GT.rows())
1187  {
1188  expectedPose.x(GT(rawlogEntry, 0));
1189  expectedPose.y(GT(rawlogEntry, 1));
1190  expectedPose.phi(GT(rawlogEntry, 2));
1191  }
1192  }
1193  else if (GT.cols() > 0)
1194  THROW_EXCEPTION("Unexpected number of columns in ground truth file");
1195 }
1196 
1198 {
1199  // Either:
1200  // - time x y phi
1201  // or
1202  // - time x y z yaw pitch roll
1203  if (GT.cols() == 4 || GT.cols() == 7)
1204  {
1205  const bool GT_is_3D = (GT.cols() == 7);
1206  bool GT_index_is_time = false;
1207 
1208  // First column can be: timestamps, or rawlogentries:
1209  // Auto-figure it out:
1210  if (GT.rows() > 2)
1211  {
1212  GT_index_is_time =
1213  floor(GT(0, 0)) != GT(0, 0) && floor(GT(1, 0)) != GT(1, 0);
1214  }
1215 
1216  if (GT_index_is_time)
1217  {
1218  // Look for the timestamp:
1219  using namespace std::chrono_literals;
1220  GT_path.setMaxTimeInterpolation(200ms);
1221 
1222  for (int i = 0; i < GT.rows(); i++)
1223  {
1224  GT_path.insert(
1225  mrpt::Clock::fromDouble(GT(i, 0)),
1226  TPose2D(GT(i, 1), GT(i, 2), GT(i, GT_is_3D ? 4 : 3)));
1227  }
1228  }
1229  }
1230 }
1231 
1233 {
1234  MRPT_START
1235 
1236  // Detect 2D vs 3D particle filter?
1237  const bool is_3D = params.read_bool(sect, "use_3D_poses", false);
1238 
1239  if (is_3D)
1240  {
1241  MRPT_LOG_INFO("Running for: CMonteCarloLocalization3D");
1242  do_pf_localization<CMonteCarloLocalization3D>();
1243  }
1244  else
1245  {
1246  MRPT_LOG_INFO("Running for: CMonteCarloLocalization2D");
1247  do_pf_localization<CMonteCarloLocalization2D>();
1248  }
1249 
1250  MRPT_END
1251 }
1252 
1253 // ---------------------------------------
1254 // MonteCarloLocalization_Rawlog
1255 // ---------------------------------------
1257 {
1258  setLoggerName("MonteCarloLocalization_Rawlog");
1259 }
1260 
1262 {
1263  MRPT_START
1264  // Rawlog file: from args. line or from config file:
1265  if (argc == 3)
1266  m_rawlogFileName = std::string(argv[2]);
1267  else
1268  m_rawlogFileName = params.read_string(
1269  sect, "rawlog_file", std::string("log.rawlog"), true);
1270 
1271  m_rawlog_offset = params.read_int(sect, "rawlog_offset", 0);
1272 
1273  ASSERT_FILE_EXISTS_(m_rawlogFileName);
1274 
1275  MRPT_END
1276 }
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:87
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: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
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)
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:165
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...
static void resetOnFreeSpace([[maybe_unused]] CMonteCarloLocalization3D &pdf, [[maybe_unused]] mrpt::maps::CMultiMetricMap &metricMap, [[maybe_unused]] size_t PARTICLE_COUNT, [[maybe_unused]] const mrpt::math::TPose3D &init_min, [[maybe_unused]] const mrpt::math::TPose3D &init_max)
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:441
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:403
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:198
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:76
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.5 Git: 40e60e732 Thu Jul 9 08:38:35 2020 +0200 at jue jul 9 08:45:11 CEST 2020