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-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 
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(0.01, 0.96, "Red: Estimated path", 100);
361  win3d->addTextMessage(0.01, 0.93, "Black: Ground truth path", 101);
362  }
363 
364  // Create DA-log output file:
365  std::ofstream out_da_log;
366  if (SAVE_DA_LOG)
367  {
368  const std::string f =
369  std::string(OUT_DIR + std::string("/data_association.log"));
370  out_da_log.open(f.c_str());
371  ASSERTMSG_(out_da_log.is_open(), std::string("Error writing to: ") + f);
372 
373  // Header:
374  out_da_log << "% TIMESTAMP INDEX_IN_OBS ID "
375  " RANGE(m) YAW(rad) PITCH(rad) \n"
376  << "%-------------------------------------------------------"
377  "-------------------------------------\n";
378  }
379 
380  // The main loop:
381  // ---------------------------------------
382  CActionCollection::Ptr action;
383  CSensoryFrame::Ptr observations;
384  size_t rawlogEntry = 0, step = 0;
385 
386  vector<TPose3D> meanPath; // The estimated path
387  typename traits_t::posepdf_t robotPose;
388  const bool is_pose_3d = robotPose.state_length != 3;
389 
390  std::vector<typename IMPL::landmark_point_t> LMs;
391  std::map<unsigned int, CLandmark::TLandmarkID> LM_IDs;
392  CMatrixDouble fullCov;
393  CVectorDouble fullState;
394  CTicTac kftictac;
395 
396  auto rawlogArch = mrpt::serialization::archiveFrom(rawlogFile);
397 
398  for (;;)
399  {
400  if (os::kbhit())
401  {
402  char pushKey = os::getch();
403  if (27 == pushKey) break;
404  }
405 
406  // Load action/observation pair from the rawlog:
407  // --------------------------------------------------
408  if (!CRawlog::readActionObservationPair(
409  rawlogArch, action, observations, rawlogEntry))
410  break; // file EOF
411 
412  if (rawlogEntry >= rawlog_offset)
413  {
414  // Process the action and observations:
415  // --------------------------------------------
416  kftictac.Tic();
417 
418  mapping.processActionObservation(action, observations);
419 
420  const double tim_kf_iter = kftictac.Tac();
421 
422  // Get current state:
423  // -------------------------------
424  mapping.getCurrentState(robotPose, LMs, LM_IDs, fullState, fullCov);
425  MRPT_LOG_INFO_STREAM("Mean pose: " << robotPose.mean);
426  MRPT_LOG_INFO_STREAM("# of landmarks in the map: " << LMs.size());
427 
428  // Get the mean robot pose as 3D:
429  const CPose3D robotPoseMean3D = CPose3D(robotPose.mean);
430 
431  // Build the path:
432  meanPath.push_back(robotPoseMean3D.asTPose());
433 
434  // Save mean pose:
435  if (!(step % SAVE_LOG_FREQUENCY))
436  {
437  const auto p = robotPose.mean.asVectorVal();
438  p.saveToTextFile(
439  OUT_DIR +
440  format("/robot_pose_%05u.txt", (unsigned int)step));
441  }
442 
443  // Save full cov:
444  if (!(step % SAVE_LOG_FREQUENCY))
445  {
446  fullCov.saveToTextFile(
447  OUT_DIR + format("/full_cov_%05u.txt", (unsigned int)step));
448  }
449 
450  // Generate Data Association log?
451  if (SAVE_DA_LOG)
452  {
453  const typename ekfslam_t::TDataAssocInfo& da =
454  mapping.getLastDataAssociation();
455 
457  observations
458  ->getObservationByClass<CObservationBearingRange>();
459  if (obs)
460  {
461  const CObservationBearingRange* obsRB = obs.get();
462  const double tim =
464 
465  for (size_t i = 0; i < obsRB->sensedData.size(); i++)
466  {
467  auto it = da.results.associations.find(i);
468  int assoc_ID_in_SLAM;
469  if (it != da.results.associations.end())
470  assoc_ID_in_SLAM = it->second;
471  else
472  {
473  // It should be a newly created LM:
474  auto itNewLM = da.newly_inserted_landmarks.find(i);
475  if (itNewLM != da.newly_inserted_landmarks.end())
476  assoc_ID_in_SLAM = itNewLM->second;
477  else
478  assoc_ID_in_SLAM = -1;
479  }
480 
481  out_da_log << format(
482  "%35.22f %8i %10i %10f %12f %12f\n", tim, (int)i,
483  assoc_ID_in_SLAM,
484  (double)obsRB->sensedData[i].range,
485  (double)obsRB->sensedData[i].yaw,
486  (double)obsRB->sensedData[i].pitch);
487  }
488  }
489  }
490 
491  // Save report on DA performance:
492  {
493  const typename ekfslam_t::TDataAssocInfo& da =
494  mapping.getLastDataAssociation();
495 
497  observations
498  ->getObservationByClass<CObservationBearingRange>();
499  if (obs)
500  {
501  const CObservationBearingRange* obsRB = obs.get();
502  const double tim =
504 
505  auto itDA = GT_DA.find(tim);
506 
507  for (size_t i = 0; i < obsRB->sensedData.size(); i++)
508  {
509  bool is_FP = false, is_TP = false, is_FN = false,
510  is_TN = false;
511 
512  if (itDA != GT_DA.end())
513  {
514  const std::vector<int>& vDA = itDA->second;
515  ASSERT_BELOW_(i, vDA.size());
516  const int GT_ASSOC = vDA[i];
517 
518  auto it = da.results.associations.find(i);
519  if (it != da.results.associations.end())
520  {
521  // This observation was assigned the already
522  // existing LM in the map: "it->second"
523  // TruePos -> If that LM index corresponds to
524  // that in the GT (with index mapping):
525 
526  // mrpt::containers::bimap<int,int>
527  // DA2GTDA_indices;
528  // // Landmark indices bimapping: SLAM DA <--->
529  // GROUND TRUTH DA
530  if (DA2GTDA_indices.hasKey(it->second))
531  {
532  const int slam_asigned_LM_idx =
533  DA2GTDA_indices.direct(it->second);
534  if (slam_asigned_LM_idx == GT_ASSOC)
535  is_TP = true;
536  else
537  is_FP = true;
538  }
539  else
540  {
541  // Is this case possible? Assigned to an
542  // index not ever seen for the first time
543  // with a GT....
544  // Just in case:
545  is_FP = true;
546  }
547  }
548  else
549  {
550  // No pairing, but should be a newly created LM:
551  auto itNewLM =
552  da.newly_inserted_landmarks.find(i);
553  if (itNewLM !=
554  da.newly_inserted_landmarks.end())
555  {
556  const int new_LM_in_SLAM = itNewLM->second;
557 
558  // Was this really a NEW LM not observed
559  // before?
560  if (DA2GTDA_indices.hasValue(GT_ASSOC))
561  {
562  // GT says this LM was already observed,
563  // so it shouldn't appear here as new:
564  is_FN = true;
565  }
566  else
567  {
568  // Really observed for the first time:
569  is_TN = true;
570  DA2GTDA_indices.insert(
571  new_LM_in_SLAM, GT_ASSOC);
572  }
573  }
574  else
575  {
576  // Not associated neither inserted:
577  // Shouldn't really never arrive here.
578  }
579  }
580  }
581 
582  // "% TIMESTAMP INDEX_IN_OBS
583  // TruePos FalsePos TrueNeg FalseNeg
584  // NoGroundTruthSoIDontKnow \n"
585  out_da_performance_log << format(
586  "%35.22f %13i %8i %8i %8i %8i %8i\n", tim, (int)i,
587  (int)(is_TP ? 1 : 0), (int)(is_FP ? 1 : 0),
588  (int)(is_TN ? 1 : 0), (int)(is_FN ? 1 : 0),
589  (int)(!is_FP && !is_TP && !is_FN && !is_TN ? 1 : 0));
590  }
591  }
592  }
593 
594  // Save map to file representations?
595  if (SAVE_MAP_REPRESENTATIONS && !(step % SAVE_LOG_FREQUENCY))
596  {
597  mapping.saveMapAndPath2DRepresentationAsMATLABFile(
598  OUT_DIR + format("/slam_state_%05u.m", (unsigned int)step));
599  }
600 
601  // Save 3D view of the filter state:
602  if (win3d || (SAVE_3D_SCENES && !(step % SAVE_LOG_FREQUENCY)))
603  {
604  COpenGLScene::Ptr scene3D = std::make_shared<COpenGLScene>();
605  {
607  std::make_shared<opengl::CGridPlaneXY>(
608  -1000, 1000, -1000, 1000, 0, 5);
609  grid->setColor(0.4f, 0.4f, 0.4f);
610  scene3D->insert(grid);
611  }
612 
613  // Robot path:
614  {
615  opengl::CSetOfLines::Ptr linesPath =
616  std::make_shared<opengl::CSetOfLines>();
617  linesPath->setColor(1, 0, 0);
618 
619  TPose3D init_pose;
620  if (!meanPath.empty())
621  init_pose = CPose3D(meanPath[0]).asTPose();
622 
623  int path_decim = 0;
624  for (auto& it : meanPath)
625  {
626  linesPath->appendLine(init_pose, it);
627  init_pose = it;
628 
629  if (++path_decim > 10)
630  {
631  path_decim = 0;
634  0.3f, 2.0f);
635  xyz->setPose(CPose3D(it));
636  scene3D->insert(xyz);
637  }
638  }
639  scene3D->insert(linesPath);
640 
641  // finally a big corner for the latest robot pose:
642  {
645  1.0, 2.5);
646  xyz->setPose(robotPoseMean3D);
647  scene3D->insert(xyz);
648  }
649 
650  // The camera pointing to the current robot pose:
651  if (win3d && CAMERA_3DSCENE_FOLLOWS_ROBOT)
652  {
653  win3d->setCameraPointingToPoint(
654  robotPoseMean3D.x(), robotPoseMean3D.y(),
655  robotPoseMean3D.z());
656  }
657  }
658 
659  // Do we have a ground truth?
660  if (GT_PATH.cols() == 6 || GT_PATH.cols() == 3)
661  {
662  opengl::CSetOfLines::Ptr GT_path =
663  std::make_shared<opengl::CSetOfLines>();
664  GT_path->setColor(0, 0, 0);
665  size_t N =
666  std::min((int)GT_PATH.rows(), (int)meanPath.size());
667 
668  if (GT_PATH.cols() == 6)
669  {
670  double gtx0 = 0, gty0 = 0, gtz0 = 0;
671  for (size_t i = 0; i < N; i++)
672  {
673  const CPose3D p(
674  GT_PATH(i, 0), GT_PATH(i, 1), GT_PATH(i, 2),
675  GT_PATH(i, 3), GT_PATH(i, 4), GT_PATH(i, 5));
676 
677  GT_path->appendLine(
678  gtx0, gty0, gtz0, p.x(), p.y(), p.z());
679  gtx0 = p.x();
680  gty0 = p.y();
681  gtz0 = p.z();
682  }
683  }
684  else if (GT_PATH.cols() == 3)
685  {
686  double gtx0 = 0, gty0 = 0;
687  for (size_t i = 0; i < N; i++)
688  {
689  const CPose2D p(
690  GT_PATH(i, 0), GT_PATH(i, 1), GT_PATH(i, 2));
691 
692  GT_path->appendLine(gtx0, gty0, 0, p.x(), p.y(), 0);
693  gtx0 = p.x();
694  gty0 = p.y();
695  }
696  }
697  scene3D->insert(GT_path);
698  }
699 
700  // Draw latest data association:
701  {
702  const typename ekfslam_t::TDataAssocInfo& da =
703  mapping.getLastDataAssociation();
704 
707  lins->setLineWidth(1.2f);
708  lins->setColor(1, 1, 1);
709  for (auto it = da.results.associations.begin();
710  it != da.results.associations.end(); ++it)
711  {
712  const prediction_index_t idxPred = it->second;
713  // This index must match the internal list of features
714  // in the map:
715  typename ekfslam_t::KFArray_FEAT featMean;
716  mapping.getLandmarkMean(idxPred, featMean);
717 
718  TPoint3D featMean3D;
719  traits_t::landmark_to_3d(featMean, featMean3D);
720 
721  // Line: robot -> landmark:
722  lins->appendLine(
723  robotPoseMean3D.x(), robotPoseMean3D.y(),
724  robotPoseMean3D.z(), featMean3D.x, featMean3D.y,
725  featMean3D.z);
726  }
727  scene3D->insert(lins);
728  }
729 
730  // The current state of KF-SLAM:
731  {
733  std::make_shared<opengl::CSetOfObjects>();
734  mapping.getAs3DObject(objs);
735  scene3D->insert(objs);
736  }
737 
738  if (win3d)
739  {
741  win3d->get3DSceneAndLock();
742  scn = scene3D;
743 
744  // Update text messages:
745  win3d->addTextMessage(
746  0.02, 0.02,
747  format(
748  "Step %u - Landmarks in the map: %u",
749  (unsigned int)step, (unsigned int)LMs.size()),
750  0);
751 
752  win3d->addTextMessage(
753  0.02, 0.06,
754  format(
755  is_pose_3d
756  ? "Estimated pose: (x y z qr qx qy qz) = %s"
757  : "Estimated pose: (x y yaw) = %s",
758  robotPose.mean.asString().c_str()),
759  1);
760 
761  static vector<double> estHz_vals;
762  const double curHz = 1.0 / std::max(1e-9, tim_kf_iter);
763  estHz_vals.push_back(curHz);
764  if (estHz_vals.size() > 50)
765  estHz_vals.erase(estHz_vals.begin());
766  const double meanHz = mrpt::math::mean(estHz_vals);
767 
768  win3d->addTextMessage(
769  0.02, 0.10,
770  format(
771  "Iteration time: %7ss",
772  mrpt::system::unitsFormat(tim_kf_iter).c_str()),
773  2);
774 
775  win3d->addTextMessage(
776  0.02, 0.14,
777  format(
778  "Execution rate: %7sHz",
779  mrpt::system::unitsFormat(meanHz).c_str()),
780  3);
781 
782  win3d->unlockAccess3DScene();
783  win3d->repaint();
784  }
785 
786  if (SAVE_3D_SCENES && !(step % SAVE_LOG_FREQUENCY))
787  {
788  // Save to file:
790  OUT_DIR +
791  format("/kf_state_%05u.3Dscene", (unsigned int)step));
792  mrpt::serialization::archiveFrom(f) << *scene3D;
793  }
794  }
795 
796  // Free rawlog items memory:
797  // --------------------------------------------
798  action.reset();
799  observations.reset();
800 
801  } // (rawlogEntry>=rawlog_offset)
802 
804  "Step " << step << " - Rawlog entries processed: " << rawlogEntry);
805 
806  step++;
807  }; // end "while(1)"
808 
809  // Partitioning experiment: Only for 6D SLAM:
810  traits_t::doPartitioningExperiment(mapping, fullCov, OUT_DIR);
811 
812  // Is there ground truth of landmarks positions??
813  if (ground_truth_file.size() && fileExists(ground_truth_file))
814  {
815  CMatrixFloat GT(0, 0);
816  try
817  {
818  GT.loadFromTextFile(ground_truth_file);
819  }
820  catch (const std::exception& e)
821  {
823  "Ignoring the following error loading ground truth file: "
824  << mrpt::exception_to_str(e));
825  }
826 
827  if (GT.rows() > 0 && !LMs.empty())
828  {
829  // Each row has:
830  // [0] [1] [2] [6]
831  // x y z ID
832  CVectorDouble ERRS(0);
833  for (size_t i = 0; i < LMs.size(); i++)
834  {
835  // Find the entry in the GT for this mapped LM:
836  bool found = false;
837  for (int r = 0; r < GT.rows(); r++)
838  {
839  if (std::abs(LM_IDs[i] - GT(r, 6)) < 1e-9)
840  {
841  const CPoint3D gtPt(GT(r, 0), GT(r, 1), GT(r, 2));
842  // All these conversions are to make it work with either
843  // CPoint3D & TPoint2D:
844  ERRS.push_back(
845  gtPt.distanceTo(CPoint3D(TPoint3D(LMs[i]))));
846  found = true;
847  break;
848  }
849  }
850  if (!found)
851  {
853  "Ground truth entry not found for landmark ID:"
854  << LM_IDs[i]);
855  }
856  }
857 
858  loc_error_wrt_gt = math::mean(ERRS);
859 
860  MRPT_LOG_INFO("ERRORS VS. GROUND TRUTH:");
861  MRPT_LOG_INFO_FMT("Mean Error: %f meters\n", loc_error_wrt_gt);
863  "Minimum error: %f meters\n", math::minimum(ERRS));
865  "Maximum error: %f meters\n", math::maximum(ERRS));
866  }
867  } // end if GT
868 
869  MRPT_LOG_INFO("********* KF-SLAM finished! **********");
870 
871  if (win3d)
872  {
873  MRPT_LOG_WARN("Close the 3D window to quit the application.");
874  win3d->waitForKey();
875  }
876 
877  MRPT_END
878 }
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:759
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:213
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:370
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:592
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:844
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
#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:29
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...
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:13
bool kbhit() noexcept
An OS-independent version of kbhit, which returns true if a key has been pushed.
Definition: os.cpp:392
std::string file_get_contents(const std::string &fileName)
Loads an entire text file and return its contents as a single std::string.
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:35
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:148
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: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020