MRPT  2.0.4
CGraphSlamEngine_impl.h
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 #pragma once
11 
16 #include <mrpt/opengl/CAxis.h>
18 #include <mrpt/system/filesystem.h>
19 
20 namespace mrpt::graphslam
21 {
22 template <class GRAPH_T>
23 const std::string CGraphSlamEngine<GRAPH_T>::header_sep = std::string(80, '-');
24 template <class GRAPH_T>
25 const std::string CGraphSlamEngine<GRAPH_T>::report_sep = std::string(2, '\n');
26 
27 template <class GRAPH_T>
29  const std::string& config_file, const std::string& rawlog_fname,
30  const std::string& fname_GT, mrpt::graphslam::CWindowManager* win_manager,
34  : m_node_reg(node_reg),
35  m_edge_reg(edge_reg),
36  m_optimizer(optimizer),
37  m_enable_visuals(win_manager != nullptr),
38  m_config_fname(config_file),
39  m_rawlog_fname(rawlog_fname),
40  m_fname_GT(fname_GT),
41  m_GT_poses_step(1),
42  m_win_manager(win_manager),
43  m_paused_message("Program is paused. Press \"p/P\" to resume."),
44  m_text_index_paused_message(345), // just a large number.
45  m_odometry_color(0, 0, 255),
46  m_GT_color(0, 255, 0),
47  m_estimated_traj_color(255, 165, 0),
48  m_optimized_map_color(255, 0, 0),
49  m_current_constraint_type_color(255, 255, 255),
50  m_robot_model_size(1),
51  m_class_name("CGraphSlamEngine"),
52  m_is_first_time_node_reg(true)
53 {
54  this->initClass();
55 }
56 
57 template <class GRAPH_T>
59 {
60  // change back the CImage path
61  if (mrpt::system::strCmpI(m_GT_file_format, "rgbd_tum"))
62  {
63  MRPT_LOG_DEBUG_STREAM("Changing back the CImage PATH");
64  mrpt::img::CImage::setImagesPathBase(m_img_prev_path_base);
65  }
66 }
67 
68 template <class GRAPH_T>
69 typename GRAPH_T::global_pose_t
71 {
72  std::lock_guard<std::mutex> graph_lock(m_graph_section);
73  return m_node_reg->getCurrentRobotPosEstimation();
74 }
75 
76 template <class GRAPH_T>
77 typename GRAPH_T::global_poses_t
79 {
80  std::lock_guard<std::mutex> graph_lock(m_graph_section);
81  return m_graph.nodes;
82 }
83 
84 template <class GRAPH_T>
86  std::set<mrpt::graphs::TNodeID>* nodes_set) const
87 {
88  ASSERTDEB_(nodes_set);
89  m_graph.getAllNodes(*nodes_set);
90 }
91 
92 template <class GRAPH_T>
94 {
96  using namespace mrpt;
97  using namespace mrpt::opengl;
98  using namespace std;
99 
100  // logger instance properties
101  m_time_logger.setName(m_class_name);
102  this->logging_enable_keep_record = true;
103  this->setLoggerName(m_class_name);
104 
105  // Assert that the deciders/optimizer pointers are valid
106  ASSERTDEB_(m_node_reg);
107  ASSERTDEB_(m_edge_reg);
108  ASSERTDEB_(m_optimizer);
109 
110  // Assert that the graph class used is supported.
111  {
112  MRPT_LOG_INFO_STREAM("Verifying support for given MRPT graph class...");
113 
114  // TODO - initialize vector in a smarter way.
115  m_supported_constraint_types.push_back("CPosePDFGaussianInf");
116  m_supported_constraint_types.push_back("CPose3DPDFGaussianInf");
117 
118  constraint_t c;
119  const string c_str(c.GetRuntimeClass()->className);
120 
121  bool found =
122  (std::find(
123  m_supported_constraint_types.begin(),
124  m_supported_constraint_types.end(),
125  c_str) != m_supported_constraint_types.end());
126 
127  if (found)
128  {
129  MRPT_LOG_INFO_STREAM("[OK] Class: " << c_str);
130  }
131  else
132  {
134  "Given graph class " << c_str
135  << " has not been tested consistently yet."
136  << "Proceed at your own risk.");
138  }
139 
140  // store it in a string for future use.
141  m_current_constraint_type = c_str;
142  m_current_constraint_type = "Constraints: " + m_current_constraint_type;
143  }
144 
145  // If a valid CWindowManager pointer is given then visuals are on.
146  if (m_enable_visuals)
147  {
148  m_win = m_win_manager->win;
149  m_win_observer = m_win_manager->observer;
150  }
151  else
152  {
153  m_win = nullptr;
154  m_win_observer = nullptr;
155 
156  MRPT_LOG_WARN_STREAM("Visualization is off. Running on headless mode");
157  }
158 
159  // set the CDisplayWindowPlots pointer to null for starters, we don't know
160  // if
161  // we are using it
162 
163  m_observation_only_dataset = false;
164  m_request_to_exit = false;
165 
166  // max node number already in the graph
167  m_nodeID_max = INVALID_NODEID;
168 
169  m_is_paused = false;
170  m_GT_poses_index = 0;
171 
172  // pass the necessary variables/objects to the deciders/optimizes
173  // pass a graph ptr after the instance initialization
174  m_node_reg->setGraphPtr(&m_graph);
175  m_edge_reg->setGraphPtr(&m_graph);
176  m_optimizer->setGraphPtr(&m_graph);
177 
178  // pass the window manager pointer
179  // note: m_win_manager contains a pointer to the CDisplayWindow3D instance
180  if (m_enable_visuals)
181  {
182  m_node_reg->setWindowManagerPtr(m_win_manager);
183  m_edge_reg->setWindowManagerPtr(m_win_manager);
184  m_optimizer->setWindowManagerPtr(m_win_manager);
185  m_edge_counter.setWindowManagerPtr(m_win_manager);
186  }
187 
188  // pass a lock in case of multithreaded implementation
189  m_node_reg->setCriticalSectionPtr(&m_graph_section);
190  m_edge_reg->setCriticalSectionPtr(&m_graph_section);
191  m_optimizer->setCriticalSectionPtr(&m_graph_section);
192 
193  // Load the parameters that each one of the self/deciders/optimizer classes
194  // needs
195  this->loadParams(m_config_fname);
196 
197  if (!m_enable_visuals)
198  {
199  MRPT_LOG_WARN_STREAM("Switching all visualization parameters off...");
200  m_visualize_odometry_poses = 0;
201  m_visualize_GT = 0;
202  m_visualize_map = 0;
203  m_visualize_estimated_trajectory = 0;
204  m_visualize_SLAM_metric = 0;
205  m_enable_curr_pos_viewport = 0;
206  m_enable_range_viewport = 0;
207  m_enable_intensity_viewport = 0;
208  }
209 
210  m_use_GT = !m_fname_GT.empty();
211  if (m_use_GT)
212  {
213  if (mrpt::system::strCmpI(m_GT_file_format, "rgbd_tum"))
214  {
215  THROW_EXCEPTION("Not Implemented Yet.");
216  this->alignOpticalWithMRPTFrame();
217  // this->readGTFileRGBD_TUM(m_fname_GT, &m_GT_poses);
218  }
219  else if (mrpt::system::strCmpI(m_GT_file_format, "navsimul"))
220  {
221  this->readGTFile(m_fname_GT, &m_GT_poses);
222  }
223  }
224 
225  // plot the GT related visuals only if ground-truth file is given
226  if (!m_use_GT)
227  {
229  "Ground truth file was not provided. Switching the related "
230  "visualization parameters off...");
231  m_visualize_GT = 0;
232  m_visualize_SLAM_metric = 0;
233  }
234 
235  // timestamp
236  if (m_enable_visuals)
237  {
238  m_win_manager->assignTextMessageParameters(
239  &m_offset_y_timestamp, &m_text_index_timestamp);
240  }
241 
242  if (m_visualize_map)
243  {
244  this->initMapVisualization();
245  }
246 
247  // Configuration of various trajectories visualization
248  ASSERTDEB_(m_has_read_config);
249  if (m_enable_visuals)
250  {
251  // odometry visualization
252  if (m_visualize_odometry_poses)
253  {
254  this->initOdometryVisualization();
255  }
256  // GT Visualization
257  if (m_visualize_GT)
258  {
259  this->initGTVisualization();
260  }
261  // estimated trajectory visualization
262  this->initEstimatedTrajectoryVisualization();
263  // current robot pose viewport
264  if (m_enable_curr_pos_viewport)
265  {
266  this->initCurrPosViewport();
267  }
268  }
269 
270  // change the CImage path in case of RGBD datasets
271  if (mrpt::system::strCmpI(m_GT_file_format, "rgbd_tum"))
272  {
273  // keep the last path - change back to it after rawlog parsing
274  m_img_prev_path_base = mrpt::img::CImage::getImagesPathBase();
275 
276  std::string rawlog_fname_noext =
277  system::extractFileName(m_rawlog_fname);
278  std::string rawlog_dir = system::extractFileDirectory(m_rawlog_fname);
279  m_img_external_storage_dir =
280  rawlog_dir + rawlog_fname_noext + "_Images/";
281  mrpt::img::CImage::setImagesPathBase(m_img_external_storage_dir);
282  }
283 
284  // 3DRangeScans viewports initialization, in case of RGBD datasets
285  if (mrpt::system::strCmpI(m_GT_file_format, "rgbd_tum"))
286  {
287  if (m_enable_range_viewport)
288  {
289  this->initRangeImageViewport();
290  }
291  if (m_enable_intensity_viewport)
292  {
293  this->initIntensityImageViewport();
294  }
295  }
296  // axis
297  if (m_enable_visuals)
298  {
299  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
300 
301  CAxis::Ptr obj = std::make_shared<CAxis>();
302  obj->setFrequency(5);
303  obj->enableTickMarks();
304  obj->setAxisLimits(-10, -10, -10, 10, 10, 10);
305  obj->setName("axis");
306  scene->insert(obj);
307 
308  m_win->unlockAccess3DScene();
309  m_win->forceRepaint();
310  }
311 
312  // Add additional keystrokes in the CDisplayWindow3D help textBox
313  if (m_enable_visuals)
314  {
315  // keystrokes initialization
316  m_keystroke_pause_exec = "p";
317  m_keystroke_odometry = "o";
318  m_keystroke_GT = "g";
319  m_keystroke_estimated_trajectory = "t";
320  m_keystroke_map = "m";
321 
322  // keystrokes registration
323  m_win_observer->registerKeystroke(
324  m_keystroke_pause_exec, "Pause/Resume program execution");
325  m_win_observer->registerKeystroke(
326  m_keystroke_odometry, "Toggle Odometry visualization");
327  m_win_observer->registerKeystroke(
328  m_keystroke_GT, "Toggle Ground-Truth visualization");
329  m_win_observer->registerKeystroke(
330  m_keystroke_estimated_trajectory,
331  "Toggle Estimated trajectory visualization");
332  m_win_observer->registerKeystroke(
333  m_keystroke_map, "Toggle Map visualization");
334  }
335 
336  // register the types of edges
337  vector<string> vec_edge_types;
338  vec_edge_types.emplace_back("Odometry");
339  vec_edge_types.emplace_back("ICP2D");
340  vec_edge_types.emplace_back("ICP3D");
341 
342  for (auto cit = vec_edge_types.begin(); cit != vec_edge_types.end(); ++cit)
343  {
344  m_edge_counter.addEdgeType(*cit);
345  }
346 
347  // Visualize the edge statistics
348  if (m_enable_visuals)
349  {
350  // total edges - loop closure edges
351  double offset_y_total_edges, offset_y_loop_closures;
352  int text_index_total_edges, text_index_loop_closures;
353 
354  m_win_manager->assignTextMessageParameters(
355  &offset_y_total_edges, &text_index_total_edges);
356 
357  // build each one of these
358  map<string, double> name_to_offset_y;
359  map<string, int> name_to_text_index;
360  for (auto it = vec_edge_types.begin(); it != vec_edge_types.end(); ++it)
361  {
362  m_win_manager->assignTextMessageParameters(
363  &name_to_offset_y[*it], &name_to_text_index[*it]);
364  }
365 
366  m_win_manager->assignTextMessageParameters(
367  &offset_y_loop_closures, &text_index_loop_closures);
368 
369  // add all the parameters to the CEdgeCounter object
370  m_edge_counter.setTextMessageParams(
371  name_to_offset_y, name_to_text_index, offset_y_total_edges,
372  text_index_total_edges, offset_y_loop_closures,
373  text_index_loop_closures);
374  }
375 
376  // Type of the generated graph
377  if (m_enable_visuals)
378  {
379  m_win_manager->assignTextMessageParameters(
380  &m_offset_y_current_constraint_type,
381  &m_text_index_current_constraint_type);
382  m_win_manager->addTextMessage(
383  m_offset_x_left, -m_offset_y_current_constraint_type,
384  m_current_constraint_type,
385  mrpt::img::TColorf(m_current_constraint_type_color),
386  m_text_index_current_constraint_type);
387  }
388 
389  // query node/edge deciders for visual objects initialization
390  if (m_enable_visuals)
391  {
392  std::lock_guard<std::mutex> graph_lock(m_graph_section);
393  m_time_logger.enter("Visuals");
394  m_node_reg->initializeVisuals();
395  m_edge_reg->initializeVisuals();
396  m_optimizer->initializeVisuals();
397  m_time_logger.leave("Visuals");
398  }
399 
400  m_init_timestamp = INVALID_TIMESTAMP;
401 
402  // COccupancyGridMap2D Initialization
403  {
406 
407  gridmap->setSize(
408  /* min_x = */ -20.0f,
409  /* float max_x = */ 20.0f,
410  /* float min_y = */ -20.0f,
411  /* float max_y = */ 20.0f,
412  /* float resolution = */ 0.05f);
413 
414  // TODO - Read these from the .ini file
415  // observation insertion options
416  gridmap->insertionOptions.maxOccupancyUpdateCertainty = 0.8f;
417  gridmap->insertionOptions.maxDistanceInsertion = 5;
418  gridmap->insertionOptions.wideningBeamsWithDistance = true;
419  gridmap->insertionOptions.decimation = 2;
420 
421  m_gridmap_cached = gridmap;
422  m_map_is_cached = false;
423  }
424 
425  // COctoMap Initialization
426  {
428 
429  // TODO - adjust the insertionoptions...
430  // TODO - Read these from the .ini file
431  octomap->insertionOptions.setOccupancyThres(0.5);
432  octomap->insertionOptions.setProbHit(0.7);
433  octomap->insertionOptions.setProbMiss(0.4);
434  octomap->insertionOptions.setClampingThresMin(0.1192);
435  octomap->insertionOptions.setClampingThresMax(0.971);
436 
437  m_octomap_cached = octomap;
438  m_map_is_cached = false;
439  }
440 
441  // In case we are given an RGBD TUM Dataset - try and read the info file so
442  // that we know how to play back the GT poses.
443  try
444  {
445  m_info_params.setRawlogFile(m_rawlog_fname);
446  m_info_params.parseFile();
447  // set the rate at which we read from the GT poses vector
448  int num_of_objects = std::atoi(
449  m_info_params.fields["Overall number of objects"].c_str());
450  m_GT_poses_step = m_GT_poses.size() / num_of_objects;
451 
453  "Overall number of objects in rawlog: " << num_of_objects);
455  "Setting the Ground truth read step to: " << m_GT_poses_step);
456  }
457  catch (const std::exception& e)
458  {
459  MRPT_LOG_INFO_STREAM("RGBD_TUM info file was not found: " << e.what());
460  }
461 
462  // SLAM evaluation metric
463  m_curr_deformation_energy = 0;
464  if (m_visualize_SLAM_metric)
465  {
466  this->initSlamMetricVisualization();
467  }
468 
469  // Message to be displayed on pause
470  if (m_enable_visuals)
471  {
472  this->m_win->addTextMessage(0.5, 0.3, "", m_text_index_paused_message);
473  }
474 
475  MRPT_END
476 } // end of initClass
477 
478 template <class GRAPH_T>
480  mrpt::obs::CObservation::Ptr& observation, size_t& rawlog_entry)
481 {
482  using namespace mrpt::obs;
483 
484  CActionCollection::Ptr action;
485  CSensoryFrame::Ptr observations;
486 
487  return this->_execGraphSlamStep(
488  action, observations, observation, rawlog_entry);
489 } // end of execGraphSlamStep
490 
491 template <class GRAPH_T>
494  mrpt::obs::CSensoryFrame::Ptr& observations,
495  mrpt::obs::CObservation::Ptr& observation, size_t& rawlog_entry)
496 {
497  MRPT_START
498 
499  using namespace std;
500  using namespace mrpt;
501  using namespace mrpt::poses;
502  using namespace mrpt::obs;
503  using namespace mrpt::obs::utils;
504  using namespace mrpt::opengl;
505  using namespace mrpt::system;
506 
507  m_time_logger.enter("proc_time");
508 
510  m_has_read_config, "\nConfig file is not read yet.\nExiting... \n");
511  // good to go..
512 
513  // read first measurement independently if we haven't already
514  if (m_init_timestamp == INVALID_TIMESTAMP)
515  {
516  m_init_timestamp = getTimeStamp(action, observations, observation);
517  MRPT_LOG_DEBUG_STREAM("execGraphSlamStep: first run");
518 
519  if (observation)
520  {
521  MRPT_LOG_DEBUG_STREAM("Observation only dataset!");
522  m_observation_only_dataset = true; // false by default
523  }
524  else
525  {
526  MRPT_LOG_DEBUG_STREAM("Action-observation dataset!");
527  ASSERTDEB_(action);
528  m_observation_only_dataset = false;
529 
530  CPose3D increment_pose_3d;
531  action->getFirstMovementEstimationMean(increment_pose_3d);
532  pose_t increment_pose(increment_pose_3d);
533  m_curr_odometry_only_pose += increment_pose;
534  }
535 
536  // TODO enable this and test this.
537  // return true;
538  }
539 
540  // NRD
541  bool registered_new_node;
542  {
543  std::lock_guard<std::mutex> graph_lock(m_graph_section);
544  m_time_logger.enter("node_registrar");
545  registered_new_node =
546  m_node_reg->updateState(action, observations, observation);
547  m_time_logger.leave("node_registrar");
548  }
549 
550  { // get the 2D laser scan, if there
552  getObservation<CObservation2DRangeScan>(observations, observation);
553  if (scan)
554  {
555  m_last_laser_scan2D = scan;
556 
557  if (!m_first_laser_scan2D)
558  { // capture first laser scan seperately
559  m_first_laser_scan2D = m_last_laser_scan2D;
560  }
561  }
562  }
563 
564  if (registered_new_node)
565  {
566  // At the first node registration, must have registered exactly 2 nodes
567  // (root + first)
568  if (m_is_first_time_node_reg)
569  {
570  std::lock_guard<std::mutex> graph_lock(m_graph_section);
571  m_nodeID_max = 0;
572  if (m_graph.nodeCount() != 2)
573  {
575  "Expected [2] new registered nodes"
576  << " but got [" << m_graph.nodeCount() << "]");
577  THROW_EXCEPTION("Illegal node registration");
578  }
579 
580  m_nodes_to_laser_scans2D.insert(
581  make_pair(m_nodeID_max, m_first_laser_scan2D));
582 
583  m_is_first_time_node_reg = false;
584  }
585 
586  // going to be incremented in monitorNodeRegistration anyway.
587  m_nodeID_max++;
588 
589  // either way add just one odometry edge
590  m_edge_counter.addEdge("Odometry");
591  }
592  this->monitorNodeRegistration(
593  registered_new_node, "NodeRegistrationDecider");
594 
595  // Edge registration procedure - Optimization
596  // run this so that the ERD, GSO can be updated with the latest
597  // observations even when no new nodes have been added to the graph
598  { // ERD
599  std::lock_guard<std::mutex> graph_lock(m_graph_section);
600 
601  m_time_logger.enter("edge_registrar");
602  m_edge_reg->updateState(action, observations, observation);
603  m_time_logger.leave("edge_registrar");
604  }
605  this->monitorNodeRegistration(
606  registered_new_node, "EdgeRegistrationDecider");
607 
608  { // GSO
609  std::lock_guard<std::mutex> graph_lock(m_graph_section);
610 
611  m_time_logger.enter("optimizer");
612  m_optimizer->updateState(action, observations, observation);
613  m_time_logger.leave("optimizer");
614  }
615  this->monitorNodeRegistration(registered_new_node, "GraphSlamOptimizer");
616 
617  // current timestamp - to be filled depending on the format
618  m_curr_timestamp = getTimeStamp(action, observations, observation);
619 
620  if (observation)
621  {
622  // Read a single observation from the rawlog
623  // (Format #2 rawlog file)
624 
625  // odometry
626  if (IS_CLASS(*observation, CObservationOdometry))
627  {
628  CObservationOdometry::Ptr obs_odometry =
629  std::dynamic_pointer_cast<CObservationOdometry>(observation);
630 
631  m_curr_odometry_only_pose = pose_t(obs_odometry->odometry);
632  m_odometry_poses.push_back(m_curr_odometry_only_pose);
633  }
634  else if (IS_CLASS(*observation, CObservation3DRangeScan))
635  {
636  m_last_laser_scan3D =
637  std::dynamic_pointer_cast<mrpt::obs::CObservation3DRangeScan>(
638  observation);
639  }
640  }
641  else
642  {
643  // action, observations should contain a pair of valid data
644  // (Format #1 rawlog file)
645  ASSERTDEB_(observations);
646  ASSERTDEB_(action);
647 
648  CPose3D increment_pose_3d;
649  action->getFirstMovementEstimationMean(increment_pose_3d);
650  pose_t increment_pose(increment_pose_3d);
651  m_curr_odometry_only_pose += increment_pose;
652  m_odometry_poses.push_back(m_curr_odometry_only_pose);
653  } // ELSE FORMAT #1 - Action/Observations
654 
655  if (registered_new_node)
656  {
657  this->execDijkstraNodesEstimation();
658 
659  // keep track of the laser scans so that I can later visualize the map
660  m_nodes_to_laser_scans2D[m_nodeID_max] = m_last_laser_scan2D;
661 
662  if (m_enable_visuals && m_visualize_map)
663  {
664  std::lock_guard<std::mutex> graph_lock(m_graph_section);
665  // bool full_update = m_optimizer->justFullyOptimizedGraph();
666  bool full_update = true;
667  this->updateMapVisualization(m_nodes_to_laser_scans2D, full_update);
668  }
669 
670  // query node/edge deciders for visual objects update
671  if (m_enable_visuals)
672  {
673  this->updateAllVisuals();
674  }
675 
676  // update the edge counter
677  std::map<std::string, int> edge_types_to_nums;
678  m_edge_reg->getEdgesStats(&edge_types_to_nums);
679  if (edge_types_to_nums.size())
680  {
681  for (auto it = edge_types_to_nums.begin();
682  it != edge_types_to_nums.end(); ++it)
683  {
684  // loop closure
685  if (mrpt::system::strCmpI(it->first, "lc"))
686  {
687  m_edge_counter.setLoopClosureEdgesManually(it->second);
688  }
689  else
690  {
691  m_edge_counter.setEdgesManually(it->first, it->second);
692  }
693  }
694  }
695 
696  // update the graph visualization
697 
698  if (m_enable_curr_pos_viewport)
699  {
700  updateCurrPosViewport();
701  }
702  // update visualization of estimated trajectory
703  if (m_enable_visuals)
704  {
705  bool full_update = true; // don't care, do it anyway.
706  m_time_logger.enter("Visuals");
707  this->updateEstimatedTrajectoryVisualization(full_update);
708  m_time_logger.leave("Visuals");
709  }
710 
711  // refine the SLAM metric and update its corresponding visualization
712  // This is done only when the GT is available.
713  if (m_use_GT)
714  {
715  m_time_logger.enter("SLAM_metric");
716  this->computeSlamMetric(m_nodeID_max, m_GT_poses_index);
717  m_time_logger.leave("SLAM_metric");
718  if (m_visualize_SLAM_metric)
719  {
720  m_time_logger.enter("Visuals");
721  this->updateSlamMetricVisualization();
722  m_time_logger.leave("Visuals");
723  }
724  }
725 
726  // mark the map outdated
727  m_map_is_cached = false;
728 
729  } // IF REGISTERED_NEW_NODE
730 
731  //
732  // Visualization related actions
733  //
734  m_time_logger.enter("Visuals");
735  // Timestamp textMessage
736  // Use the dataset timestamp otherwise fallback to
737  // mrpt::system::getCurrentTime
738  if (m_enable_visuals)
739  {
740  if (m_curr_timestamp != INVALID_TIMESTAMP)
741  {
742  m_win_manager->addTextMessage(
743  m_offset_x_left, -m_offset_y_timestamp,
744  format(
745  "Simulated time: %s",
746  timeToString(m_curr_timestamp).c_str()),
747  mrpt::img::TColorf(1.0, 1.0, 1.0),
748  /* unique_index = */ m_text_index_timestamp);
749  }
750  else
751  {
752  m_win_manager->addTextMessage(
753  m_offset_x_left, -m_offset_y_timestamp,
754  format(
755  "Wall time: %s",
757  mrpt::img::TColorf(1.0, 1.0, 1.0),
758  /* unique_index = */ m_text_index_timestamp);
759  }
760  }
761 
762  // Odometry visualization
763  if (m_visualize_odometry_poses && m_odometry_poses.size())
764  {
765  this->updateOdometryVisualization();
766  }
767 
768  // ensure that the GT is visualized at the same rate as the SLAM procedure
769  // handle RGBD-TUM datasets manually. Advance the GT index accordingly
770  if (m_use_GT)
771  {
772  if (mrpt::system::strCmpI(m_GT_file_format, "rgbd_tum"))
773  { // 1/loop
774  if (m_enable_visuals)
775  {
776  this->updateGTVisualization(); // I have already taken care of
777  // the step
778  }
779  m_GT_poses_index += m_GT_poses_step;
780  }
781  else if (mrpt::system::strCmpI(m_GT_file_format, "navsimul"))
782  {
783  if (m_observation_only_dataset)
784  { // 1/2loops
785  if (rawlog_entry % 2 == 0)
786  {
787  if (m_enable_visuals)
788  {
789  this->updateGTVisualization(); // I have already taken
790  // care of the step
791  }
792  m_GT_poses_index += m_GT_poses_step;
793  }
794  }
795  else
796  { // 1/loop
797  // get both action and observation at a single step - same rate
798  // as GT
799  if (m_enable_visuals)
800  {
801  this->updateGTVisualization(); // I have already taken care
802  // of the step
803  }
804  m_GT_poses_index += m_GT_poses_step;
805  }
806  }
807  }
808 
809  // 3DRangeScans viewports update
810  if (mrpt::system::strCmpI(m_GT_file_format, "rgbd_tum"))
811  {
812  if (m_enable_range_viewport && m_last_laser_scan3D)
813  {
814  this->updateRangeImageViewport();
815  }
816 
817  if (m_enable_intensity_viewport && m_last_laser_scan3D)
818  {
819  this->updateIntensityImageViewport();
820  }
821  }
822 
823  // Query for events and take corresponding actions
824  if (m_enable_visuals)
825  {
826  this->queryObserverForEvents();
827  }
828  m_time_logger.leave("Visuals");
829 
830  m_dataset_grab_time =
831  mrpt::system::timeDifference(m_init_timestamp, m_curr_timestamp);
832  m_time_logger.leave("proc_time");
833 
834  return !m_request_to_exit;
835  MRPT_END
836 } // end of _execGraphSlamStep
837 
838 template <class GRAPH_T>
840 {
841  {
842  std::lock_guard<std::mutex> graph_lock(m_graph_section);
843  m_time_logger.enter("dijkstra_nodes_estimation");
844  m_graph.dijkstra_nodes_estimate();
845  m_time_logger.leave("dijkstra_nodes_estimation");
846  }
847 }
848 
849 template <class GRAPH_T>
851  bool registered /*=false*/, std::string class_name /*="Class"*/)
852 {
853  MRPT_START
854  using namespace mrpt;
855 
856  std::lock_guard<std::mutex> graph_lock(m_graph_section);
857  size_t listed_nodeCount =
858  (m_nodeID_max == INVALID_NODEID ? 0 : m_nodeID_max + 1);
859 
860  if (!registered)
861  { // just check that it's the same.
863  listed_nodeCount == m_graph.nodeCount(),
864  format(
865  "listed_nodeCount [%lu] != nodeCount() [%lu]",
866  static_cast<unsigned long>(listed_nodeCount),
867  static_cast<unsigned long>(m_graph.nodeCount())));
868  }
869  else
870  {
871  if (listed_nodeCount != m_graph.nodeCount())
872  {
874  class_name << " illegally added new nodes to the graph "
875  << ", wanted to see [" << listed_nodeCount
876  << "] but saw [" << m_graph.nodeCount() << "]");
878  format("Illegal node registration by %s.", class_name.c_str()));
879  }
880  }
881  MRPT_END
882 }
883 
884 template <class GRAPH_T>
887  mrpt::system::TTimeStamp* acquisition_time) const
888 {
889  MRPT_START
890 
891  if (!map)
892  {
894  }
895  ASSERTDEB_(map);
896 
897  if (!m_map_is_cached)
898  {
899  this->computeMap();
900  }
901  map->copyMapContentFrom(*m_gridmap_cached);
902 
903  // fill the timestamp if this is given
904  if (acquisition_time)
905  {
906  *acquisition_time = m_map_acq_time;
907  }
908  MRPT_END
909 }
910 
911 template <class GRAPH_T>
914  mrpt::system::TTimeStamp* acquisition_time) const
915 {
916  MRPT_START
917  THROW_EXCEPTION("Not Implemented Yet.");
918 
919  if (!m_map_is_cached)
920  {
921  this->computeMap();
922  }
923  // map =
924  // dynamic_cast<mrpt::maps::COctoMap::Ptr>(m_octomap_cached->clone());
925  ASSERTDEB_(map);
926 
927  // fill the timestamp if this is given
928  if (acquisition_time)
929  {
930  *acquisition_time = m_map_acq_time;
931  }
932 
933  MRPT_END
934 }
935 
936 template <class GRAPH_T>
938 {
939  MRPT_START
940  using namespace std;
941  using namespace mrpt;
942  using namespace mrpt::maps;
943  using namespace mrpt::poses;
944 
945  std::lock_guard<std::mutex> graph_lock(m_graph_section);
946 
947  if (!constraint_t::is_3D())
948  { // 2D Poses
949  // MRPT_LOG_DEBUG_STREAM("Computing the occupancy gridmap...");
950  mrpt::maps::COccupancyGridMap2D::Ptr gridmap = m_gridmap_cached;
951  gridmap->clear();
952 
953  // traverse all the nodes - add their laser scans at their corresponding
954  // poses
955  for (auto it = m_nodes_to_laser_scans2D.begin();
956  it != m_nodes_to_laser_scans2D.end(); ++it)
957  {
958  const mrpt::graphs::TNodeID& curr_node = it->first;
959 
960  // fetch LaserScan
961  const mrpt::obs::CObservation2DRangeScan::Ptr& curr_laser_scan =
962  it->second;
964  curr_laser_scan, format(
965  "LaserScan of nodeID: %lu is not present.",
966  static_cast<unsigned long>(curr_node)));
967 
968  // Fetch pose at which to display the LaserScan
969  CPose3D scan_pose = getLSPoseForGridMapVisualization(curr_node);
970 
971  // Add all to gridmap
972  gridmap->insertObservation(*curr_laser_scan, &scan_pose);
973  }
974 
975  m_map_is_cached = true;
976  m_map_acq_time = mrpt::system::now();
977  }
978  else
979  { // 3D Pose
980  // MRPT_LOG_DEBUG_STREAM("Computing the Octomap...");
981  THROW_EXCEPTION("Not Implemented Yet. Method is to compute a COctoMap");
982  // MRPT_LOG_DEBUG_STREAM("Computed COctoMap successfully.");
983  }
984 
985  MRPT_END
986 } // end of computeMap
987 
988 template <class GRAPH_T>
989 void CGraphSlamEngine<GRAPH_T>::loadParams(const std::string& fname)
990 {
991  MRPT_START
992 
995  mrpt::format("\nConfiguration file not found: \n%s\n", fname.c_str()));
996 
997  mrpt::config::CConfigFile cfg_file(fname);
998 
999  // Section: GeneralConfiguration
1000 
1001  m_user_decides_about_output_dir = cfg_file.read_bool(
1002  "GeneralConfiguration", "user_decides_about_output_dir", false, false);
1003  m_GT_file_format = cfg_file.read_string(
1004  "GeneralConfiguration", "ground_truth_file_format", "NavSimul", false);
1005 
1006  // Minimum verbosity level of the logger
1007  int min_verbosity_level =
1008  cfg_file.read_int("GeneralConfiguration", "class_verbosity", 1, false);
1009  this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));
1010 
1011  // Section: VisualizationParameters
1012 
1013  // map visualization
1014  m_visualize_map = cfg_file.read_bool(
1015  "VisualizationParameters", "visualize_map", true, false);
1016 
1017  // odometry-only visualization
1018  m_visualize_odometry_poses = cfg_file.read_bool(
1019  "VisualizationParameters", "visualize_odometry_poses", true, false);
1020  m_visualize_estimated_trajectory = cfg_file.read_bool(
1021  "VisualizationParameters", "visualize_estimated_trajectory", true,
1022  false);
1023 
1024  // GT configuration visualization
1025  m_visualize_GT = cfg_file.read_bool(
1026  "VisualizationParameters", "visualize_ground_truth", true, false);
1027  // SLAM metric plot
1028  m_visualize_SLAM_metric = cfg_file.read_bool(
1029  "VisualizationParameters", "visualize_SLAM_metric", true, false);
1030 
1031  // Viewports flags
1032  m_enable_curr_pos_viewport = cfg_file.read_bool(
1033  "VisualizationParameters", "enable_curr_pos_viewport", true, false);
1034  m_enable_range_viewport = cfg_file.read_bool(
1035  "VisualizationParameters", "enable_range_viewport", false, false);
1036  m_enable_intensity_viewport = cfg_file.read_bool(
1037  "VisualizationParameters", "enable_intensity_viewport", false, false);
1038 
1039  m_node_reg->loadParams(fname);
1040  m_edge_reg->loadParams(fname);
1041  m_optimizer->loadParams(fname);
1042 
1043  m_has_read_config = true;
1044  MRPT_END
1045 }
1046 template <class GRAPH_T>
1048 {
1049  MRPT_START
1050 
1051  std::string str;
1052  this->getParamsAsString(&str);
1053  return str;
1054 
1055  MRPT_END
1056 }
1057 template <class GRAPH_T>
1058 void CGraphSlamEngine<GRAPH_T>::getParamsAsString(std::string* params_out) const
1059 {
1060  MRPT_START
1061  ASSERTDEB_(m_has_read_config);
1062  using namespace std;
1063 
1064  stringstream ss_out;
1065 
1066  ss_out
1067  << "\n------------[ Graphslam_engine Problem Parameters ]------------"
1068  << std::endl;
1069  ss_out << "Config filename = " << m_config_fname
1070  << std::endl;
1071 
1072  ss_out << "Ground Truth File format = " << m_GT_file_format
1073  << std::endl;
1074  ss_out << "Ground Truth filename = " << m_fname_GT << std::endl;
1075 
1076  ss_out << "Visualize odometry = "
1077  << (m_visualize_odometry_poses ? "TRUE" : "FALSE") << std::endl;
1078  ss_out << "Visualize estimated trajectory = "
1079  << (m_visualize_estimated_trajectory ? "TRUE" : "FALSE")
1080  << std::endl;
1081  ss_out << "Visualize map = "
1082  << (m_visualize_map ? "TRUE" : "FALSE") << std::endl;
1083  ss_out << "Visualize Ground Truth = "
1084  << (m_visualize_GT ? "TRUE" : "FALSE") << std::endl;
1085 
1086  ss_out << "Visualize SLAM metric plot = "
1087  << (m_visualize_SLAM_metric ? "TRUE" : "FALSE") << std::endl;
1088 
1089  ss_out << "Enable curr. position viewport = "
1090  << (m_enable_curr_pos_viewport ? "TRUE" : "FALSE") << endl;
1091  ss_out << "Enable range img viewport = "
1092  << (m_enable_range_viewport ? "TRUE" : "FALSE") << endl;
1093  ss_out << "Enable intensity img viewport = "
1094  << (m_enable_intensity_viewport ? "TRUE" : "FALSE") << endl;
1095 
1096  ss_out << "-----------------------------------------------------------"
1097  << std::endl;
1098  ss_out << std::endl;
1099 
1100  // copy the stringstream contents to the passed in string
1101  *params_out = ss_out.str();
1102 
1103  MRPT_END
1104 }
1105 
1106 template <class GRAPH_T>
1108 {
1109  MRPT_START
1110  std::cout << getParamsAsString();
1111 
1112  m_node_reg->printParams();
1113  m_edge_reg->printParams();
1114  m_optimizer->printParams();
1115 
1116  MRPT_END
1117 }
1118 
1119 template <class GRAPH_T>
1120 void CGraphSlamEngine<GRAPH_T>::initResultsFile(const std::string& fname)
1121 {
1122  MRPT_START
1123  using namespace std;
1124  using namespace mrpt::system;
1125 
1126  MRPT_LOG_INFO_STREAM("Setting up file: " << fname);
1127 
1128  // current time vars
1130  std::string time_spec = "UTC Time";
1131  string cur_date_str(dateTimeToString(cur_date));
1132  string cur_date_validstr(fileNameStripInvalidChars(cur_date_str));
1133 
1134  m_out_streams[fname].open(fname);
1135  ASSERTDEBMSG_(
1136  m_out_streams[fname].fileOpenCorrectly(),
1137  mrpt::format("\nError while trying to open %s\n", fname.c_str()));
1138 
1139  const std::string sep(80, '#');
1140 
1141  m_out_streams[fname].printf("# Mobile Robot Programming Toolkit (MRPT)\n");
1142  m_out_streams[fname].printf("# http::/www.mrpt.org\n");
1143  m_out_streams[fname].printf("# GraphSlamEngine Application\n");
1144  m_out_streams[fname].printf(
1145  "# Automatically generated file - %s: %s\n", time_spec.c_str(),
1146  cur_date_str.c_str());
1147  m_out_streams[fname].printf("%s\n\n", sep.c_str());
1148 
1149  MRPT_END
1150 }
1151 
1152 template <class GRAPH_T>
1154 {
1155  MRPT_START
1156  ASSERTDEB_(m_enable_visuals);
1157  using namespace mrpt::opengl;
1158 
1159  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1160  COpenGLViewport::Ptr viewp_range;
1161 
1162  viewp_range = scene->createViewport("viewp_range");
1163  double x, y, h, w;
1164  m_win_manager->assignViewportParameters(&x, &y, &w, &h);
1165  viewp_range->setViewportPosition(x, y, h, w);
1166 
1167  m_win->unlockAccess3DScene();
1168  m_win->forceRepaint();
1169 
1170  MRPT_END
1171 }
1172 
1173 template <class GRAPH_T>
1175 {
1176  MRPT_START
1177  std::lock_guard<std::mutex> graph_lock(m_graph_section);
1178  m_time_logger.enter("Visuals");
1179 
1180  m_node_reg->updateVisuals();
1181  m_edge_reg->updateVisuals();
1182  m_optimizer->updateVisuals();
1183 
1184  m_time_logger.leave("Visuals");
1185  MRPT_END
1186 }
1187 
1188 template <class GRAPH_T>
1190 {
1191  MRPT_START
1192  ASSERTDEB_(m_enable_visuals);
1193  using namespace mrpt::math;
1194  using namespace mrpt::opengl;
1195 
1196  if (m_last_laser_scan3D->hasRangeImage)
1197  {
1198  // TODO - make this a static class member - or at least a private member
1199  // of the class
1200 
1201  mrpt::img::CImage img;
1202 
1203  // load the image if not already loaded..
1204  m_last_laser_scan3D->load();
1205  img.setFromMatrix(
1206  m_last_laser_scan3D->rangeImage, false /* do normalize */);
1207 
1208  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1209  COpenGLViewport::Ptr viewp_range = scene->getViewport("viewp_range");
1210  viewp_range->setImageView(img);
1211  m_win->unlockAccess3DScene();
1212  m_win->forceRepaint();
1213  }
1214 
1215  MRPT_END
1216 }
1217 
1218 template <class GRAPH_T>
1220 {
1221  MRPT_START
1222  ASSERTDEB_(m_enable_visuals);
1223  using namespace mrpt::opengl;
1224 
1225  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1226  COpenGLViewport::Ptr viewp_intensity;
1227 
1228  viewp_intensity = scene->createViewport("viewp_intensity");
1229  double x, y, w, h;
1230  m_win_manager->assignViewportParameters(&x, &y, &w, &h);
1231  viewp_intensity->setViewportPosition(x, y, w, h);
1232 
1233  m_win->unlockAccess3DScene();
1234  m_win->forceRepaint();
1235 
1236  MRPT_END
1237 }
1238 template <class GRAPH_T>
1240 {
1241  MRPT_START
1242  ASSERTDEB_(m_enable_visuals);
1243  using namespace mrpt::opengl;
1244 
1245  if (m_last_laser_scan3D->hasIntensityImage)
1246  {
1247  mrpt::img::CImage img;
1248 
1249  // load the image if not already loaded..
1250  m_last_laser_scan3D->load();
1251  img = m_last_laser_scan3D->intensityImage;
1252 
1253  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1254  COpenGLViewport::Ptr viewp_intensity =
1255  scene->getViewport("viewp_intensity");
1256  viewp_intensity->setImageView(img);
1257  m_win->unlockAccess3DScene();
1258  m_win->forceRepaint();
1259  }
1260 
1261  MRPT_END
1262 } // end of updateIntensityImageViewport
1263 
1264 template <class GRAPH_T>
1267 {
1268  pose_t p;
1269  return this->initRobotModelVisualizationInternal(p);
1270 } // end of initRobotModelVisualization
1271 
1272 template <class GRAPH_T>
1275  const mrpt::poses::CPose2D& p_unused)
1276 {
1278 
1279 } // end of initRobotModelVisualizationInternal - CPose2D
1280 
1281 template <class GRAPH_T>
1284  const mrpt::poses::CPose3D& p_unused)
1285 {
1287 } // end of initRobotModelVisualizationInternal - CPose3D
1288 
1289 template <class GRAPH_T>
1291 {
1292  MRPT_START
1293  ASSERTDEB_(m_enable_visuals);
1294  using namespace mrpt::opengl;
1295 
1296  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1297  COpenGLViewport::Ptr viewp =
1298  scene->createViewport("curr_robot_pose_viewport");
1299  // Add a clone viewport, using [0,1] factor X,Y,Width,Height coordinates:
1300  viewp->setCloneView("main");
1301  double x, y, h, w;
1302  m_win_manager->assignViewportParameters(&x, &y, &w, &h);
1303  viewp->setViewportPosition(x, y, h, w);
1304  viewp->setTransparent(false);
1305  viewp->getCamera().setAzimuthDegrees(90);
1306  viewp->getCamera().setElevationDegrees(90);
1307  viewp->setCustomBackgroundColor(
1308  mrpt::img::TColorf(205, 193, 197, /*alpha = */ 255));
1309  viewp->getCamera().setZoomDistance(30);
1310  viewp->getCamera().setOrthogonal();
1311 
1312  m_win->unlockAccess3DScene();
1313  m_win->forceRepaint();
1314 
1315  MRPT_END
1316 }
1317 
1318 template <class GRAPH_T>
1320 {
1321  MRPT_START
1322  ASSERTDEB_(m_enable_visuals);
1323  using namespace mrpt::opengl;
1324  using namespace mrpt::poses;
1325 
1326  ASSERTDEB_(m_enable_visuals);
1327 
1328  global_pose_t curr_robot_pose = this->getCurrentRobotPosEstimation();
1329 
1330  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1331  COpenGLViewport::Ptr viewp = scene->getViewport("curr_robot_pose_viewport");
1332  viewp->getCamera().setPointingAt(CPose3D(curr_robot_pose));
1333 
1334  m_win->unlockAccess3DScene();
1335  m_win->forceRepaint();
1336  MRPT_LOG_DEBUG_STREAM("Updated the \"current_pos\" viewport");
1337 
1338  MRPT_END
1339 }
1340 
1341 template <class GRAPH_T>
1343  const std::string& fname_GT, std::vector<mrpt::poses::CPose2D>* gt_poses,
1344  std::vector<mrpt::system::TTimeStamp>* gt_timestamps)
1345 {
1346  MRPT_START
1347  using namespace std;
1348  using namespace mrpt::system;
1349 
1350  // make sure file exists
1351  ASSERTDEBMSG_(
1352  fileExists(fname_GT),
1353  format(
1354  "\nGround-truth file %s was not found.\n"
1355  "Either specify a valid ground-truth filename or set set the "
1356  "m_visualize_GT flag to false\n",
1357  fname_GT.c_str()));
1358 
1359  mrpt::io::CFileInputStream file_GT(fname_GT);
1360  ASSERTDEBMSG_(file_GT.fileOpenCorrectly(), "\nCouldn't open GT file\n");
1361  ASSERTDEBMSG_(gt_poses, "\nNo valid std::vector<pose_t>* was given\n");
1362 
1363  string curr_line;
1364 
1365  // parse the file - get timestamp and pose and fill in the pose_t vector
1366  for (size_t line_num = 0; file_GT.readLine(curr_line); line_num++)
1367  {
1368  vector<string> curr_tokens;
1369  system::tokenize(curr_line, " ", curr_tokens);
1370 
1371  // check the current pose dimensions
1372  ASSERTDEBMSG_(
1373  curr_tokens.size() == constraint_t::state_length + 1,
1374  "\nGround Truth File doesn't seem to contain data as generated by "
1375  "the "
1376  "GridMapNavSimul application.\n Either specify the GT file format "
1377  "or set "
1378  "visualize_ground_truth to false in the .ini file\n");
1379 
1380  // timestamp
1381  if (gt_timestamps)
1382  {
1383  auto timestamp =
1384  mrpt::Clock::fromDouble(atof(curr_tokens[0].c_str()));
1385  gt_timestamps->push_back(timestamp);
1386  }
1387 
1388  // pose
1389  pose_t curr_pose(
1390  atof(curr_tokens[1].c_str()), atof(curr_tokens[2].c_str()),
1391  atof(curr_tokens[3].c_str()));
1392  gt_poses->push_back(curr_pose);
1393  }
1394 
1395  file_GT.close();
1396 
1397  MRPT_END
1398 }
1399 template <class GRAPH_T>
1401  const std::string& fname_GT, std::vector<mrpt::poses::CPose3D>* gt_poses,
1402  std::vector<mrpt::system::TTimeStamp>* gt_timestamps)
1403 {
1404  THROW_EXCEPTION("Not implemented.");
1405 }
1406 
1407 template <class GRAPH_T>
1409  const std::string& fname_GT, std::vector<mrpt::poses::CPose2D>* gt_poses,
1410  std::vector<mrpt::system::TTimeStamp>* gt_timestamps)
1411 {
1412  MRPT_START
1413  using namespace std;
1414  using namespace mrpt::math;
1415  using namespace mrpt::system;
1416 
1417  // make sure file exists
1418  ASSERTDEBMSG_(
1419  fileExists(fname_GT),
1420  format(
1421  "\nGround-truth file %s was not found.\n"
1422  "Either specify a valid ground-truth filename or set set the "
1423  "m_visualize_GT flag to false\n",
1424  fname_GT.c_str()));
1425 
1426  mrpt::io::CFileInputStream file_GT(fname_GT);
1427  ASSERTDEBMSG_(
1428  file_GT.fileOpenCorrectly(),
1429  "\nreadGTFileRGBD_TUM: Couldn't openGT file\n");
1430  ASSERTDEBMSG_(gt_poses, "No valid std::vector<pose_t>* was given");
1431 
1432  string curr_line;
1433 
1434  // move to the first non-commented immediately - comments before this..
1435  for (size_t i = 0; file_GT.readLine(curr_line); i++)
1436  {
1437  if (curr_line.at(0) != '#')
1438  {
1439  break;
1440  }
1441  }
1442 
1443  // handle the first pose seperately
1444  // make sure that the ground-truth starts at 0.
1445  pose_t pose_diff;
1446  vector<string> curr_tokens;
1447  mrpt::system::tokenize(curr_line, " ", curr_tokens);
1448 
1449  // check the current pose dimensions
1450  ASSERTDEBMSG_(
1451  curr_tokens.size() == 8,
1452  "\nGround Truth File doesn't seem to contain data as specified in "
1453  "RGBD_TUM related "
1454  "datasets.\n Either specify correct the GT file format or set "
1455  "visualize_ground_truth to false in the .ini file\n");
1456 
1457  // quaternion
1458  CQuaternionDouble quat;
1459  quat.r(atof(curr_tokens[7].c_str()));
1460  quat.x(atof(curr_tokens[4].c_str()));
1461  quat.y(atof(curr_tokens[5].c_str()));
1462  quat.z(atof(curr_tokens[6].c_str()));
1463  double r, p, y;
1464  quat.rpy(r, p, y);
1465 
1466  CVectorDouble curr_coords(3);
1467  curr_coords[0] = atof(curr_tokens[1].c_str());
1468  curr_coords[1] = atof(curr_tokens[2].c_str());
1469  curr_coords[2] = atof(curr_tokens[3].c_str());
1470 
1471  // initial pose
1472  pose_t curr_pose(curr_coords[0], curr_coords[1], y);
1473  // pose_t curr_pose(0, 0, 0);
1474 
1475  pose_diff = curr_pose;
1476 
1477  // parse the file - get timestamp and pose and fill in the pose_t vector
1478  for (; file_GT.readLine(curr_line);)
1479  {
1480  system::tokenize(curr_line, " ", curr_tokens);
1481  ASSERTDEBMSG_(
1482  curr_tokens.size() == 8,
1483  "\nGround Truth File doesn't seem to contain data as specified in "
1484  "RGBD_TUM related "
1485  "datasets.\n Either specify correct the GT file format or set "
1486  "visualize_ground_truth to false in the .ini file\n");
1487 
1488  // timestamp
1489  if (gt_timestamps)
1490  {
1491  auto timestamp =
1492  mrpt::Clock::fromDouble(atof(curr_tokens[0].c_str()));
1493  gt_timestamps->push_back(timestamp);
1494  }
1495 
1496  // quaternion
1497  quat.r(atof(curr_tokens[7].c_str()));
1498  quat.x(atof(curr_tokens[4].c_str()));
1499  quat.y(atof(curr_tokens[5].c_str()));
1500  quat.z(atof(curr_tokens[6].c_str()));
1501  quat.rpy(r, p, y);
1502 
1503  // pose
1504  curr_coords[0] = atof(curr_tokens[1].c_str());
1505  curr_coords[1] = atof(curr_tokens[2].c_str());
1506  curr_coords[2] = atof(curr_tokens[3].c_str());
1507 
1508  // current ground-truth pose
1509  pose_t gt_pose(curr_coords[0], curr_coords[1], y);
1510 
1511  gt_pose.x() -= pose_diff.x();
1512  gt_pose.y() -= pose_diff.y();
1513  gt_pose.phi() -= pose_diff.phi();
1514  // curr_pose += -pose_diff;
1515  gt_poses->push_back(gt_pose);
1516  }
1517 
1518  file_GT.close();
1519 
1520  MRPT_END
1521 }
1522 
1523 template <class GRAPH_T>
1525 {
1526  MRPT_START
1527  using namespace std;
1528  using namespace mrpt::math;
1529 
1530  // aligning GT (optical) frame with the MRPT frame
1531  // Set the rotation matrix from the corresponding RPY angles
1532  // MRPT Frame: X->forward; Y->Left; Z->Upward
1533  // Optical Frame: X->Right; Y->Downward; Z->Forward
1534  ASSERTDEB_(m_has_read_config);
1535  // rotz
1536  double anglez = 0.0_deg;
1537  const double tmpz[] = {
1538  cos(anglez), -sin(anglez), 0, sin(anglez), cos(anglez), 0, 0, 0, 1};
1539  CMatrixDouble rotz(3, 3, tmpz);
1540 
1541  // roty
1542  double angley = 0.0_deg;
1543  // double angley = 90.0_deg;
1544  const double tmpy[] = {cos(angley), 0, sin(angley), 0, 1, 0,
1545  -sin(angley), 0, cos(angley)};
1546  CMatrixDouble roty(3, 3, tmpy);
1547 
1548  // rotx
1549  // double anglex = -90.0_deg;
1550  double anglex = 0.0_deg;
1551  const double tmpx[] = {
1552  1, 0, 0, 0, cos(anglex), -sin(anglex), 0, sin(anglex), cos(anglex)};
1553  CMatrixDouble rotx(3, 3, tmpx);
1554 
1555  stringstream ss_out;
1556  ss_out << "\nConstructing the rotation matrix for the GroundTruth Data..."
1557  << endl;
1558  m_rot_TUM_to_MRPT = rotz * roty * rotx;
1559 
1560  ss_out << "Rotation matrices for optical=>MRPT transformation" << endl;
1561  ss_out << "rotz: " << endl << rotz << endl;
1562  ss_out << "roty: " << endl << roty << endl;
1563  ss_out << "rotx: " << endl << rotx << endl;
1564  ss_out << "Full rotation matrix: " << endl << m_rot_TUM_to_MRPT << endl;
1565 
1566  MRPT_LOG_DEBUG_STREAM(ss_out);
1567 
1568  MRPT_END
1569 }
1570 
1571 template <class GRAPH_T>
1573 {
1574  MRPT_START
1575  ASSERTDEB_(m_enable_visuals);
1576  ASSERTDEBMSG_(
1577  m_win_observer,
1578  "\nqueryObserverForEvents method was called even though no Observer "
1579  "object was provided\n");
1580 
1581  std::map<std::string, bool> events_occurred;
1582  m_win_observer->returnEventsStruct(&events_occurred);
1583  m_request_to_exit = events_occurred.find("Ctrl+c")->second;
1584 
1585  // odometry visualization
1586  if (events_occurred[m_keystroke_odometry])
1587  {
1588  this->toggleOdometryVisualization();
1589  }
1590  // GT visualization
1591  if (events_occurred[m_keystroke_GT])
1592  {
1593  this->toggleGTVisualization();
1594  }
1595  // Map visualization
1596  if (events_occurred[m_keystroke_map])
1597  {
1598  this->toggleMapVisualization();
1599  }
1600  // Estimated Trajectory Visualization
1601  if (events_occurred[m_keystroke_estimated_trajectory])
1602  {
1603  this->toggleEstimatedTrajectoryVisualization();
1604  }
1605  // pause/resume program execution
1606  if (events_occurred[m_keystroke_pause_exec])
1607  {
1608  this->togglePause();
1609  }
1610 
1611  // notify the deciders/optimizer of any events they may be interested in
1612  // MRPT_LOG_DEBUG_STREAM("Notifying deciders/optimizer for events");
1613  m_node_reg->notifyOfWindowEvents(events_occurred);
1614  m_edge_reg->notifyOfWindowEvents(events_occurred);
1615  m_optimizer->notifyOfWindowEvents(events_occurred);
1616 
1617  MRPT_END
1618 }
1619 
1620 template <class GRAPH_T>
1622 {
1623  MRPT_START
1624  ASSERTDEB_(m_enable_visuals);
1625  using namespace mrpt::opengl;
1626  MRPT_LOG_INFO_STREAM("Toggling Odometry visualization...");
1627 
1628  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1629 
1630  if (m_visualize_odometry_poses)
1631  {
1632  CRenderizable::Ptr obj = scene->getByName("odometry_poses_cloud");
1633  obj->setVisibility(!obj->isVisible());
1634 
1635  obj = scene->getByName("robot_odometry_poses");
1636  obj->setVisibility(!obj->isVisible());
1637  }
1638  else
1639  {
1640  dumpVisibilityErrorMsg("visualize_odometry_poses");
1641  }
1642 
1643  m_win->unlockAccess3DScene();
1644  m_win->forceRepaint();
1645 
1646  MRPT_END
1647 }
1648 template <class GRAPH_T>
1650 {
1651  MRPT_START
1652  ASSERTDEB_(m_enable_visuals);
1653  using namespace mrpt::opengl;
1654  MRPT_LOG_INFO_STREAM("Toggling Ground Truth visualization");
1655 
1656  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1657 
1658  if (m_visualize_GT)
1659  {
1660  CRenderizable::Ptr obj = scene->getByName("GT_cloud");
1661  obj->setVisibility(!obj->isVisible());
1662 
1663  obj = scene->getByName("robot_GT");
1664  obj->setVisibility(!obj->isVisible());
1665  }
1666  else
1667  {
1668  dumpVisibilityErrorMsg("visualize_ground_truth");
1669  }
1670 
1671  m_win->unlockAccess3DScene();
1672  m_win->forceRepaint();
1673 
1674  MRPT_END
1675 }
1676 template <class GRAPH_T>
1678 {
1679  MRPT_START
1680  ASSERTDEB_(m_enable_visuals);
1681  using namespace std;
1682  using namespace mrpt::opengl;
1683  MRPT_LOG_INFO_STREAM("Toggling Map visualization... ");
1684 
1685  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1686 
1687  // get total number of nodes
1688  int num_of_nodes;
1689  {
1690  std::lock_guard<std::mutex> graph_lock(m_graph_section);
1691  num_of_nodes = m_graph.nodeCount();
1692  }
1693 
1694  // name of gui object
1695  stringstream scan_name("");
1696 
1697  for (int node_cnt = 0; node_cnt != num_of_nodes; ++node_cnt)
1698  {
1699  // build the name of the potential corresponding object in the scene
1700  scan_name.str("");
1701  scan_name << "laser_scan_";
1702  scan_name << node_cnt;
1703 
1704  CRenderizable::Ptr obj = scene->getByName(scan_name.str());
1705  // current node may not have laserScans => may not have corresponding
1706  // obj
1707  if (obj)
1708  {
1709  obj->setVisibility(!obj->isVisible());
1710  }
1711  }
1712  m_win->unlockAccess3DScene();
1713  m_win->forceRepaint();
1714 
1715  MRPT_END
1716 }
1717 template <class GRAPH_T>
1719 {
1720  MRPT_START
1721  ASSERTDEB_(m_enable_visuals);
1722  using namespace mrpt::opengl;
1723  MRPT_LOG_INFO_STREAM("Toggling Estimated Trajectory visualization... ");
1724 
1725  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1726 
1727  if (m_visualize_estimated_trajectory)
1728  {
1729  CRenderizable::Ptr obj = scene->getByName("estimated_traj_setoflines");
1730  obj->setVisibility(!obj->isVisible());
1731 
1732  obj = scene->getByName("robot_estimated_traj");
1733  obj->setVisibility(!obj->isVisible());
1734  }
1735  else
1736  {
1737  dumpVisibilityErrorMsg("visualize_estimated_trajectory");
1738  }
1739 
1740  m_win->unlockAccess3DScene();
1741  m_win->forceRepaint();
1742 
1743  MRPT_END
1744 }
1745 template <class GRAPH_T>
1747  std::string viz_flag, int sleep_time)
1748 {
1749  MRPT_START
1750 
1752  "Cannot toggle visibility of specified object.\n"
1753  << "Make sure that the corresponding visualization flag (" << viz_flag
1754  << ") is set to true in the .ini file.");
1755  std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
1756 
1757  MRPT_END
1758 }
1759 
1760 template <class GRAPH_T>
1762  const mrpt::obs::CActionCollection::Ptr action,
1763  const mrpt::obs::CSensoryFrame::Ptr observations,
1764  const mrpt::obs::CObservation::Ptr observation)
1765 {
1766  MRPT_START
1767  using namespace mrpt::obs;
1768  using namespace mrpt::system;
1769 
1770  // make sure that adequate data is given.
1771  ASSERTDEBMSG_(
1772  action || observation,
1773  "Neither action or observation contains valid data.");
1774 
1776  if (observation)
1777  {
1778  timestamp = observation->timestamp;
1779  }
1780  else
1781  {
1782  // querry action part first
1783  timestamp = action->get(0)->timestamp;
1784 
1785  // if still not available query the observations in the CSensoryFrame
1786  if (timestamp == INVALID_TIMESTAMP)
1787  {
1788  for (auto sens_it = observations->begin();
1789  sens_it != observations->end(); ++sens_it)
1790  {
1791  timestamp = (*sens_it)->timestamp;
1792  if (timestamp != INVALID_TIMESTAMP)
1793  {
1794  break;
1795  }
1796  }
1797  }
1798  }
1799  return timestamp;
1800  MRPT_END
1801 }
1802 
1803 template <class GRAPH_T>
1806  const mrpt::graphs::TNodeID nodeID) const
1807 {
1808  return mrpt::poses::CPose3D(m_graph.nodes.at(nodeID));
1809 }
1810 
1811 template <class GRAPH_T>
1813 {
1814  using namespace mrpt::opengl;
1815 
1816  CSetOfObjects::Ptr map_obj = std::make_shared<CSetOfObjects>();
1817  map_obj->setName("map");
1818  COpenGLScene::Ptr& scene = this->m_win->get3DSceneAndLock();
1819  scene->insert(map_obj);
1820  this->m_win->unlockAccess3DScene();
1821  this->m_win->forceRepaint();
1822 }
1823 
1824 template <class GRAPH_T>
1826  const std::map<
1828  nodes_to_laser_scans2D,
1829  bool full_update)
1830 {
1831  MRPT_START
1832  ASSERTDEB_(m_enable_visuals);
1833  using namespace mrpt::obs;
1834  using namespace mrpt::opengl;
1835  using namespace std;
1836  using namespace mrpt::poses;
1837  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1838  CSetOfObjects::Ptr map_obj;
1839  {
1840  CRenderizable::Ptr obj = scene->getByName("map");
1841  map_obj = std::dynamic_pointer_cast<CSetOfObjects>(obj);
1842  ASSERTDEB_(map_obj);
1843  }
1844 
1845  mrpt::system::CTicTac map_update_timer;
1846  map_update_timer.Tic();
1847 
1848  // get the set of nodes for which to run the update
1849  std::set<mrpt::graphs::TNodeID> nodes_set;
1850  {
1851  if (full_update)
1852  {
1853  // for all the nodes get the node position and the corresponding
1854  // laser scan
1855  // if they were recorded and visualize them
1856  m_graph.getAllNodes(nodes_set);
1857  MRPT_LOG_DEBUG_STREAM("Executing full update of the map visuals");
1858  map_obj->clear();
1859 
1860  } // end if - full update
1861  else
1862  { // add only current nodeID
1863  nodes_set.insert(m_nodeID_max);
1864  } // end if - partial update
1865  }
1866 
1867  // for all the nodes in the previously populated set
1868  for (mrpt::graphs::TNodeID node_it : nodes_set)
1869  {
1870  // name of gui object
1871  stringstream scan_name("");
1872  scan_name << "laser_scan_";
1873  scan_name << node_it;
1874 
1875  // get the node laser scan
1876  CObservation2DRangeScan::Ptr scan_content;
1877  auto search = nodes_to_laser_scans2D.find(node_it);
1878 
1879  // make sure that the laser scan exists and is valid
1880  if (search != nodes_to_laser_scans2D.end() && search->second)
1881  {
1882  scan_content = search->second;
1883 
1884  CObservation2DRangeScan scan_decimated;
1885  this->decimateLaserScan(
1886  *scan_content, &scan_decimated,
1887  /*keep_every_n_entries = */ 5);
1888 
1889  // if the scan doesn't already exist, add it to the map object,
1890  // otherwise just
1891  // adjust its pose
1892  CRenderizable::Ptr obj = map_obj->getByName(scan_name.str());
1893  CSetOfObjects::Ptr scan_obj =
1894  std::dynamic_pointer_cast<CSetOfObjects>(obj);
1895  if (!scan_obj)
1896  {
1897  scan_obj = std::make_shared<CSetOfObjects>();
1898 
1899  // creating and inserting the observation in the CSetOfObjects
1901  m.insertObservation(scan_decimated);
1902  m.getAs3DObject(scan_obj);
1903 
1904  scan_obj->setName(scan_name.str());
1905  this->setObjectPropsFromNodeID(node_it, scan_obj);
1906 
1907  // set the visibility of the object the same value as the
1908  // visibility of
1909  // the previous - Needed for proper toggling of the visibility
1910  // of the
1911  // whole map
1912  {
1913  stringstream prev_scan_name("");
1914  prev_scan_name << "laser_scan_" << node_it - 1;
1915  CRenderizable::Ptr prev_obj =
1916  map_obj->getByName(prev_scan_name.str());
1917  if (prev_obj)
1918  {
1919  scan_obj->setVisibility(prev_obj->isVisible());
1920  }
1921  }
1922 
1923  map_obj->insert(scan_obj);
1924  }
1925  else
1926  {
1927  scan_obj = std::dynamic_pointer_cast<CSetOfObjects>(scan_obj);
1928  }
1929 
1930  // finally set the pose correctly - as computed by graphSLAM
1931  const CPose3D& scan_pose = CPose3D(m_graph.nodes.at(node_it));
1932  scan_obj->setPose(scan_pose);
1933  }
1934  else
1935  {
1937  "Laser scans of NodeID " << node_it << "are empty/invalid");
1938  }
1939 
1940  } // end for set of nodes
1941 
1942  m_win->unlockAccess3DScene();
1943  m_win->forceRepaint();
1944 
1945  double elapsed_time = map_update_timer.Tac();
1947  "updateMapVisualization took: " << elapsed_time << "s");
1948  MRPT_END
1949 } // end of updateMapVisualization
1950 
1951 template <class GRAPH_T>
1953  const mrpt::graphs::TNodeID nodeID,
1955 {
1956  MRPT_START
1957  viz_object->setColor_u8(m_optimized_map_color);
1958  MRPT_END
1959 }
1960 
1961 template <class GRAPH_T>
1963  mrpt::obs::CObservation2DRangeScan& laser_scan_in,
1964  mrpt::obs::CObservation2DRangeScan* laser_scan_out,
1965  const int keep_every_n_entries /*= 2*/)
1966 {
1967  MRPT_START
1968 
1969  size_t scan_size = laser_scan_in.getScanSize();
1970 
1971  // assign the decimated scans, ranges
1972  std::vector<float> new_scan(
1973  scan_size); // Was [], but can't use non-constant argument with arrays
1974  std::vector<char> new_validRange(scan_size);
1975  size_t new_scan_size = 0;
1976  for (size_t i = 0; i != scan_size; i++)
1977  {
1978  if (i % keep_every_n_entries == 0)
1979  {
1980  new_scan[new_scan_size] = laser_scan_in.getScanRange(i);
1981  new_validRange[new_scan_size] =
1982  laser_scan_in.getScanRangeValidity(i);
1983  new_scan_size++;
1984  }
1985  }
1986  laser_scan_out->loadFromVectors(
1987  new_scan_size, &new_scan[0], &new_validRange[0]);
1988 
1989  MRPT_END
1990 }
1991 
1992 template <class GRAPH_T>
1994 {
1995  MRPT_START
1996  ASSERTDEB_(m_enable_visuals);
1997  using namespace mrpt::opengl;
1998 
1999  // assertions
2000  ASSERTDEB_(m_has_read_config);
2001  ASSERTDEB_(
2002  m_win &&
2003  "Visualization of data was requested but no CDisplayWindow3D pointer "
2004  "was provided");
2005 
2006  // point cloud
2007  CPointCloud::Ptr GT_cloud = std::make_shared<CPointCloud>();
2008  GT_cloud->setPointSize(1.0);
2009  GT_cloud->enableColorFromX(false);
2010  GT_cloud->enableColorFromY(false);
2011  GT_cloud->enableColorFromZ(false);
2012  GT_cloud->setColor_u8(m_GT_color);
2013  GT_cloud->setName("GT_cloud");
2014 
2015  // robot model
2016  CSetOfObjects::Ptr robot_model = this->setCurrentPositionModel(
2017  "robot_GT", m_GT_color, m_robot_model_size);
2018 
2019  // insert them to the scene
2020  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
2021  scene->insert(GT_cloud);
2022  scene->insert(robot_model);
2023  m_win->unlockAccess3DScene();
2024 
2025  m_win_manager->assignTextMessageParameters(
2026  &m_offset_y_GT, &m_text_index_GT);
2027  m_win_manager->addTextMessage(
2028  m_offset_x_left, -m_offset_y_GT, "Ground truth path",
2029  mrpt::img::TColorf(m_GT_color), m_text_index_GT);
2030  m_win->forceRepaint();
2031 
2032  MRPT_END
2033 } // end of initGTVisualization
2034 
2035 template <class GRAPH_T>
2037 {
2038  MRPT_START
2039  ASSERTDEB_(m_enable_visuals);
2040  using namespace mrpt::opengl;
2041 
2042  // add to the GT PointCloud and visualize it
2043  // check that GT vector is not depleted
2044  if (m_visualize_GT && m_GT_poses_index < m_GT_poses.size())
2045  {
2046  ASSERTDEB_(
2047  m_win &&
2048  "Visualization of data was requested but no CDisplayWindow3D "
2049  "pointer was given");
2050 
2051  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
2052 
2053  CRenderizable::Ptr obj = scene->getByName("GT_cloud");
2054  CPointCloud::Ptr GT_cloud = std::dynamic_pointer_cast<CPointCloud>(obj);
2055 
2056  // add the latest GT pose
2057  mrpt::poses::CPose3D p(m_GT_poses[m_GT_poses_index]);
2058  GT_cloud->insertPoint(p.x(), p.y(), p.z());
2059 
2060  // robot model of GT trajectory
2061  obj = scene->getByName("robot_GT");
2062  CSetOfObjects::Ptr robot_obj =
2063  std::dynamic_pointer_cast<CSetOfObjects>(obj);
2064  robot_obj->setPose(p);
2065  m_win->unlockAccess3DScene();
2066  m_win->forceRepaint();
2067  }
2068 
2069  MRPT_END
2070 } // end of updateGTVisualization
2071 
2072 template <class GRAPH_T>
2074 {
2075  MRPT_START
2076  ASSERTDEB_(m_has_read_config);
2077  ASSERTDEB_(m_enable_visuals);
2078  using namespace mrpt::opengl;
2079 
2080  // point cloud
2081  CPointCloud::Ptr odometry_poses_cloud = std::make_shared<CPointCloud>();
2082  odometry_poses_cloud->setPointSize(1.0);
2083  odometry_poses_cloud->enableColorFromX(false);
2084  odometry_poses_cloud->enableColorFromY(false);
2085  odometry_poses_cloud->enableColorFromZ(false);
2086  odometry_poses_cloud->setColor_u8(m_odometry_color);
2087  odometry_poses_cloud->setName("odometry_poses_cloud");
2088 
2089  // robot model
2090  CSetOfObjects::Ptr robot_model = this->setCurrentPositionModel(
2091  "robot_odometry_poses", m_odometry_color, m_robot_model_size);
2092 
2093  // insert them to the scene
2094  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
2095  scene->insert(odometry_poses_cloud);
2096  scene->insert(robot_model);
2097  m_win->unlockAccess3DScene();
2098 
2099  m_win_manager->assignTextMessageParameters(
2100  &m_offset_y_odometry, &m_text_index_odometry);
2101  m_win_manager->addTextMessage(
2102  m_offset_x_left, -m_offset_y_odometry, "Odometry path",
2103  mrpt::img::TColorf(m_odometry_color), m_text_index_odometry);
2104 
2105  m_win->forceRepaint();
2106 
2107  MRPT_END
2108 }
2109 
2110 template <class GRAPH_T>
2112 {
2113  MRPT_START
2114  ASSERTDEB_(m_enable_visuals);
2115  ASSERTDEBMSG_(
2116  m_win,
2117  "Visualization of data was requested but no CDisplayWindow3D pointer "
2118  "was given");
2119  using namespace mrpt::opengl;
2120 
2121  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
2122 
2123  // point cloud
2124  CRenderizable::Ptr obj = scene->getByName("odometry_poses_cloud");
2125  CPointCloud::Ptr odometry_poses_cloud =
2126  std::dynamic_pointer_cast<CPointCloud>(obj);
2127  mrpt::poses::CPose3D p(m_odometry_poses.back());
2128 
2129  odometry_poses_cloud->insertPoint(p.x(), p.y(), p.z());
2130 
2131  // robot model
2132  obj = scene->getByName("robot_odometry_poses");
2133  CSetOfObjects::Ptr robot_obj =
2134  std::dynamic_pointer_cast<CSetOfObjects>(obj);
2135  robot_obj->setPose(p);
2136 
2137  m_win->unlockAccess3DScene();
2138  m_win->forceRepaint();
2139 
2140  MRPT_END
2141 }
2142 
2143 template <class GRAPH_T>
2145 {
2146  MRPT_START
2147  ASSERTDEB_(m_enable_visuals);
2148  using namespace mrpt::opengl;
2149 
2150  // SetOfLines
2151  CSetOfLines::Ptr estimated_traj_setoflines =
2152  std::make_shared<CSetOfLines>();
2153  estimated_traj_setoflines->setColor_u8(m_estimated_traj_color);
2154  estimated_traj_setoflines->setLineWidth(1.5);
2155  estimated_traj_setoflines->setName("estimated_traj_setoflines");
2156  // append a dummy line so that you can later use append using
2157  // CSetOfLines::appendLienStrip method.
2158  estimated_traj_setoflines->appendLine(
2159  /* 1st */ 0, 0, 0,
2160  /* 2nd */ 0, 0, 0);
2161 
2162  // robot model
2163  // pose_t initial_pose;
2164  CSetOfObjects::Ptr robot_model = this->setCurrentPositionModel(
2165  "robot_estimated_traj", m_estimated_traj_color, m_robot_model_size);
2166 
2167  // insert objects in the graph
2168  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
2169  if (m_visualize_estimated_trajectory)
2170  {
2171  scene->insert(estimated_traj_setoflines);
2172  }
2173  scene->insert(robot_model);
2174  m_win->unlockAccess3DScene();
2175 
2176  if (m_visualize_estimated_trajectory)
2177  {
2178  m_win_manager->assignTextMessageParameters(
2179  &m_offset_y_estimated_traj, &m_text_index_estimated_traj);
2180  m_win_manager->addTextMessage(
2181  m_offset_x_left, -m_offset_y_estimated_traj, "Estimated trajectory",
2182  mrpt::img::TColorf(m_estimated_traj_color),
2183  m_text_index_estimated_traj);
2184  }
2185 
2186  m_win->forceRepaint();
2187 
2188  MRPT_END
2189 }
2190 
2191 template <class GRAPH_T>
2193  bool full_update)
2194 {
2195  MRPT_START
2196  ASSERTDEB_(m_enable_visuals);
2197  using namespace mrpt::opengl;
2198 
2199  std::lock_guard<std::mutex> graph_lock(m_graph_section);
2200  ASSERTDEB_(m_graph.nodeCount() != 0);
2201 
2202  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
2203 
2204  CRenderizable::Ptr obj;
2205  if (m_visualize_estimated_trajectory)
2206  {
2207  // set of lines
2208  obj = scene->getByName("estimated_traj_setoflines");
2209  CSetOfLines::Ptr estimated_traj_setoflines =
2210  std::dynamic_pointer_cast<CSetOfLines>(obj);
2211 
2212  // gather set of nodes for which to append lines - all of the nodes in
2213  // the graph or just the last inserted..
2214  std::set<mrpt::graphs::TNodeID> nodes_set;
2215  {
2216  if (full_update)
2217  {
2218  this->getNodeIDsOfEstimatedTrajectory(&nodes_set);
2219  estimated_traj_setoflines->clear();
2220  // dummy way so that I can use appendLineStrip afterwards.
2221  estimated_traj_setoflines->appendLine(
2222  /* 1st */ 0, 0, 0,
2223  /* 2nd */ 0, 0, 0);
2224  }
2225  else
2226  {
2227  nodes_set.insert(m_graph.nodeCount() - 1);
2228  }
2229  }
2230  // append line for each node in the set
2231  for (unsigned long it : nodes_set)
2232  {
2233  mrpt::poses::CPose3D p(m_graph.nodes.at(it));
2234 
2235  estimated_traj_setoflines->appendLineStrip(p.x(), p.y(), p.z());
2236  }
2237  }
2238 
2239  // robot model
2240  // set the robot position to the last recorded pose in the graph
2241  obj = scene->getByName("robot_estimated_traj");
2242  CSetOfObjects::Ptr robot_obj =
2243  std::dynamic_pointer_cast<CSetOfObjects>(obj);
2244  pose_t curr_estimated_pos = m_graph.nodes[m_graph.nodeCount() - 1];
2245  robot_obj->setPose(curr_estimated_pos);
2246 
2247  m_win->unlockAccess3DScene();
2248  m_win->forceRepaint();
2249  MRPT_END
2250 } // end of updateEstimatedTrajectoryVisualization
2251 
2252 template <class GRAPH_T>
2254  const std::string& rawlog_fname)
2255 {
2256  this->setRawlogFile(rawlog_fname);
2257  this->initTRGBDInfoFileParams();
2258 }
2259 template <class GRAPH_T>
2261 {
2262  this->initTRGBDInfoFileParams();
2263 }
2264 
2265 template <class GRAPH_T>
2267  const std::string& rawlog_fname)
2268 {
2269  // get the correct info filename from the rawlog_fname
2270  std::string dir = mrpt::system::extractFileDirectory(rawlog_fname);
2271  std::string rawlog_filename = mrpt::system::extractFileName(rawlog_fname);
2272  std::string name_prefix = "rawlog_";
2273  std::string name_suffix = "_info.txt";
2274  info_fname = dir + name_prefix + rawlog_filename + name_suffix;
2275 }
2276 
2277 template <class GRAPH_T>
2279 {
2280  // fields to use
2281  fields["Overall number of objects"] = "";
2282  fields["Observations format"] = "";
2283 }
2284 
2285 template <class GRAPH_T>
2287 {
2288  ASSERT_FILE_EXISTS_(info_fname);
2289  using namespace std;
2290 
2291  // open file
2292  mrpt::io::CFileInputStream info_file(info_fname);
2293  ASSERTDEBMSG_(
2294  info_file.fileOpenCorrectly(),
2295  "\nTRGBDInfoFileParams::parseFile: Couldn't open info file\n");
2296 
2297  string curr_line;
2298  size_t line_cnt = 0;
2299 
2300  // parse until you find an empty line.
2301  while (true)
2302  {
2303  info_file.readLine(curr_line);
2304  line_cnt++;
2305  if (curr_line.size() == 0) break;
2306  }
2307 
2308  // parse the meaningful data
2309  while (info_file.readLine(curr_line))
2310  {
2311  // split current line at ":"
2312  vector<string> curr_tokens;
2313  mrpt::system::tokenize(curr_line, ":", curr_tokens);
2314 
2315  ASSERTDEB_EQUAL_(curr_tokens.size(), 2);
2316 
2317  // evaluate the name. if name in info struct then fill the corresponding
2318  // info struct parameter with the value_part in the file.
2319  std::string literal_part = mrpt::system::trim(curr_tokens[0]);
2320  std::string value_part = mrpt::system::trim(curr_tokens[1]);
2321 
2322  for (auto it = fields.begin(); it != fields.end(); ++it)
2323  {
2324  if (mrpt::system::strCmpI(it->first, literal_part))
2325  {
2326  it->second = value_part;
2327  }
2328  }
2329 
2330  line_cnt++;
2331  }
2332 }
2333 
2334 template <class GRAPH_T>
2335 void CGraphSlamEngine<GRAPH_T>::saveGraph(const std::string* fname_in) const
2336 {
2337  MRPT_START
2338 
2339  // what's the name of the file to be saved?
2340  std::string fname;
2341  if (fname_in)
2342  {
2343  fname = *fname_in;
2344  }
2345  else
2346  {
2347  fname = "output_graph.graph";
2348  }
2349 
2351  "Saving generated graph in VERTEX/EDGE format: " << fname);
2352  m_graph.saveToTextFile(fname);
2353 
2354  MRPT_END
2355 }
2356 
2357 template <class GRAPH_T>
2358 void CGraphSlamEngine<GRAPH_T>::save3DScene(const std::string* fname_in) const
2359 {
2360  MRPT_START
2361  ASSERTDEBMSG_(
2362  m_enable_visuals,
2363  "\nsave3DScene was called even though enable_visuals flag is "
2364  "off.\nExiting...\n");
2365  using namespace mrpt::opengl;
2366 
2367  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
2368 
2369  if (!scene)
2370  {
2372  "Could not fetch 3D Scene. Display window must already be closed.");
2373  return;
2374  }
2375 
2376  // TODO - what else is there to be excluded from the scene?
2377  // remove the CPlanarLaserScan if it exists
2378  {
2379  CPlanarLaserScan::Ptr laser_scan;
2380  for (; (laser_scan = scene->getByClass<CPlanarLaserScan>());)
2381  {
2383  "Removing CPlanarLaserScan from generated 3DScene...");
2384  scene->removeObject(laser_scan);
2385  }
2386  }
2387 
2388  // what's the name of the file to be saved?
2389  std::string fname;
2390  if (fname_in)
2391  {
2392  fname = *fname_in;
2393  }
2394  else
2395  {
2396  fname = "output_scene.3DScene";
2397  }
2398 
2399  MRPT_LOG_INFO_STREAM("Saving generated scene to " << fname);
2400  scene->saveToFile(fname);
2401 
2402  m_win->unlockAccess3DScene();
2403  m_win->forceRepaint();
2404 
2405  MRPT_END
2406 }
2407 
2408 template <class GRAPH_T>
2410  mrpt::graphs::TNodeID nodeID, size_t gt_index)
2411 {
2412  MRPT_START
2413  using namespace mrpt::math;
2414  using namespace mrpt::poses;
2415 
2416  // start updating the metric after a certain number of nodes have been added
2417  if (m_graph.nodeCount() < 4)
2418  {
2419  return;
2420  }
2421 
2422  // add to the map - keep track of which gt index corresponds to which nodeID
2423  m_nodeID_to_gt_indices[nodeID] = gt_index;
2424 
2425  // initialize the loop variables only once
2426  pose_t curr_node_pos;
2427  pose_t curr_gt_pos;
2428  pose_t node_delta_pos;
2429  pose_t gt_delta;
2430  double trans_diff = 0;
2431  double rot_diff = 0;
2432 
2433  size_t indices_size = m_nodeID_to_gt_indices.size();
2434 
2435  // recompute the metric from scratch
2436  m_curr_deformation_energy = 0;
2437 
2438  // first element of map
2439  auto start_it = m_nodeID_to_gt_indices.begin();
2440  start_it++;
2441 
2442  // fetch the first node, gt positions separately
2443  auto prev_it = start_it;
2444  prev_it--;
2445  pose_t prev_node_pos = m_graph.nodes[prev_it->first];
2446  pose_t prev_gt_pos = m_GT_poses[prev_it->second];
2447 
2448  // temporary constraint type
2449  constraint_t c;
2450 
2451  for (auto index_it = start_it; index_it != m_nodeID_to_gt_indices.end();
2452  index_it++)
2453  {
2454  curr_node_pos = m_graph.nodes[index_it->first];
2455  curr_gt_pos = m_GT_poses[index_it->second];
2456 
2457  node_delta_pos = curr_node_pos - prev_node_pos;
2458  gt_delta = curr_gt_pos - prev_gt_pos;
2459 
2460  trans_diff = gt_delta.distanceTo(node_delta_pos);
2461  rot_diff = this->accumulateAngleDiffs(gt_delta, node_delta_pos);
2462 
2463  m_curr_deformation_energy += (pow(trans_diff, 2) + pow(rot_diff, 2));
2464  m_curr_deformation_energy /= indices_size;
2465 
2466  // add it to the overall vector
2467  m_deformation_energy_vec.push_back(m_curr_deformation_energy);
2468 
2469  prev_node_pos = curr_node_pos;
2470  prev_gt_pos = curr_gt_pos;
2471  }
2472 
2474  "Total deformation energy: " << m_curr_deformation_energy);
2475 
2476  MRPT_END
2477 }
2478 
2479 template <class GRAPH_T>
2481  const mrpt::poses::CPose2D& p1, const mrpt::poses::CPose2D& p2)
2482 {
2483  return mrpt::math::wrapToPi(p1.phi() - p2.phi());
2484 }
2485 template <class GRAPH_T>
2487  const mrpt::poses::CPose3D& p1, const mrpt::poses::CPose3D& p2)
2488 {
2489  using mrpt::math::wrapToPi;
2490  double res = 0;
2491 
2492  res += wrapToPi(p1.roll() - p2.roll());
2493  res += wrapToPi(p1.pitch() - p2.pitch());
2494  res += wrapToPi(p1.yaw() - p2.yaw());
2495 
2496  return res;
2497 }
2498 
2499 template <class GRAPH_T>
2501 {
2502  ASSERTDEB_(m_enable_visuals);
2503 
2504  MRPT_START
2505  MRPT_LOG_DEBUG_STREAM("In initializeSlamMetricVisualization...");
2506  ASSERTDEB_(m_visualize_SLAM_metric);
2507 
2508  // initialize the m_win_plot on the stack
2509  m_win_plot = std::make_unique<mrpt::gui::CDisplayWindowPlots>(
2510  "Evolution of SLAM metric - Deformation Energy (1:1000)", 400, 300);
2511 
2512  m_win_plot->setPos(20, 50);
2513  m_win_plot->clf();
2514  // just plot the deformation points from scratch every time
2515  m_win_plot->hold_off();
2516  m_win_plot->enableMousePanZoom(true);
2517 
2518  MRPT_END
2519 }
2520 
2521 template <class GRAPH_T>
2523 {
2524  MRPT_START
2525  ASSERTDEB_(m_enable_visuals);
2526  ASSERTDEB_(m_win_plot && m_visualize_SLAM_metric);
2527 
2528  // build the X, Y vectors for plotting - use log scale
2529  std::vector<double> x(m_deformation_energy_vec.size(), 0);
2530  std::vector<double> y(m_deformation_energy_vec.size(), 0);
2531  for (size_t i = 0; i != x.size(); i++)
2532  {
2533  x[i] = i;
2534  y[i] = m_deformation_energy_vec[i] * 1000;
2535  }
2536 
2537  m_win_plot->plot(x, y, "r-1", "Deformation Energy (x1000)");
2538 
2539  // set the limits so that he y-values can be monitored
2540  // set the xmin limit with respect to xmax, which is constantly growing
2541  std::vector<double>::const_iterator xmax, ymax;
2542  xmax = std::max_element(x.begin(), x.end());
2543  ymax = std::max_element(y.begin(), y.end());
2544 
2545  m_win_plot->axis(
2546  /*x_min = */ xmax != x.end() ? -(*xmax / 12) : -1,
2547  /*x_max = */ (xmax != x.end() ? *xmax : 1),
2548  /*y_min = */ -0.4f,
2549  /*y_max = */ (ymax != y.end() ? *ymax : 1));
2550 
2551  MRPT_END
2552 }
2553 
2554 template <class GRAPH_T>
2556  std::string* report_str) const
2557 {
2558  MRPT_START
2559  using namespace std;
2560 
2561  // Summary of Results
2562  stringstream results_ss;
2563  results_ss << "Summary: " << std::endl;
2564  results_ss << this->header_sep << std::endl;
2565  results_ss << "\tProcessing time: "
2566  << m_time_logger.getMeanTime("proc_time") << std::endl;
2567  ;
2568  results_ss << "\tDataset Grab time: " << m_dataset_grab_time << std::endl;
2569  results_ss << "\tReal-time capable: "
2570  << (m_time_logger.getMeanTime("proc_time") < m_dataset_grab_time
2571  ? "TRUE"
2572  : "FALSE")
2573  << std::endl;
2574  results_ss << m_edge_counter.getAsString();
2575  results_ss << "\tNum of Nodes: " << m_graph.nodeCount() << std::endl;
2576  ;
2577 
2578  // Class configuration parameters
2579  std::string config_params = this->getParamsAsString();
2580 
2581  // time and output logging
2582  const std::string time_res = m_time_logger.getStatsAsText();
2583  const std::string output_res = this->getLogAsString();
2584 
2585  // merge the individual reports
2586  report_str->clear();
2587 
2588  *report_str += results_ss.str();
2589  *report_str += this->report_sep;
2590 
2591  *report_str += config_params;
2592  *report_str += this->report_sep;
2593 
2594  *report_str += time_res;
2595  *report_str += this->report_sep;
2596 
2597  *report_str += output_res;
2598  *report_str += this->report_sep;
2599 
2600  MRPT_END
2601 }
2602 
2603 template <class GRAPH_T>
2605  std::map<std::string, int>* node_stats,
2606  std::map<std::string, int>* edge_stats, mrpt::system::TTimeStamp* timestamp)
2607 {
2608  MRPT_START
2609  using namespace std;
2610  using namespace mrpt::graphslam::detail;
2611 
2612  ASSERTDEBMSG_(node_stats, "Invalid pointer to node_stats is given");
2613  ASSERTDEBMSG_(edge_stats, "Invalid pointer to edge_stats is given");
2614 
2615  if (m_nodeID_max == 0)
2616  {
2617  return false;
2618  }
2619 
2620  // fill the node stats
2621  (*node_stats)["nodes_total"] = m_nodeID_max + 1;
2622 
2623  // fill the edge stats
2624  for (auto it = m_edge_counter.cbegin(); it != m_edge_counter.cend(); ++it)
2625  {
2626  (*edge_stats)[it->first] = it->second;
2627  }
2628 
2629  (*edge_stats)["loop_closures"] = m_edge_counter.getLoopClosureEdges();
2630  (*edge_stats)["edges_total"] = m_edge_counter.getTotalNumOfEdges();
2631 
2632  // fill the timestamp
2633  if (timestamp)
2634  {
2635  *timestamp = m_curr_timestamp;
2636  }
2637 
2638  return true;
2639  MRPT_END
2640 }
2641 
2642 template <class GRAPH_T>
2644  const std::string& output_dir_fname)
2645 {
2646  MRPT_START
2647  using namespace mrpt::system;
2648  using namespace mrpt;
2649 
2650  ASSERTDEBMSG_(
2651  directoryExists(output_dir_fname),
2652  format(
2653  "Output directory \"%s\" doesn't exist", output_dir_fname.c_str()));
2654 
2655  MRPT_LOG_INFO_STREAM("Generating detailed class report...");
2656  std::lock_guard<std::mutex> graph_lock(m_graph_section);
2657 
2658  std::string report_str;
2659  std::string fname;
2660  const std::string ext = ".log";
2661 
2662  { // CGraphSlamEngine
2663  report_str.clear();
2664  fname = output_dir_fname + "/" + m_class_name + ext;
2665  // initialize the output file - refer to the stream through the
2666  // m_out_streams map
2667  this->initResultsFile(fname);
2668  this->getDescriptiveReport(&report_str);
2669  m_out_streams[fname].printf("%s", report_str.c_str());
2670 
2671  // write the timings into a separate file
2672  const std::string time_res = m_time_logger.getStatsAsText();
2673  fname = output_dir_fname + "/" + "timings" + ext;
2674  this->initResultsFile(fname);
2675  m_out_streams[fname].printf("%s", time_res.c_str());
2676  }
2677  { // node_registrar
2678  report_str.clear();
2679  fname = output_dir_fname + "/" + "node_registrar" + ext;
2680  this->initResultsFile(fname);
2681  m_node_reg->getDescriptiveReport(&report_str);
2682  m_out_streams[fname].printf("%s", report_str.c_str());
2683  }
2684  { // edge_registrar
2685  report_str.clear();
2686  fname = output_dir_fname + "/" + "edge_registrar" + ext;
2687  this->initResultsFile(fname);
2688  m_edge_reg->getDescriptiveReport(&report_str);
2689  m_out_streams[fname].printf("%s", report_str.c_str());
2690  }
2691  { // optimizer
2692  report_str.clear();
2693  fname = output_dir_fname + "/" + "optimizer" + ext;
2694  this->initResultsFile(fname);
2695  m_optimizer->getDescriptiveReport(&report_str);
2696  m_out_streams[fname].printf("%s", report_str.c_str());
2697  }
2698 
2699  if (m_use_GT)
2700  { // slam evaluation metric
2701  report_str.clear();
2702  const std::string desc(
2703  "# File includes the evolution of the SLAM metric. Implemented "
2704  "metric computes the \"deformation energy\" that is needed to "
2705  "transfer the estimated trajectory into the ground-truth "
2706  "trajectory (computed as sum of the difference between estimated "
2707  "trajectory and ground truth consecutive poses See \"A comparison "
2708  "of SLAM algorithms based on a graph of relations - W.Burgard et "
2709  "al.\", for more details on the metric.\n");
2710 
2711  fname = output_dir_fname + "/" + "SLAM_evaluation_metric" + ext;
2712  this->initResultsFile(fname);
2713 
2714  m_out_streams[fname].printf("%s\n", desc.c_str());
2715  for (auto vec_it = m_deformation_energy_vec.begin();
2716  vec_it != m_deformation_energy_vec.end(); ++vec_it)
2717  {
2718  m_out_streams[fname].printf("%f\n", *vec_it);
2719  }
2720  }
2721 
2722  MRPT_END
2723 }
2724 template <class GRAPH_T>
2727  const std::string& model_name, const mrpt::img::TColor& model_color,
2728  const size_t model_size, const pose_t& init_pose)
2729 {
2731  this->initRobotModelVisualization();
2732  model->setName(model_name);
2733  model->setColor_u8(model_color);
2734  model->setScale(model_size);
2735  model->setPose(init_pose);
2736 
2737  return model;
2738 }
2739 
2740 template <class GRAPH_T>
2742  std::vector<double>* vec_out) const
2743 {
2744  *vec_out = m_deformation_energy_vec;
2745 }
2746 } // namespace mrpt::graphslam
void updateOdometryVisualization()
Update odometry-only cloud with latest odometry estimation.
mrpt::opengl::CSetOfObjects::Ptr setCurrentPositionModel(const std::string &model_name, const mrpt::img::TColor &model_color=mrpt::img::TColor(0, 0, 0), const size_t model_size=1, const pose_t &init_pose=pose_t())
Set the opengl model that indicates the latest position of the trajectory at hand.
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:87
typename GRAPH_T::global_pose_t global_pose_t
void close()
Close the stream.
virtual void getNodeIDsOfEstimatedTrajectory(std::set< mrpt::graphs::TNodeID > *nodes_set) const
Return the list of nodeIDs which make up robot trajectory.
CGraphSlamEngine(const std::string &config_file, const std::string &rawlog_fname="", const std::string &fname_GT="", mrpt::graphslam::CWindowManager *win_manager=nullptr, mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > *node_reg=nullptr, mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T > *edge_reg=nullptr, mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T > *optimizer=nullptr)
Constructor of CGraphSlamEngine class template.
void queryObserverForEvents()
Query the observer instance for any user events.
#define MRPT_START
Definition: exceptions.h:241
void updateMapVisualization(const std::map< mrpt::graphs::TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr > &nodes_to_laser_scans2D, bool full_update=false)
Update the map visualization based on the current graphSLAM state.
static time_point fromDouble(const double t) noexcept
Create a timestamp from its double representation.
Definition: Clock.cpp:99
void parseFile()
Parse the RGBD information file to gain information about the rawlog file contents.
void save3DScene(const std::string *fname_in=nullptr) const
Wrapper method around the COpenGLScene::saveToFile method.
VerbosityLevel
Enumeration of available verbosity levels.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
bool getGraphSlamStats(std::map< std::string, int > *node_stats, std::map< std::string, int > *edge_stats, mrpt::system::TTimeStamp *timestamp=nullptr)
Fill the given maps with stats regarding the overall execution of graphslam.
const_iterator find(const KEY &key) const
Definition: ts_hash_map.h:224
A set of object, which are referenced to the coordinates framework established in this object...
Definition: CSetOfObjects.h:26
void updateGTVisualization()
Display the next ground truth position in the visualization window.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
void computeMap() const
Compute the map of the environment based on the recorded measurements.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
app setMinLoggingLevel(mrpt::system::LVL_ERROR)
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.h:82
void updateSlamMetricVisualization()
Update the displayPlots window with the new information with regards to the metric.
Interface for implementing node registration classes.
This class allows loading and storing values and vectors of different types from ".ini" files easily.
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.
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
global_pose_t getCurrentRobotPosEstimation() const
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:552
Interface for implementing edge registration classes.
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
std::string timeToString(const mrpt::system::TTimeStamp t)
Convert a timestamp into this textual form (UTC): HH:MM:SS.MMMMMM.
Definition: datetime.cpp:244
STL namespace.
virtual void setObjectPropsFromNodeID(const mrpt::graphs::TNodeID nodeID, mrpt::opengl::CSetOfObjects::Ptr &viz_object)
Set the properties of the map visual object based on the nodeID that it was produced by...
std::string fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores (&#39;_&#39;) or any other user-given char. ...
Definition: filesystem.cpp:329
static Ptr Create(Args &&... args)
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
size_t getScanSize() const
Get number of scan rays.
void tokenize(const std::string &inString, const std::string &inDelimiters, OUT_CONTAINER &outTokens, bool skipBlankTokens=true) noexcept
Tokenizes a string according to a set of delimiting characters.
virtual void monitorNodeRegistration(bool registered=false, std::string class_name="Class")
Assert that the given nodes number matches the registered graph nodes, otherwise throw exception...
void loadParams(const std::string &fname)
Read the configuration variables from the .ini file specified by the user.
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void computeSlamMetric(mrpt::graphs::TNodeID nodeID, size_t gt_index)
Compare the SLAM result (estimated trajectory) with the GT path.
void updateAllVisuals()
Wrapper around the deciders/optimizer updateVisuals methods.
This CStream derived class allow using a file as a read-only, binary stream.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
SLAM methods related to graphs of pose constraints.
static double accumulateAngleDiffs(const mrpt::poses::CPose2D &p1, const mrpt::poses::CPose2D &p2)
This base provides a set of functions for maths stuff.
static const std::string report_sep
T r() const
Return r (real part) coordinate of the quaternion.
Definition: CQuaternion.h:101
typename GRAPH_T::constraint_t::type_value pose_t
Type of underlying poses (2D/3D).
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
void execDijkstraNodesEstimation()
Wrapper around the GRAPH_T::dijkstra_nodes_estimate.
void updateIntensityImageViewport()
In RGB-D TUM Datasets update the Intensity image displayed in a seperate viewport.
#define ASSERTDEB_EQUAL_(__A, __B)
Definition: exceptions.h:192
bool readLine(std::string &str)
Reads one string line from the file (until a new-line character)
This namespace contains representation of robot actions and observations.
This object renders a 2D laser scan by means of three elements: the points, the line along end-points...
virtual void updateCurrPosViewport()
Update the viewport responsible for displaying the graph-building procedure in the estimated position...
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
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
mrpt::opengl::CSetOfObjects::Ptr initRobotModelVisualizationInternal(const mrpt::poses::CPose2D &p_unused)
Method to help overcome the partial template specialization restriction of C++.
void updateEstimatedTrajectoryVisualization(bool full_update=false)
Update the Esstimated robot trajectory with the latest estimated robot position.
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
void saveGraph(const std::string *fname_in=nullptr) const
Wrapper method around the GRAPH_T::saveToTextFile method.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the class state.
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
static const std::string header_sep
Separator string to be used in debugging messages.
CSetOfObjects::Ptr RobotPioneer()
Returns a representation of a Pioneer II mobile base.
virtual mrpt::poses::CPose3D getLSPoseForGridMapVisualization(const mrpt::graphs::TNodeID nodeID) const
return the 3D Pose of a LaserScan that is to be visualized.
void pause(const std::string &msg=std::string("Press any key to continue...")) noexcept
Shows the message "Press any key to continue" (or other custom message) to the current standard outpu...
Definition: os.cpp:441
virtual bool _execGraphSlamStep(mrpt::obs::CActionCollection::Ptr &action, mrpt::obs::CSensoryFrame::Ptr &observations, mrpt::obs::CObservation::Ptr &observation, size_t &rawlog_entry)
Main class method responsible for parsing each measurement and for executing graphSLAM.
mrpt::opengl::CSetOfObjects::Ptr initRobotModelVisualization()
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:191
void printParams() const
Print the problem parameters to the console for verification.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
virtual GRAPH_T::global_poses_t getRobotEstimatedTrajectory() const
static mrpt::system::TTimeStamp getTimeStamp(const mrpt::obs::CActionCollection::Ptr action, const mrpt::obs::CSensoryFrame::Ptr observations, const mrpt::obs::CObservation::Ptr observation)
Fill the TTimestamp in a consistent manner.
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
std::string dateTimeToString(const mrpt::system::TTimeStamp t)
Convert a timestamp into this textual form (UTC time): YEAR/MONTH/DAY,HH:MM:SS.MMM.
Definition: datetime.cpp:154
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:190
static Ptr Create(Args &&... args)
Definition: COctoMap.h:43
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
const float & getScanRange(const size_t i) const
The range values of the scan, in meters.
#define MRPT_END
Definition: exceptions.h:245
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
void decimateLaserScan(mrpt::obs::CObservation2DRangeScan &laser_scan_in, mrpt::obs::CObservation2DRangeScan *laser_scan_out, const int keep_every_n_entries=2)
Cut down on the size of the given laser scan.
void getDeformationEnergyVector(std::vector< double > *vec_out) const
Fill the given vector with the deformation energy values computed for the SLAM evaluation metric...
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
static void readGTFileRGBD_TUM(const std::string &fname_GT, std::vector< mrpt::poses::CPose2D > *gt_poses, std::vector< mrpt::system::TTimeStamp > *gt_timestamps=nullptr)
Parse the ground truth .txt file and fill in the corresponding m_GT_poses vector. ...
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
typename GRAPH_T::constraint_t constraint_t
Type of graph constraints.
void loadFromVectors(size_t nRays, const float *scanRanges, const char *scanValidity)
std::string trim(const std::string &str)
Removes leading and trailing spaces.
bool execGraphSlamStep(mrpt::obs::CObservation::Ptr &observation, size_t &rawlog_entry)
Wrapper method around _execGraphSlamStep.
#define INVALID_NODEID
Definition: TNodeID.h:19
std::string getParamsAsString() const
Wrapper around getParamsAsString.
bool directoryExists(const std::string &fileName)
Test if a given directory exists (it fails if the given path refers to an existing file)...
Definition: filesystem.cpp:137
double timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
Definition: datetime.h:123
static uint64_t getCurrentTime() noexcept
Definition: Clock.cpp:46
A RGB color - 8bit.
Definition: TColor.h:25
An observation of the current (cumulative) odometry for a wheeled robot.
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:76
std::string extractFileName(const std::string &filePath)
Extract just the name (without extension) of a filename from a complete path plus name plus extension...
Definition: filesystem.cpp:62
#define ASSERT_FILE_EXISTS_(FIL)
Definition: filesystem.h:22
static void readGTFile(const std::string &fname_GT, std::vector< mrpt::poses::CPose2D > *gt_poses, std::vector< mrpt::system::TTimeStamp > *gt_timestamps=nullptr)
Parse the ground truth .txt file and fill in the corresponding gt_poses vector.
A set of independent lines (or segments), one line with its own start and end positions (X...
Definition: CSetOfLines.h:32
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
bool strCmpI(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case insensitive)
static void setImagesPathBase(const std::string &path)
Definition: CImage.cpp:80
Internal auxiliary classes.
Definition: levmarq_impl.h:19
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
Wrapper method that used for printing error messages in a consistent manner.
bool fileOpenCorrectly() const
Returns true if the file was open without errors.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
std::string extractFileDirectory(const std::string &filePath)
Extract the whole path (the directory) of a filename from a complete path plus name plus extension...
Definition: filesystem.cpp:78
void updateRangeImageViewport()
In RGB-D TUM Datasets update the Range image displayed in a seperate viewport.
Class acts as a container for storing pointers to mrpt::gui::CDisplayWindow3D, mrpt::graphslam::CWind...
void generateReportFiles(const std::string &output_dir_fname_in)
Generate and write to a corresponding report for each of the respective self/decider/optimizer classe...
bool insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
Definition: CMetricMap.cpp:93
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
void getMap(mrpt::maps::COccupancyGridMap2D::Ptr map, mrpt::system::TTimeStamp *acquisition_time=nullptr) const
A cloud of points, all with the same color or each depending on its value along a particular coordina...
Definition: CPointCloud.h:44
void initResultsFile(const std::string &fname)
Automate the creation and initialization of a results file relevant to the application.
void initClass()
General initialization method to call from the Class Constructors.
static const std::string & getImagesPathBase()
By default, ".".
Definition: CImage.cpp:79
bool getScanRangeValidity(const size_t i) const
It&#39;s false (=0) on no reflected rays, referenced to elements in scan.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020