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