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);
55  std::vector<std::string> cfgLines;
56  mrpt::io::loadTextFile(cfgLines, configFile);
57  params.setContent(cfgLines);
58 
59  // Rawlog file: from args. line or from config file:
60  if (argc == 3)
62  else
63  rawlogFileName = params.read_string(
64  "MappingApplication", "rawlog_file", std::string("log.rawlog"));
65 
66  MRPT_END
67 }
68 
70 {
72 
73  // 2D or 3D implementation:
74  const auto kf_implementation = mrpt::system::trim(params.read_string(
75  "MappingApplication", "kf_implementation", "CRangeBearingKFSLAM"));
76 
77  if (kf_implementation == "CRangeBearingKFSLAM")
78  Run_KF_SLAM<mrpt::slam::CRangeBearingKFSLAM>();
79  else if (kf_implementation == "CRangeBearingKFSLAM2D")
80  Run_KF_SLAM<mrpt::slam::CRangeBearingKFSLAM2D>();
81  else
82  throw std::runtime_error(
83  "kf_implementation: Invalid value found in the config file.");
84 
85  MRPT_END
86 }
87 
88 using namespace mrpt;
89 using namespace mrpt::slam;
90 using namespace mrpt::maps;
91 using namespace mrpt::opengl;
92 using namespace mrpt::system;
93 using namespace mrpt::math;
94 using namespace mrpt::poses;
95 using namespace mrpt::config;
96 using namespace mrpt::io;
97 using namespace mrpt::img;
98 using namespace mrpt::obs;
99 using namespace std;
100 
101 // ------------------------------------------------------
102 // traits
103 // ------------------------------------------------------
104 template <class IMPL>
106 
107 // Specialization for 2D (3D) SLAM:
108 template <>
110 {
113  using pose_t = CPose2D;
114  using lm_t = TPoint2D;
115  template <class ARR>
116  static void landmark_to_3d(const ARR& lm, TPoint3D& p)
117  {
118  p.x = lm[0];
119  p.y = lm[1];
120  p.z = 0;
121  }
122 
124  [[maybe_unused]] ekfslam_t& mapping,
125  [[maybe_unused]] CMatrixDouble& fullCov,
126  [[maybe_unused]] const string& OUT_DIR)
127  {
128  // Nothing to do
129  }
130 };
131 
132 // Specialization for 3D (6D) SLAM:
133 template <>
135 {
138  using pose_t = CPose3D;
139  using lm_t = CPoint3D;
140 
141  template <class ARR>
142  static void landmark_to_3d(const ARR& lm, TPoint3D& p)
143  {
144  p.x = lm[0];
145  p.y = lm[1];
146  p.z = lm[2];
147  }
148 
150  ekfslam_t& mapping, CMatrixDouble& fullCov, const string& OUT_DIR)
151  {
152  // Compute the "information" between partitions:
153  if (mapping.options.doPartitioningExperiment)
154  {
155  // --------------------------------------------
156  // PART I:
157  // Comparison to fixed partitioning every K obs.
158  // --------------------------------------------
159 
160  // Compute the information matrix:
161  for (size_t i = 0; i < 6; i++)
162  fullCov(i, i) = max(fullCov(i, i), 1e-6);
163 
164  CMatrixF H(fullCov.inverse_LLt());
165  H.saveToTextFile(OUT_DIR + string("/information_matrix_final.txt"));
166 
167  // Replace by absolute values:
168  H = H.array().abs().matrix();
169  CMatrixF H2(H);
170  CImage imgF;
171  imgF.setFromMatrix(H2, false /*it's not normalized*/);
172  imgF.saveToFile(OUT_DIR + string("/information_matrix_final.png"));
173 
174  // ----------------------------------------
175  // Compute the "approximation error factor" E:
176  // E = SUM() / SUM(ALL ELEMENTS IN MATRIX)
177  // ----------------------------------------
178  vector<std::vector<uint32_t>> landmarksMembership, partsInObsSpace;
179  mrpt::math::CMatrixDouble ERRS(50, 3);
180 
181  for (int i = 0; i < ERRS.rows(); i++)
182  {
183  int K;
184 
185  if (i == 0)
186  {
187  K = 0;
188  mapping.getLastPartitionLandmarks(landmarksMembership);
189  }
190  else
191  {
192  K = i + 1;
193  mapping.getLastPartitionLandmarksAsIfFixedSubmaps(
194  i + 1, landmarksMembership);
195  }
196 
197  mapping.getLastPartition(partsInObsSpace);
198 
199  ERRS(i, 0) = K;
200  ERRS(i, 1) = partsInObsSpace.size();
201  ERRS(i, 2) = mapping.computeOffDiagonalBlocksApproximationError(
202  landmarksMembership);
203  }
204 
205  ERRS.saveToTextFile(OUT_DIR + string("/ERRORS.txt"));
206 
207  // --------------------------------------------
208  // PART II:
209  // Sweep partitioning threshold:
210  // --------------------------------------------
211  size_t STEPS = 50;
212  CVectorFloat ERRS_SWEEP(STEPS), ERRS_SWEEP_THRESHOLD(STEPS);
213 
214  // Compute the error for each partitioning-threshold
215  for (size_t i = 0; i < STEPS; i++)
216  {
217  float th = (1.0f * i) / (STEPS - 1.0f);
218  ERRS_SWEEP_THRESHOLD[i] = th;
219  mapping.mapPartitionOptions()->partitionThreshold = th;
220 
221  mapping.reconsiderPartitionsNow();
222 
223  mapping.getLastPartitionLandmarks(landmarksMembership);
224  ERRS_SWEEP[i] =
225  mapping.computeOffDiagonalBlocksApproximationError(
226  landmarksMembership);
227  }
228 
229  ERRS_SWEEP.saveToTextFile(OUT_DIR + string("/ERRORS_SWEEP.txt"));
230  ERRS_SWEEP_THRESHOLD.saveToTextFile(
231  OUT_DIR + string("/ERRORS_SWEEP_THRESHOLD.txt"));
232 
233  } // end if doPartitioningExperiment
234  }
235 };
236 
237 template <class IMPL>
239 {
240  MRPT_START
241 
242  auto& cfgFile = params; // for backwards compat of old code
243 
244  // The EKF-SLAM class:
245  // Traits for this KF implementation (2D or 3D)
246  using traits_t = kfslam_traits<IMPL>;
247  using ekfslam_t = typename traits_t::ekfslam_t;
248 
249  ekfslam_t mapping;
250 
251  // The rawlog file:
252  // ----------------------------------------
253  const unsigned int rawlog_offset =
254  cfgFile.read_int("MappingApplication", "rawlog_offset", 0);
255 
256  const unsigned int SAVE_LOG_FREQUENCY =
257  cfgFile.read_int("MappingApplication", "SAVE_LOG_FREQUENCY", 1);
258 
259  const bool SAVE_DA_LOG =
260  cfgFile.read_bool("MappingApplication", "SAVE_DA_LOG", true);
261 
262  const bool SAVE_3D_SCENES =
263  cfgFile.read_bool("MappingApplication", "SAVE_3D_SCENES", true);
264  const bool SAVE_MAP_REPRESENTATIONS = cfgFile.read_bool(
265  "MappingApplication", "SAVE_MAP_REPRESENTATIONS", true);
266  bool SHOW_3D_LIVE =
267  cfgFile.read_bool("MappingApplication", "SHOW_3D_LIVE", false);
268  const bool CAMERA_3DSCENE_FOLLOWS_ROBOT = cfgFile.read_bool(
269  "MappingApplication", "CAMERA_3DSCENE_FOLLOWS_ROBOT", false);
270 
271 #if !MRPT_HAS_WXWIDGETS
272  SHOW_3D_LIVE = false;
273 #endif
274 
275  string OUT_DIR = cfgFile.read_string(
276  "MappingApplication", "logOutput_dir", "OUT_KF-SLAM");
277  string ground_truth_file =
278  cfgFile.read_string("MappingApplication", "ground_truth_file", "");
279  string ground_truth_file_robot = cfgFile.read_string(
280  "MappingApplication", "ground_truth_file_robot", "");
281 
282  string ground_truth_data_association = cfgFile.read_string(
283  "MappingApplication", "ground_truth_data_association", "");
284 
285  MRPT_LOG_INFO_STREAM("RAWLOG FILE: " << rawlogFileName);
286  ASSERT_FILE_EXISTS_(rawlogFileName);
287  CFileGZInputStream rawlogFile(rawlogFileName);
288 
289  deleteFilesInDirectory(OUT_DIR);
290  createDirectory(OUT_DIR);
291 
292  // Load the config options for mapping:
293  // ----------------------------------------
294  mapping.loadOptions(cfgFile);
295  {
296  std::stringstream o;
297  mapping.KF_options.dumpToTextStream(o);
298  mapping.options.dumpToTextStream(o);
299  MRPT_LOG_INFO(o.str());
300  }
301 
302  // debug:
303  // mapping.KF_options.use_analytic_observation_jacobian = true;
304  // mapping.KF_options.use_analytic_transition_jacobian = true;
305  // mapping.KF_options.debug_verify_analytic_jacobians = true;
306 
307  // Is there ground truth of the robot poses??
308  CMatrixDouble GT_PATH(0, 0);
309  if (ground_truth_file_robot.size() && fileExists(ground_truth_file_robot))
310  {
311  GT_PATH.loadFromTextFile(ground_truth_file_robot);
312  ASSERT_(GT_PATH.rows() > 0 && GT_PATH.cols() == 6);
313  }
314 
315  // Is there a ground truth file of the data association?
316  // Map: timestamp -> vector(index in observation -> real index)
317  std::map<double, std::vector<int>> GT_DA;
318  // Landmark indices bimapping: SLAM DA <---> GROUND TRUTH DA
319  mrpt::containers::bimap<int, int> DA2GTDA_indices;
320  if (!ground_truth_data_association.empty() &&
321  fileExists(ground_truth_data_association))
322  {
323  CMatrixDouble mGT_DA;
324  mGT_DA.loadFromTextFile(ground_truth_data_association);
325  ASSERT_ABOVEEQ_(mGT_DA.cols(), 3);
326  // Convert the loaded matrix into a std::map in GT_DA:
327  for (int i = 0; i < mGT_DA.rows(); i++)
328  {
329  std::vector<int>& v = GT_DA[mGT_DA(i, 0)];
330  if (v.size() <= mGT_DA(i, 1)) v.resize(mGT_DA(i, 1) + 1);
331  v[mGT_DA(i, 1)] = mGT_DA(i, 2);
332  }
334  "Loaded " << GT_DA.size() << " entries from DA ground truth file.");
335  }
336 
337  // Create output file for DA perf:
338  std::ofstream out_da_performance_log;
339  {
340  const std::string f = std::string(
341  OUT_DIR + std::string("/data_association_performance.log"));
342  out_da_performance_log.open(f.c_str());
343  ASSERTMSG_(
344  out_da_performance_log.is_open(),
345  std::string("Error writing to: ") + f);
346 
347  // Header:
348  out_da_performance_log
349  << "% TIMESTAMP INDEX_IN_OBS TruePos "
350  "FalsePos TrueNeg FalseNeg NoGroundTruthSoIDontKnow \n"
351  << "%--------------------------------------------------------------"
352  "--------------------------------------------------\n";
353  }
354 
356 
357  if (SHOW_3D_LIVE)
358  {
359  win3d =
360  mrpt::gui::CDisplayWindow3D::Create("KF-SLAM live view", 800, 500);
361 
362  win3d->addTextMessage(
363  0.01, 0.96, "Red: Estimated path", TColorf(0.8f, 0.8f, 0.8f), 100,
365  win3d->addTextMessage(
366  0.01, 0.93, "Black: Ground truth path", TColorf(0.8f, 0.8f, 0.8f),
368  }
369 
370  // Create DA-log output file:
371  std::ofstream out_da_log;
372  if (SAVE_DA_LOG)
373  {
374  const std::string f =
375  std::string(OUT_DIR + std::string("/data_association.log"));
376  out_da_log.open(f.c_str());
377  ASSERTMSG_(out_da_log.is_open(), std::string("Error writing to: ") + f);
378 
379  // Header:
380  out_da_log << "% TIMESTAMP INDEX_IN_OBS ID "
381  " RANGE(m) YAW(rad) PITCH(rad) \n"
382  << "%-------------------------------------------------------"
383  "-------------------------------------\n";
384  }
385 
386  // The main loop:
387  // ---------------------------------------
388  CActionCollection::Ptr action;
389  CSensoryFrame::Ptr observations;
390  size_t rawlogEntry = 0, step = 0;
391 
392  vector<TPose3D> meanPath; // The estimated path
393  typename traits_t::posepdf_t robotPose;
394  const bool is_pose_3d = robotPose.state_length != 3;
395 
396  std::vector<typename IMPL::landmark_point_t> LMs;
397  std::map<unsigned int, CLandmark::TLandmarkID> LM_IDs;
398  CMatrixDouble fullCov;
399  CVectorDouble fullState;
400  CTicTac kftictac;
401 
402  auto rawlogArch = mrpt::serialization::archiveFrom(rawlogFile);
403 
404  for (;;)
405  {
406  if (os::kbhit())
407  {
408  char pushKey = os::getch();
409  if (27 == pushKey) break;
410  }
411 
412  // Load action/observation pair from the rawlog:
413  // --------------------------------------------------
414  if (!CRawlog::readActionObservationPair(
415  rawlogArch, action, observations, rawlogEntry))
416  break; // file EOF
417 
418  if (rawlogEntry >= rawlog_offset)
419  {
420  // Process the action and observations:
421  // --------------------------------------------
422  kftictac.Tic();
423 
424  mapping.processActionObservation(action, observations);
425 
426  const double tim_kf_iter = kftictac.Tac();
427 
428  // Get current state:
429  // -------------------------------
430  mapping.getCurrentState(robotPose, LMs, LM_IDs, fullState, fullCov);
431  MRPT_LOG_INFO_STREAM("Mean pose: " << robotPose.mean);
432  MRPT_LOG_INFO_STREAM("# of landmarks in the map: " << LMs.size());
433 
434  // Get the mean robot pose as 3D:
435  const CPose3D robotPoseMean3D = CPose3D(robotPose.mean);
436 
437  // Build the path:
438  meanPath.push_back(robotPoseMean3D.asTPose());
439 
440  // Save mean pose:
441  if (!(step % SAVE_LOG_FREQUENCY))
442  {
443  const auto p = robotPose.mean.asVectorVal();
444  p.saveToTextFile(
445  OUT_DIR +
446  format("/robot_pose_%05u.txt", (unsigned int)step));
447  }
448 
449  // Save full cov:
450  if (!(step % SAVE_LOG_FREQUENCY))
451  {
452  fullCov.saveToTextFile(
453  OUT_DIR + format("/full_cov_%05u.txt", (unsigned int)step));
454  }
455 
456  // Generate Data Association log?
457  if (SAVE_DA_LOG)
458  {
459  const typename ekfslam_t::TDataAssocInfo& da =
460  mapping.getLastDataAssociation();
461 
463  observations
464  ->getObservationByClass<CObservationBearingRange>();
465  if (obs)
466  {
467  const CObservationBearingRange* obsRB = obs.get();
468  const double tim =
470 
471  for (size_t i = 0; i < obsRB->sensedData.size(); i++)
472  {
473  auto it = da.results.associations.find(i);
474  int assoc_ID_in_SLAM;
475  if (it != da.results.associations.end())
476  assoc_ID_in_SLAM = it->second;
477  else
478  {
479  // It should be a newly created LM:
480  auto itNewLM = da.newly_inserted_landmarks.find(i);
481  if (itNewLM != da.newly_inserted_landmarks.end())
482  assoc_ID_in_SLAM = itNewLM->second;
483  else
484  assoc_ID_in_SLAM = -1;
485  }
486 
487  out_da_log << format(
488  "%35.22f %8i %10i %10f %12f %12f\n", tim, (int)i,
489  assoc_ID_in_SLAM,
490  (double)obsRB->sensedData[i].range,
491  (double)obsRB->sensedData[i].yaw,
492  (double)obsRB->sensedData[i].pitch);
493  }
494  }
495  }
496 
497  // Save report on DA performance:
498  {
499  const typename ekfslam_t::TDataAssocInfo& da =
500  mapping.getLastDataAssociation();
501 
503  observations
504  ->getObservationByClass<CObservationBearingRange>();
505  if (obs)
506  {
507  const CObservationBearingRange* obsRB = obs.get();
508  const double tim =
510 
511  auto itDA = GT_DA.find(tim);
512 
513  for (size_t i = 0; i < obsRB->sensedData.size(); i++)
514  {
515  bool is_FP = false, is_TP = false, is_FN = false,
516  is_TN = false;
517 
518  if (itDA != GT_DA.end())
519  {
520  const std::vector<int>& vDA = itDA->second;
521  ASSERT_BELOW_(i, vDA.size());
522  const int GT_ASSOC = vDA[i];
523 
524  auto it = da.results.associations.find(i);
525  if (it != da.results.associations.end())
526  {
527  // This observation was assigned the already
528  // existing LM in the map: "it->second"
529  // TruePos -> If that LM index corresponds to
530  // that in the GT (with index mapping):
531 
532  // mrpt::containers::bimap<int,int>
533  // DA2GTDA_indices;
534  // // Landmark indices bimapping: SLAM DA <--->
535  // GROUND TRUTH DA
536  if (DA2GTDA_indices.hasKey(it->second))
537  {
538  const int slam_asigned_LM_idx =
539  DA2GTDA_indices.direct(it->second);
540  if (slam_asigned_LM_idx == GT_ASSOC)
541  is_TP = true;
542  else
543  is_FP = true;
544  }
545  else
546  {
547  // Is this case possible? Assigned to an
548  // index not ever seen for the first time
549  // with a GT....
550  // Just in case:
551  is_FP = true;
552  }
553  }
554  else
555  {
556  // No pairing, but should be a newly created LM:
557  auto itNewLM =
558  da.newly_inserted_landmarks.find(i);
559  if (itNewLM !=
560  da.newly_inserted_landmarks.end())
561  {
562  const int new_LM_in_SLAM = itNewLM->second;
563 
564  // Was this really a NEW LM not observed
565  // before?
566  if (DA2GTDA_indices.hasValue(GT_ASSOC))
567  {
568  // GT says this LM was already observed,
569  // so it shouldn't appear here as new:
570  is_FN = true;
571  }
572  else
573  {
574  // Really observed for the first time:
575  is_TN = true;
576  DA2GTDA_indices.insert(
577  new_LM_in_SLAM, GT_ASSOC);
578  }
579  }
580  else
581  {
582  // Not associated neither inserted:
583  // Shouldn't really never arrive here.
584  }
585  }
586  }
587 
588  // "% TIMESTAMP INDEX_IN_OBS
589  // TruePos FalsePos TrueNeg FalseNeg
590  // NoGroundTruthSoIDontKnow \n"
591  out_da_performance_log << format(
592  "%35.22f %13i %8i %8i %8i %8i %8i\n", tim, (int)i,
593  (int)(is_TP ? 1 : 0), (int)(is_FP ? 1 : 0),
594  (int)(is_TN ? 1 : 0), (int)(is_FN ? 1 : 0),
595  (int)(!is_FP && !is_TP && !is_FN && !is_TN ? 1 : 0));
596  }
597  }
598  }
599 
600  // Save map to file representations?
601  if (SAVE_MAP_REPRESENTATIONS && !(step % SAVE_LOG_FREQUENCY))
602  {
603  mapping.saveMapAndPath2DRepresentationAsMATLABFile(
604  OUT_DIR + format("/slam_state_%05u.m", (unsigned int)step));
605  }
606 
607  // Save 3D view of the filter state:
608  if (win3d || (SAVE_3D_SCENES && !(step % SAVE_LOG_FREQUENCY)))
609  {
610  COpenGLScene::Ptr scene3D = std::make_shared<COpenGLScene>();
611  {
613  std::make_shared<opengl::CGridPlaneXY>(
614  -1000, 1000, -1000, 1000, 0, 5);
615  grid->setColor(0.4, 0.4, 0.4);
616  scene3D->insert(grid);
617  }
618 
619  // Robot path:
620  {
621  opengl::CSetOfLines::Ptr linesPath =
622  std::make_shared<opengl::CSetOfLines>();
623  linesPath->setColor(1, 0, 0);
624 
625  TPose3D init_pose;
626  if (!meanPath.empty())
627  init_pose = CPose3D(meanPath[0]).asTPose();
628 
629  int path_decim = 0;
630  for (auto& it : meanPath)
631  {
632  linesPath->appendLine(init_pose, it);
633  init_pose = it;
634 
635  if (++path_decim > 10)
636  {
637  path_decim = 0;
640  0.3f, 2.0f);
641  xyz->setPose(CPose3D(it));
642  scene3D->insert(xyz);
643  }
644  }
645  scene3D->insert(linesPath);
646 
647  // finally a big corner for the latest robot pose:
648  {
651  1.0, 2.5);
652  xyz->setPose(robotPoseMean3D);
653  scene3D->insert(xyz);
654  }
655 
656  // The camera pointing to the current robot pose:
657  if (win3d && CAMERA_3DSCENE_FOLLOWS_ROBOT)
658  {
659  win3d->setCameraPointingToPoint(
660  robotPoseMean3D.x(), robotPoseMean3D.y(),
661  robotPoseMean3D.z());
662  }
663  }
664 
665  // Do we have a ground truth?
666  if (GT_PATH.cols() == 6 || GT_PATH.cols() == 3)
667  {
668  opengl::CSetOfLines::Ptr GT_path =
669  std::make_shared<opengl::CSetOfLines>();
670  GT_path->setColor(0, 0, 0);
671  size_t N =
672  std::min((int)GT_PATH.rows(), (int)meanPath.size());
673 
674  if (GT_PATH.cols() == 6)
675  {
676  double gtx0 = 0, gty0 = 0, gtz0 = 0;
677  for (size_t i = 0; i < N; i++)
678  {
679  const CPose3D p(
680  GT_PATH(i, 0), GT_PATH(i, 1), GT_PATH(i, 2),
681  GT_PATH(i, 3), GT_PATH(i, 4), GT_PATH(i, 5));
682 
683  GT_path->appendLine(
684  gtx0, gty0, gtz0, p.x(), p.y(), p.z());
685  gtx0 = p.x();
686  gty0 = p.y();
687  gtz0 = p.z();
688  }
689  }
690  else if (GT_PATH.cols() == 3)
691  {
692  double gtx0 = 0, gty0 = 0;
693  for (size_t i = 0; i < N; i++)
694  {
695  const CPose2D p(
696  GT_PATH(i, 0), GT_PATH(i, 1), GT_PATH(i, 2));
697 
698  GT_path->appendLine(gtx0, gty0, 0, p.x(), p.y(), 0);
699  gtx0 = p.x();
700  gty0 = p.y();
701  }
702  }
703  scene3D->insert(GT_path);
704  }
705 
706  // Draw latest data association:
707  {
708  const typename ekfslam_t::TDataAssocInfo& da =
709  mapping.getLastDataAssociation();
710 
713  lins->setLineWidth(1.2f);
714  lins->setColor(1, 1, 1);
715  for (auto it = da.results.associations.begin();
716  it != da.results.associations.end(); ++it)
717  {
718  const prediction_index_t idxPred = it->second;
719  // This index must match the internal list of features
720  // in the map:
721  typename ekfslam_t::KFArray_FEAT featMean;
722  mapping.getLandmarkMean(idxPred, featMean);
723 
724  TPoint3D featMean3D;
725  traits_t::landmark_to_3d(featMean, featMean3D);
726 
727  // Line: robot -> landmark:
728  lins->appendLine(
729  robotPoseMean3D.x(), robotPoseMean3D.y(),
730  robotPoseMean3D.z(), featMean3D.x, featMean3D.y,
731  featMean3D.z);
732  }
733  scene3D->insert(lins);
734  }
735 
736  // The current state of KF-SLAM:
737  {
739  std::make_shared<opengl::CSetOfObjects>();
740  mapping.getAs3DObject(objs);
741  scene3D->insert(objs);
742  }
743 
744  if (win3d)
745  {
747  win3d->get3DSceneAndLock();
748  scn = scene3D;
749 
750  // Update text messages:
751  win3d->addTextMessage(
752  0.02, 0.02,
753  format(
754  "Step %u - Landmarks in the map: %u",
755  (unsigned int)step, (unsigned int)LMs.size()),
757 
758  win3d->addTextMessage(
759  0.02, 0.06,
760  format(
761  is_pose_3d
762  ? "Estimated pose: (x y z qr qx qy qz) = %s"
763  : "Estimated pose: (x y yaw) = %s",
764  robotPose.mean.asString().c_str()),
766 
767  static vector<double> estHz_vals;
768  const double curHz = 1.0 / std::max(1e-9, tim_kf_iter);
769  estHz_vals.push_back(curHz);
770  if (estHz_vals.size() > 50)
771  estHz_vals.erase(estHz_vals.begin());
772  const double meanHz = mrpt::math::mean(estHz_vals);
773 
774  win3d->addTextMessage(
775  0.02, 0.10,
776  format(
777  "Iteration time: %7ss",
778  mrpt::system::unitsFormat(tim_kf_iter).c_str()),
780 
781  win3d->addTextMessage(
782  0.02, 0.14,
783  format(
784  "Execution rate: %7sHz",
785  mrpt::system::unitsFormat(meanHz).c_str()),
787 
788  win3d->unlockAccess3DScene();
789  win3d->repaint();
790  }
791 
792  if (SAVE_3D_SCENES && !(step % SAVE_LOG_FREQUENCY))
793  {
794  // Save to file:
796  OUT_DIR +
797  format("/kf_state_%05u.3Dscene", (unsigned int)step));
798  mrpt::serialization::archiveFrom(f) << *scene3D;
799  }
800  }
801 
802  // Free rawlog items memory:
803  // --------------------------------------------
804  action.reset();
805  observations.reset();
806 
807  } // (rawlogEntry>=rawlog_offset)
808 
810  "Step " << step << " - Rawlog entries processed: " << rawlogEntry);
811 
812  step++;
813  }; // end "while(1)"
814 
815  // Partitioning experiment: Only for 6D SLAM:
816  traits_t::doPartitioningExperiment(mapping, fullCov, OUT_DIR);
817 
818  // Is there ground truth of landmarks positions??
819  if (ground_truth_file.size() && fileExists(ground_truth_file))
820  {
821  CMatrixFloat GT(0, 0);
822  try
823  {
824  GT.loadFromTextFile(ground_truth_file);
825  }
826  catch (const std::exception& e)
827  {
829  "Ignoring the following error loading ground truth file: "
830  << mrpt::exception_to_str(e));
831  }
832 
833  if (GT.rows() > 0 && !LMs.empty())
834  {
835  // Each row has:
836  // [0] [1] [2] [6]
837  // x y z ID
838  CVectorDouble ERRS(0);
839  for (size_t i = 0; i < LMs.size(); i++)
840  {
841  // Find the entry in the GT for this mapped LM:
842  bool found = false;
843  for (int r = 0; r < GT.rows(); r++)
844  {
845  if (std::abs(LM_IDs[i] - GT(r, 6)) < 1e-9)
846  {
847  const CPoint3D gtPt(GT(r, 0), GT(r, 1), GT(r, 2));
848  // All these conversions are to make it work with either
849  // CPoint3D & TPoint2D:
850  ERRS.push_back(
851  gtPt.distanceTo(CPoint3D(TPoint3D(LMs[i]))));
852  found = true;
853  break;
854  }
855  }
856  if (!found)
857  {
859  "Ground truth entry not found for landmark ID:"
860  << LM_IDs[i]);
861  }
862  }
863 
864  loc_error_wrt_gt = math::mean(ERRS);
865 
866  MRPT_LOG_INFO("ERRORS VS. GROUND TRUTH:");
867  MRPT_LOG_INFO_FMT("Mean Error: %f meters\n", loc_error_wrt_gt);
869  "Minimum error: %f meters\n", math::minimum(ERRS));
871  "Maximum error: %f meters\n", math::maximum(ERRS));
872  }
873  } // end if GT
874 
875  MRPT_LOG_INFO("********* KF-SLAM finished! **********");
876 
877  if (win3d)
878  {
879  MRPT_LOG_WARN("Close the 3D window to quit the application.");
880  win3d->waitForKey();
881  }
882 
883  MRPT_END
884 }
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
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
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
#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
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...
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
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void run()
Runs with the current parameter set.
Definition: KFSLAMApp.cpp:69
bool loadTextFile(std::vector< std::string > &o, const std::string &fileName)
Loads a text file as a vector of string lines.
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
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
GLsizei const GLchar ** string
Definition: glext.h:4116
size_type rows() const
Number of rows in the matrix.
A class used to store a 3D point.
Definition: CPoint3D.h:31
GLenum GLenum GLenum GLenum mapping
Definition: glext.h:6601
size_type cols() const
Number of columns in the matrix.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
CONTAINER::Scalar minimum(const CONTAINER &v)
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
const GLdouble * v
Definition: glext.h:3684
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:123
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
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
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...
#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
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.
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:337
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
Lightweight 3D point.
Definition: TPoint3D.h:90
#define ASSERT_FILE_EXISTS_(FIL)
Definition: filesystem.h:22
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
Lightweight 2D point.
Definition: TPoint2D.h:31
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:149
GLfloat GLfloat p
Definition: glext.h:6398
GLenum const GLfloat * params
Definition: glext.h:3538
static void landmark_to_3d(const ARR &lm, TPoint3D &p)
Definition: KFSLAMApp.cpp:142
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
static Ptr Create(Args &&... args)
Definition: CSetOfLines.h:33
static void landmark_to_3d(const ARR &lm, TPoint3D &p)
Definition: KFSLAMApp.cpp:116
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: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019