MRPT  1.9.9
KFSLAMApp.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 
12 #include <mrpt/apps/KFSLAMApp.h>
13 
20 #include <mrpt/obs/CRawlog.h>
26 #include <mrpt/system/filesystem.h>
27 #include <mrpt/system/os.h>
29 #include <fstream>
30 
31 using namespace mrpt::apps;
32 
33 KFSLAMApp::KFSLAMApp() : mrpt::system::COutputLogger("KFSLAMApp") {}
34 
35 void KFSLAMApp::initialize(int argc, const char** argv)
36 {
38 
40  " kf-slam - Part of the MRPT\n"
41  " MRPT C++ Library: %s - Sources timestamp: %s\n\n",
44 
45  // Process arguments:
46  if (argc < 2)
47  {
48  THROW_EXCEPTION("Usage: kf-slam <config_file> [dataset.rawlog]");
49  }
50 
51  // Config file:
52  const std::string configFile = std::string(argv[1]);
53 
54  ASSERT_FILE_EXISTS_(configFile);
56 
57  // Rawlog file: from args. line or from config file:
58  if (argc == 3)
59  rawlogFileName = std::string(argv[2]);
60  else
62  "MappingApplication", "rawlog_file", std::string("log.rawlog"));
63 
64  MRPT_END
65 }
66 
68 {
70 
71  // 2D or 3D implementation:
72  const auto kf_implementation = mrpt::system::trim(params.read_string(
73  "MappingApplication", "kf_implementation", "CRangeBearingKFSLAM"));
74 
75  if (kf_implementation == "CRangeBearingKFSLAM")
76  Run_KF_SLAM<mrpt::slam::CRangeBearingKFSLAM>();
77  else if (kf_implementation == "CRangeBearingKFSLAM2D")
78  Run_KF_SLAM<mrpt::slam::CRangeBearingKFSLAM2D>();
79  else
80  throw std::runtime_error(
81  "kf_implementation: Invalid value found in the config file.");
82 
83  MRPT_END
84 }
85 
86 using namespace mrpt;
87 using namespace mrpt::slam;
88 using namespace mrpt::maps;
89 using namespace mrpt::opengl;
90 using namespace mrpt::system;
91 using namespace mrpt::math;
92 using namespace mrpt::poses;
93 using namespace mrpt::config;
94 using namespace mrpt::io;
95 using namespace mrpt::img;
96 using namespace mrpt::obs;
97 using namespace std;
98 
99 // ------------------------------------------------------
100 // traits
101 // ------------------------------------------------------
102 template <class IMPL>
104 
105 // Specialization for 2D (3D) SLAM:
106 template <>
108 {
111  using pose_t = CPose2D;
112  using lm_t = TPoint2D;
113  template <class ARR>
114  static void landmark_to_3d(const ARR& lm, TPoint3D& p)
115  {
116  p.x = lm[0];
117  p.y = lm[1];
118  p.z = 0;
119  }
120 
122  [[maybe_unused]] ekfslam_t& mapping,
123  [[maybe_unused]] CMatrixDouble& fullCov,
124  [[maybe_unused]] const string& OUT_DIR)
125  {
126  // Nothing to do
127  }
128 };
129 
130 // Specialization for 3D (6D) SLAM:
131 template <>
133 {
136  using pose_t = CPose3D;
137  using lm_t = CPoint3D;
138 
139  template <class ARR>
140  static void landmark_to_3d(const ARR& lm, TPoint3D& p)
141  {
142  p.x = lm[0];
143  p.y = lm[1];
144  p.z = lm[2];
145  }
146 
148  ekfslam_t& mapping, CMatrixDouble& fullCov, const string& OUT_DIR)
149  {
150  // Compute the "information" between partitions:
151  if (mapping.options.doPartitioningExperiment)
152  {
153  // --------------------------------------------
154  // PART I:
155  // Comparison to fixed partitioning every K obs.
156  // --------------------------------------------
157 
158  // Compute the information matrix:
159  for (size_t i = 0; i < 6; i++)
160  fullCov(i, i) = max(fullCov(i, i), 1e-6);
161 
162  CMatrixF H(fullCov.inverse_LLt());
163  H.saveToTextFile(OUT_DIR + string("/information_matrix_final.txt"));
164 
165  // Replace by absolute values:
166  H = H.array().abs().matrix();
167  CMatrixF H2(H);
168  CImage imgF;
169  imgF.setFromMatrix(H2, false /*it's not normalized*/);
170  imgF.saveToFile(OUT_DIR + string("/information_matrix_final.png"));
171 
172  // ----------------------------------------
173  // Compute the "approximation error factor" E:
174  // E = SUM() / SUM(ALL ELEMENTS IN MATRIX)
175  // ----------------------------------------
176  vector<std::vector<uint32_t>> landmarksMembership, partsInObsSpace;
177  mrpt::math::CMatrixDouble ERRS(50, 3);
178 
179  for (int i = 0; i < ERRS.rows(); i++)
180  {
181  int K;
182 
183  if (i == 0)
184  {
185  K = 0;
186  mapping.getLastPartitionLandmarks(landmarksMembership);
187  }
188  else
189  {
190  K = i + 1;
192  i + 1, landmarksMembership);
193  }
194 
195  mapping.getLastPartition(partsInObsSpace);
196 
197  ERRS(i, 0) = K;
198  ERRS(i, 1) = partsInObsSpace.size();
199  ERRS(i, 2) = mapping.computeOffDiagonalBlocksApproximationError(
200  landmarksMembership);
201  }
202 
203  ERRS.saveToTextFile(OUT_DIR + string("/ERRORS.txt"));
204 
205  // --------------------------------------------
206  // PART II:
207  // Sweep partitioning threshold:
208  // --------------------------------------------
209  size_t STEPS = 50;
210  CVectorFloat ERRS_SWEEP(STEPS), ERRS_SWEEP_THRESHOLD(STEPS);
211 
212  // Compute the error for each partitioning-threshold
213  for (size_t i = 0; i < STEPS; i++)
214  {
215  float th = (1.0f * i) / (STEPS - 1.0f);
216  ERRS_SWEEP_THRESHOLD[i] = th;
217  mapping.mapPartitionOptions()->partitionThreshold = th;
218 
219  mapping.reconsiderPartitionsNow();
220 
221  mapping.getLastPartitionLandmarks(landmarksMembership);
222  ERRS_SWEEP[i] =
224  landmarksMembership);
225  }
226 
227  ERRS_SWEEP.saveToTextFile(OUT_DIR + string("/ERRORS_SWEEP.txt"));
228  ERRS_SWEEP_THRESHOLD.saveToTextFile(
229  OUT_DIR + string("/ERRORS_SWEEP_THRESHOLD.txt"));
230 
231  } // end if doPartitioningExperiment
232  }
233 };
234 
235 template <class IMPL>
237 {
238  MRPT_START
239 
240  auto& cfgFile = params; // for backwards compat of old code
241 
242  // The EKF-SLAM class:
243  // Traits for this KF implementation (2D or 3D)
244  using traits_t = kfslam_traits<IMPL>;
245  using ekfslam_t = typename traits_t::ekfslam_t;
246 
247  ekfslam_t mapping;
248 
249  // The rawlog file:
250  // ----------------------------------------
251  const unsigned int rawlog_offset =
252  cfgFile.read_int("MappingApplication", "rawlog_offset", 0);
253 
254  const unsigned int SAVE_LOG_FREQUENCY =
255  cfgFile.read_int("MappingApplication", "SAVE_LOG_FREQUENCY", 1);
256 
257  const bool SAVE_DA_LOG =
258  cfgFile.read_bool("MappingApplication", "SAVE_DA_LOG", true);
259 
260  const bool SAVE_3D_SCENES =
261  cfgFile.read_bool("MappingApplication", "SAVE_3D_SCENES", true);
262  const bool SAVE_MAP_REPRESENTATIONS = cfgFile.read_bool(
263  "MappingApplication", "SAVE_MAP_REPRESENTATIONS", true);
264  bool SHOW_3D_LIVE =
265  cfgFile.read_bool("MappingApplication", "SHOW_3D_LIVE", false);
266  const bool CAMERA_3DSCENE_FOLLOWS_ROBOT = cfgFile.read_bool(
267  "MappingApplication", "CAMERA_3DSCENE_FOLLOWS_ROBOT", false);
268 
269 #if !MRPT_HAS_WXWIDGETS
270  SHOW_3D_LIVE = false;
271 #endif
272 
273  string OUT_DIR = cfgFile.read_string(
274  "MappingApplication", "logOutput_dir", "OUT_KF-SLAM");
275  string ground_truth_file =
276  cfgFile.read_string("MappingApplication", "ground_truth_file", "");
277  string ground_truth_file_robot = cfgFile.read_string(
278  "MappingApplication", "ground_truth_file_robot", "");
279 
280  string ground_truth_data_association = cfgFile.read_string(
281  "MappingApplication", "ground_truth_data_association", "");
282 
283  MRPT_LOG_INFO_STREAM("RAWLOG FILE: " << rawlogFileName);
284  ASSERT_FILE_EXISTS_(rawlogFileName);
285  CFileGZInputStream rawlogFile(rawlogFileName);
286 
287  deleteFilesInDirectory(OUT_DIR);
288  createDirectory(OUT_DIR);
289 
290  // Load the config options for mapping:
291  // ----------------------------------------
292  mapping.loadOptions(cfgFile);
293  {
294  std::stringstream o;
295  mapping.KF_options.dumpToTextStream(o);
296  mapping.options.dumpToTextStream(o);
297  MRPT_LOG_INFO(o.str());
298  }
299 
300  // debug:
301  // mapping.KF_options.use_analytic_observation_jacobian = true;
302  // mapping.KF_options.use_analytic_transition_jacobian = true;
303  // mapping.KF_options.debug_verify_analytic_jacobians = true;
304 
305  // Is there ground truth of the robot poses??
306  CMatrixDouble GT_PATH(0, 0);
307  if (ground_truth_file_robot.size() && fileExists(ground_truth_file_robot))
308  {
309  GT_PATH.loadFromTextFile(ground_truth_file_robot);
310  ASSERT_(GT_PATH.rows() > 0 && GT_PATH.cols() == 6);
311  }
312 
313  // Is there a ground truth file of the data association?
314  // Map: timestamp -> vector(index in observation -> real index)
315  std::map<double, std::vector<int>> GT_DA;
316  // Landmark indices bimapping: SLAM DA <---> GROUND TRUTH DA
317  mrpt::containers::bimap<int, int> DA2GTDA_indices;
318  if (!ground_truth_data_association.empty() &&
319  fileExists(ground_truth_data_association))
320  {
321  CMatrixDouble mGT_DA;
322  mGT_DA.loadFromTextFile(ground_truth_data_association);
323  ASSERT_ABOVEEQ_(mGT_DA.cols(), 3);
324  // Convert the loaded matrix into a std::map in GT_DA:
325  for (int i = 0; i < mGT_DA.rows(); i++)
326  {
327  std::vector<int>& v = GT_DA[mGT_DA(i, 0)];
328  if (v.size() <= mGT_DA(i, 1)) v.resize(mGT_DA(i, 1) + 1);
329  v[mGT_DA(i, 1)] = mGT_DA(i, 2);
330  }
332  "Loaded " << GT_DA.size() << " entries from DA ground truth file.");
333  }
334 
335  // Create output file for DA perf:
336  std::ofstream out_da_performance_log;
337  {
338  const std::string f = std::string(
339  OUT_DIR + std::string("/data_association_performance.log"));
340  out_da_performance_log.open(f.c_str());
341  ASSERTMSG_(
342  out_da_performance_log.is_open(),
343  std::string("Error writing to: ") + f);
344 
345  // Header:
346  out_da_performance_log
347  << "% TIMESTAMP INDEX_IN_OBS TruePos "
348  "FalsePos TrueNeg FalseNeg NoGroundTruthSoIDontKnow \n"
349  << "%--------------------------------------------------------------"
350  "--------------------------------------------------\n";
351  }
352 
354 
355  if (SHOW_3D_LIVE)
356  {
357  win3d =
358  mrpt::gui::CDisplayWindow3D::Create("KF-SLAM live view", 800, 500);
359 
360  win3d->addTextMessage(
361  0.01, 0.96, "Red: Estimated path", TColorf(0.8f, 0.8f, 0.8f), 100,
363  win3d->addTextMessage(
364  0.01, 0.93, "Black: Ground truth path", TColorf(0.8f, 0.8f, 0.8f),
366  }
367 
368  // Create DA-log output file:
369  std::ofstream out_da_log;
370  if (SAVE_DA_LOG)
371  {
372  const std::string f =
373  std::string(OUT_DIR + std::string("/data_association.log"));
374  out_da_log.open(f.c_str());
375  ASSERTMSG_(out_da_log.is_open(), std::string("Error writing to: ") + f);
376 
377  // Header:
378  out_da_log << "% TIMESTAMP INDEX_IN_OBS ID "
379  " RANGE(m) YAW(rad) PITCH(rad) \n"
380  << "%-------------------------------------------------------"
381  "-------------------------------------\n";
382  }
383 
384  // The main loop:
385  // ---------------------------------------
386  CActionCollection::Ptr action;
387  CSensoryFrame::Ptr observations;
388  size_t rawlogEntry = 0, step = 0;
389 
390  vector<TPose3D> meanPath; // The estimated path
391  typename traits_t::posepdf_t robotPose;
392  const bool is_pose_3d = robotPose.state_length != 3;
393 
394  std::vector<typename IMPL::landmark_point_t> LMs;
395  std::map<unsigned int, CLandmark::TLandmarkID> LM_IDs;
396  CMatrixDouble fullCov;
397  CVectorDouble fullState;
398  CTicTac kftictac;
399 
400  auto rawlogArch = mrpt::serialization::archiveFrom(rawlogFile);
401 
402  for (;;)
403  {
404  if (os::kbhit())
405  {
406  char pushKey = os::getch();
407  if (27 == pushKey) break;
408  }
409 
410  // Load action/observation pair from the rawlog:
411  // --------------------------------------------------
412  if (!CRawlog::readActionObservationPair(
413  rawlogArch, action, observations, rawlogEntry))
414  break; // file EOF
415 
416  if (rawlogEntry >= rawlog_offset)
417  {
418  // Process the action and observations:
419  // --------------------------------------------
420  kftictac.Tic();
421 
422  mapping.processActionObservation(action, observations);
423 
424  const double tim_kf_iter = kftictac.Tac();
425 
426  // Get current state:
427  // -------------------------------
428  mapping.getCurrentState(robotPose, LMs, LM_IDs, fullState, fullCov);
429  MRPT_LOG_INFO_STREAM("Mean pose: " << robotPose.mean);
430  MRPT_LOG_INFO_STREAM("# of landmarks in the map: " << LMs.size());
431 
432  // Get the mean robot pose as 3D:
433  const CPose3D robotPoseMean3D = CPose3D(robotPose.mean);
434 
435  // Build the path:
436  meanPath.push_back(robotPoseMean3D.asTPose());
437 
438  // Save mean pose:
439  if (!(step % SAVE_LOG_FREQUENCY))
440  {
441  const auto p = robotPose.mean.asVectorVal();
442  p.saveToTextFile(
443  OUT_DIR +
444  format("/robot_pose_%05u.txt", (unsigned int)step));
445  }
446 
447  // Save full cov:
448  if (!(step % SAVE_LOG_FREQUENCY))
449  {
450  fullCov.saveToTextFile(
451  OUT_DIR + format("/full_cov_%05u.txt", (unsigned int)step));
452  }
453 
454  // Generate Data Association log?
455  if (SAVE_DA_LOG)
456  {
457  const typename ekfslam_t::TDataAssocInfo& da =
458  mapping.getLastDataAssociation();
459 
461  observations
462  ->getObservationByClass<CObservationBearingRange>();
463  if (obs)
464  {
465  const CObservationBearingRange* obsRB = obs.get();
466  const double tim =
468 
469  for (size_t i = 0; i < obsRB->sensedData.size(); i++)
470  {
471  auto it = da.results.associations.find(i);
472  int assoc_ID_in_SLAM;
473  if (it != da.results.associations.end())
474  assoc_ID_in_SLAM = it->second;
475  else
476  {
477  // It should be a newly created LM:
478  auto itNewLM = da.newly_inserted_landmarks.find(i);
479  if (itNewLM != da.newly_inserted_landmarks.end())
480  assoc_ID_in_SLAM = itNewLM->second;
481  else
482  assoc_ID_in_SLAM = -1;
483  }
484 
485  out_da_log << format(
486  "%35.22f %8i %10i %10f %12f %12f\n", tim, (int)i,
487  assoc_ID_in_SLAM,
488  (double)obsRB->sensedData[i].range,
489  (double)obsRB->sensedData[i].yaw,
490  (double)obsRB->sensedData[i].pitch);
491  }
492  }
493  }
494 
495  // Save report on DA performance:
496  {
497  const typename ekfslam_t::TDataAssocInfo& da =
498  mapping.getLastDataAssociation();
499 
501  observations
502  ->getObservationByClass<CObservationBearingRange>();
503  if (obs)
504  {
505  const CObservationBearingRange* obsRB = obs.get();
506  const double tim =
508 
509  auto itDA = GT_DA.find(tim);
510 
511  for (size_t i = 0; i < obsRB->sensedData.size(); i++)
512  {
513  bool is_FP = false, is_TP = false, is_FN = false,
514  is_TN = false;
515 
516  if (itDA != GT_DA.end())
517  {
518  const std::vector<int>& vDA = itDA->second;
519  ASSERT_BELOW_(i, vDA.size());
520  const int GT_ASSOC = vDA[i];
521 
522  auto it = da.results.associations.find(i);
523  if (it != da.results.associations.end())
524  {
525  // This observation was assigned the already
526  // existing LM in the map: "it->second"
527  // TruePos -> If that LM index corresponds to
528  // that in the GT (with index mapping):
529 
530  // mrpt::containers::bimap<int,int>
531  // DA2GTDA_indices;
532  // // Landmark indices bimapping: SLAM DA <--->
533  // GROUND TRUTH DA
534  if (DA2GTDA_indices.hasKey(it->second))
535  {
536  const int slam_asigned_LM_idx =
537  DA2GTDA_indices.direct(it->second);
538  if (slam_asigned_LM_idx == GT_ASSOC)
539  is_TP = true;
540  else
541  is_FP = true;
542  }
543  else
544  {
545  // Is this case possible? Assigned to an
546  // index not ever seen for the first time
547  // with a GT....
548  // Just in case:
549  is_FP = true;
550  }
551  }
552  else
553  {
554  // No pairing, but should be a newly created LM:
555  auto itNewLM =
556  da.newly_inserted_landmarks.find(i);
557  if (itNewLM !=
558  da.newly_inserted_landmarks.end())
559  {
560  const int new_LM_in_SLAM = itNewLM->second;
561 
562  // Was this really a NEW LM not observed
563  // before?
564  if (DA2GTDA_indices.hasValue(GT_ASSOC))
565  {
566  // GT says this LM was already observed,
567  // so it shouldn't appear here as new:
568  is_FN = true;
569  }
570  else
571  {
572  // Really observed for the first time:
573  is_TN = true;
574  DA2GTDA_indices.insert(
575  new_LM_in_SLAM, GT_ASSOC);
576  }
577  }
578  else
579  {
580  // Not associated neither inserted:
581  // Shouldn't really never arrive here.
582  }
583  }
584  }
585 
586  // "% TIMESTAMP INDEX_IN_OBS
587  // TruePos FalsePos TrueNeg FalseNeg
588  // NoGroundTruthSoIDontKnow \n"
589  out_da_performance_log << format(
590  "%35.22f %13i %8i %8i %8i %8i %8i\n", tim, (int)i,
591  (int)(is_TP ? 1 : 0), (int)(is_FP ? 1 : 0),
592  (int)(is_TN ? 1 : 0), (int)(is_FN ? 1 : 0),
593  (int)(!is_FP && !is_TP && !is_FN && !is_TN ? 1 : 0));
594  }
595  }
596  }
597 
598  // Save map to file representations?
599  if (SAVE_MAP_REPRESENTATIONS && !(step % SAVE_LOG_FREQUENCY))
600  {
601  mapping.saveMapAndPath2DRepresentationAsMATLABFile(
602  OUT_DIR + format("/slam_state_%05u.m", (unsigned int)step));
603  }
604 
605  // Save 3D view of the filter state:
606  if (win3d || (SAVE_3D_SCENES && !(step % SAVE_LOG_FREQUENCY)))
607  {
608  COpenGLScene::Ptr scene3D = std::make_shared<COpenGLScene>();
609  {
611  std::make_shared<opengl::CGridPlaneXY>(
612  -1000, 1000, -1000, 1000, 0, 5);
613  grid->setColor(0.4, 0.4, 0.4);
614  scene3D->insert(grid);
615  }
616 
617  // Robot path:
618  {
619  opengl::CSetOfLines::Ptr linesPath =
620  std::make_shared<opengl::CSetOfLines>();
621  linesPath->setColor(1, 0, 0);
622 
623  TPose3D init_pose;
624  if (!meanPath.empty())
625  init_pose = CPose3D(meanPath[0]).asTPose();
626 
627  int path_decim = 0;
628  for (auto& it : meanPath)
629  {
630  linesPath->appendLine(init_pose, it);
631  init_pose = it;
632 
633  if (++path_decim > 10)
634  {
635  path_decim = 0;
638  0.3f, 2.0f);
639  xyz->setPose(CPose3D(it));
640  scene3D->insert(xyz);
641  }
642  }
643  scene3D->insert(linesPath);
644 
645  // finally a big corner for the latest robot pose:
646  {
649  1.0, 2.5);
650  xyz->setPose(robotPoseMean3D);
651  scene3D->insert(xyz);
652  }
653 
654  // The camera pointing to the current robot pose:
655  if (win3d && CAMERA_3DSCENE_FOLLOWS_ROBOT)
656  {
657  win3d->setCameraPointingToPoint(
658  robotPoseMean3D.x(), robotPoseMean3D.y(),
659  robotPoseMean3D.z());
660  }
661  }
662 
663  // Do we have a ground truth?
664  if (GT_PATH.cols() == 6 || GT_PATH.cols() == 3)
665  {
666  opengl::CSetOfLines::Ptr GT_path =
667  std::make_shared<opengl::CSetOfLines>();
668  GT_path->setColor(0, 0, 0);
669  size_t N =
670  std::min((int)GT_PATH.rows(), (int)meanPath.size());
671 
672  if (GT_PATH.cols() == 6)
673  {
674  double gtx0 = 0, gty0 = 0, gtz0 = 0;
675  for (size_t i = 0; i < N; i++)
676  {
677  const CPose3D p(
678  GT_PATH(i, 0), GT_PATH(i, 1), GT_PATH(i, 2),
679  GT_PATH(i, 3), GT_PATH(i, 4), GT_PATH(i, 5));
680 
681  GT_path->appendLine(
682  gtx0, gty0, gtz0, p.x(), p.y(), p.z());
683  gtx0 = p.x();
684  gty0 = p.y();
685  gtz0 = p.z();
686  }
687  }
688  else if (GT_PATH.cols() == 3)
689  {
690  double gtx0 = 0, gty0 = 0;
691  for (size_t i = 0; i < N; i++)
692  {
693  const CPose2D p(
694  GT_PATH(i, 0), GT_PATH(i, 1), GT_PATH(i, 2));
695 
696  GT_path->appendLine(gtx0, gty0, 0, p.x(), p.y(), 0);
697  gtx0 = p.x();
698  gty0 = p.y();
699  }
700  }
701  scene3D->insert(GT_path);
702  }
703 
704  // Draw latest data association:
705  {
706  const typename ekfslam_t::TDataAssocInfo& da =
707  mapping.getLastDataAssociation();
708 
711  lins->setLineWidth(1.2f);
712  lins->setColor(1, 1, 1);
713  for (auto it = da.results.associations.begin();
714  it != da.results.associations.end(); ++it)
715  {
716  const prediction_index_t idxPred = it->second;
717  // This index must match the internal list of features
718  // in the map:
719  typename ekfslam_t::KFArray_FEAT featMean;
720  mapping.getLandmarkMean(idxPred, featMean);
721 
722  TPoint3D featMean3D;
723  traits_t::landmark_to_3d(featMean, featMean3D);
724 
725  // Line: robot -> landmark:
726  lins->appendLine(
727  robotPoseMean3D.x(), robotPoseMean3D.y(),
728  robotPoseMean3D.z(), featMean3D.x, featMean3D.y,
729  featMean3D.z);
730  }
731  scene3D->insert(lins);
732  }
733 
734  // The current state of KF-SLAM:
735  {
737  std::make_shared<opengl::CSetOfObjects>();
738  mapping.getAs3DObject(objs);
739  scene3D->insert(objs);
740  }
741 
742  if (win3d)
743  {
745  win3d->get3DSceneAndLock();
746  scn = scene3D;
747 
748  // Update text messages:
749  win3d->addTextMessage(
750  0.02, 0.02,
751  format(
752  "Step %u - Landmarks in the map: %u",
753  (unsigned int)step, (unsigned int)LMs.size()),
755 
756  win3d->addTextMessage(
757  0.02, 0.06,
758  format(
759  is_pose_3d
760  ? "Estimated pose: (x y z qr qx qy qz) = %s"
761  : "Estimated pose: (x y yaw) = %s",
762  robotPose.mean.asString().c_str()),
764 
765  static vector<double> estHz_vals;
766  const double curHz = 1.0 / std::max(1e-9, tim_kf_iter);
767  estHz_vals.push_back(curHz);
768  if (estHz_vals.size() > 50)
769  estHz_vals.erase(estHz_vals.begin());
770  const double meanHz = mrpt::math::mean(estHz_vals);
771 
772  win3d->addTextMessage(
773  0.02, 0.10,
774  format(
775  "Iteration time: %7ss",
776  mrpt::system::unitsFormat(tim_kf_iter).c_str()),
778 
779  win3d->addTextMessage(
780  0.02, 0.14,
781  format(
782  "Execution rate: %7sHz",
783  mrpt::system::unitsFormat(meanHz).c_str()),
785 
786  win3d->unlockAccess3DScene();
787  win3d->repaint();
788  }
789 
790  if (SAVE_3D_SCENES && !(step % SAVE_LOG_FREQUENCY))
791  {
792  // Save to file:
794  OUT_DIR +
795  format("/kf_state_%05u.3Dscene", (unsigned int)step));
796  mrpt::serialization::archiveFrom(f) << *scene3D;
797  }
798  }
799 
800  // Free rawlog items memory:
801  // --------------------------------------------
802  action.reset();
803  observations.reset();
804 
805  } // (rawlogEntry>=rawlog_offset)
806 
808  "Step " << step << " - Rawlog entries processed: " << rawlogEntry);
809 
810  step++;
811  }; // end "while(1)"
812 
813  // Partitioning experiment: Only for 6D SLAM:
814  traits_t::doPartitioningExperiment(mapping, fullCov, OUT_DIR);
815 
816  // Is there ground truth of landmarks positions??
817  if (ground_truth_file.size() && fileExists(ground_truth_file))
818  {
819  CMatrixFloat GT(0, 0);
820  try
821  {
822  GT.loadFromTextFile(ground_truth_file);
823  }
824  catch (const std::exception& e)
825  {
827  "Ignoring the following error loading ground truth file: "
828  << mrpt::exception_to_str(e));
829  }
830 
831  if (GT.rows() > 0 && !LMs.empty())
832  {
833  // Each row has:
834  // [0] [1] [2] [6]
835  // x y z ID
836  CVectorDouble ERRS(0);
837  for (size_t i = 0; i < LMs.size(); i++)
838  {
839  // Find the entry in the GT for this mapped LM:
840  bool found = false;
841  for (int r = 0; r < GT.rows(); r++)
842  {
843  if (std::abs(LM_IDs[i] - GT(r, 6)) < 1e-9)
844  {
845  const CPoint3D gtPt(GT(r, 0), GT(r, 1), GT(r, 2));
846  // All these conversions are to make it work with either
847  // CPoint3D & TPoint2D:
848  ERRS.push_back(
849  gtPt.distanceTo(CPoint3D(TPoint3D(LMs[i]))));
850  found = true;
851  break;
852  }
853  }
854  if (!found)
855  {
857  "Ground truth entry not found for landmark ID:"
858  << LM_IDs[i]);
859  }
860  }
861 
862  loc_error_wrt_gt = math::mean(ERRS);
863 
864  MRPT_LOG_INFO("ERRORS VS. GROUND TRUTH:");
865  MRPT_LOG_INFO_FMT("Mean Error: %f meters\n", loc_error_wrt_gt);
867  "Minimum error: %f meters\n", math::minimum(ERRS));
869  "Maximum error: %f meters\n", math::maximum(ERRS));
870  }
871  } // end if GT
872 
873  MRPT_LOG_INFO("********* KF-SLAM finished! **********");
874 
875  if (win3d)
876  {
877  MRPT_LOG_WARN("Close the 3D window to quit the application.");
878  win3d->waitForKey();
879  }
880 
881  MRPT_END
882 }
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:775
An implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot p...
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:161
An implementation of EKF-based SLAM with range-bearing sensors, odometry, a full 6D robot pose...
#define MRPT_START
Definition: exceptions.h:241
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:204
void getLastPartitionLandmarks(std::vector< std::vector< uint32_t >> &landmarksMembership) const
Return the partitioning of the landmarks in clusters accoring to the last partition.
double timestampToDouble(const mrpt::system::TTimeStamp t) noexcept
Transform from TTimeStamp to standard "time_t" (actually a double number, it can contain fractions of...
Definition: datetime.h:116
Template for column vectors of dynamic size, compatible with Eigen.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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
void reconsiderPartitionsNow()
The partitioning of the entire map is recomputed again.
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
void initialize(int argc, const char **argv)
Initializes the application from CLI parameters.
Definition: KFSLAMApp.cpp:35
This file implements several operations that operate element-wise on individual or pairs of container...
size_t prediction_index_t
Used in mrpt::slam::TDataAssociationResults.
bool hasKey(const KEY &k) const
Return true if the given key &#39;k&#39; is in the bi-map.
Definition: bimap.h:92
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.
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:71
mrpt::vision::TStereoCalibParams params
STL namespace.
static CDisplayWindow3D::Ptr Create(const std::string &windowCaption, unsigned int initialWindowWidth=400, unsigned int initialWindowHeight=300)
Class factory returning a smart pointer.
std::string MRPT_getCompilationDate()
Returns the MRPT source code timestamp, according to the Reproducible-Builds specifications: https://...
Definition: os.cpp:154
std::string rawlogFileName
rawlog to process
Definition: KFSLAMApp.h:55
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.
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
CIncrementalMapPartitioner::TOptions * mapPartitionOptions()
Provides access to the parameters of the map partitioning algorithm.
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
mrpt::config::CConfigFileMemory params
Populated in initialize().
Definition: KFSLAMApp.h:52
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void run()
Runs with the current parameter set.
Definition: KFSLAMApp.cpp:67
This base provides a set of functions for maths stuff.
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:31
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Versatile class for consistent logging and management of output messages.
CONTAINER::Scalar maximum(const CONTAINER &v)
This namespace contains representation of robot actions and observations.
void setFromMatrix(const MAT &m, bool matrix_is_normalized=true)
Set the image from a matrix, interpreted as grayscale intensity values, in the range [0...
Definition: img/CImage.h:832
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:242
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
double computeOffDiagonalBlocksApproximationError(const std::vector< std::vector< uint32_t >> &landmarksMembership) const
Computes the ratio of the missing information matrix elements which are ignored under a certain parti...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
size_type rows() const
Number of rows in the matrix.
A class used to store a 3D point.
Definition: CPoint3D.h:31
size_type cols() const
Number of columns in the matrix.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double partitionThreshold
!< N-cut partition threshold [0,2] (default=1)
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
CONTAINER::Scalar minimum(const CONTAINER &v)
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:23
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const char * argv[]
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
static void doPartitioningExperiment([[maybe_unused]] ekfslam_t &mapping, [[maybe_unused]] CMatrixDouble &fullCov, [[maybe_unused]] const string &OUT_DIR)
Definition: KFSLAMApp.cpp:121
mrpt::slam::CRangeBearingKFSLAM::TOptions options
std::string unitsFormat(const double val, int nDecimalDigits=2, bool middle_space=true)
This function implements formatting with the appropriate SI metric unit prefix: 1e-12->&#39;p&#39;, 1e-9->&#39;n&#39;, 1e-6->&#39;u&#39;, 1e-3->&#39;m&#39;, 1->&#39;&#39;, 1e3->&#39;K&#39;, 1e6->&#39;M&#39;, 1e9->&#39;G&#39;, 1e12->&#39;T&#39;.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
void setContent(const std::vector< std::string > &stringList)
Changes the contents of the virtual "config file".
CSetOfObjects::Ptr CornerXYZSimple(float scale=1.0, float lineWidth=1.0)
Returns three arrows representing a X,Y,Z 3D corner (just thick lines instead of complex arrows for f...
void getLastPartitionLandmarksAsIfFixedSubmaps(size_t K, std::vector< std::vector< uint32_t >> &landmarksMembership)
For testing only: returns the partitioning as "getLastPartitionLandmarks" but as if a fixed-size subm...
#define MRPT_END
Definition: exceptions.h:245
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
A RGB color - floats in the range [0,1].
Definition: TColor.h:78
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
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.
double mean(const CONTAINER &v)
Computes the mean value of a vector.
std::string exception_to_str(const std::exception &e)
Builds a nice textual representation of a nested exception, which if generated using MRPT macros (THR...
Definition: exceptions.cpp:59
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
#define MRPT_LOG_WARN(_STRING)
std::string trim(const std::string &str)
Removes leading and trailing spaces.
Transparently opens a compressed "gz" file and reads uncompressed data from it.
bool doPartitioningExperiment
If set to true (default=false), map will be partitioned using the method stated by partitioningMethod...
const int argc
bool saveToFile(const std::string &fileName, int jpeg_quality=95) const
Save the image to a file, whose format is determined from the extension (internally uses OpenCV)...
Definition: CImage.cpp:330
std::string MRPT_getVersion()
Returns a string describing the MRPT version.
Definition: os.cpp:187
Saves data to a file and transparently compress the data using the given compression level...
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
bool hasValue(const VALUE &v) const
Return true if the given value &#39;v&#39; is in the bi-map.
Definition: bimap.h:98
#define ASSERT_FILE_EXISTS_(FIL)
Definition: filesystem.h:22
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
bool direct(const KEY &k, VALUE &out_v) const
Get the value associated the given key, KEY->VALUE, returning false if not present.
Definition: bimap.h:82
static void doPartitioningExperiment(ekfslam_t &mapping, CMatrixDouble &fullCov, const string &OUT_DIR)
Definition: KFSLAMApp.cpp:147
static void landmark_to_3d(const ARR &lm, TPoint3D &p)
Definition: KFSLAMApp.cpp:140
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
static Ptr Create(Args &&... args)
Definition: CSetOfLines.h:33
void getLastPartition(std::vector< std::vector< uint32_t >> &parts)
Return the last partition of the sequence of sensoryframes (it is NOT a partition of the map!!) Only ...
static void landmark_to_3d(const ARR &lm, TPoint3D &p)
Definition: KFSLAMApp.cpp:114
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147
TMeasurementList sensedData
The list of observed ranges:
#define MRPT_LOG_INFO(_STRING)
void loadFromTextFile(std::istream &f)
Loads a vector/matrix from a text file, compatible with MATLAB text format.



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