MRPT  1.9.9
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-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #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>
24 template <class GRAPH_T>
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 {
95  MRPT_START;
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 = mrpt::make_aligned_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  {
405  mrpt::make_aligned_shared<mrpt::maps::COccupancyGridMap2D>();
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  mrpt::make_aligned_shared<mrpt::maps::COctoMap>();
429 
430  // TODO - adjust the insertionoptions...
431  // TODO - Read these from the .ini file
432  octomap->insertionOptions.setOccupancyThres(0.5);
433  octomap->insertionOptions.setProbHit(0.7);
434  octomap->insertionOptions.setProbMiss(0.4);
435  octomap->insertionOptions.setClampingThresMin(0.1192);
436  octomap->insertionOptions.setClampingThresMax(0.971);
437 
438  m_octomap_cached = octomap;
439  m_map_is_cached = false;
440  }
441 
442  // In case we are given an RGBD TUM Dataset - try and read the info file so
443  // that we know how to play back the GT poses.
444  try
445  {
446  m_info_params.setRawlogFile(m_rawlog_fname);
447  m_info_params.parseFile();
448  // set the rate at which we read from the GT poses vector
449  int num_of_objects = std::atoi(
450  m_info_params.fields["Overall number of objects"].c_str());
451  m_GT_poses_step = m_GT_poses.size() / num_of_objects;
452 
454  "Overall number of objects in rawlog: " << num_of_objects);
456  "Setting the Ground truth read step to: " << m_GT_poses_step);
457  }
458  catch (const std::exception& e)
459  {
460  MRPT_LOG_INFO_STREAM("RGBD_TUM info file was not found: " << e.what());
461  }
462 
463  // SLAM evaluation metric
464  m_curr_deformation_energy = 0;
465  if (m_visualize_SLAM_metric)
466  {
467  this->initSlamMetricVisualization();
468  }
469 
470  // Message to be displayed on pause
471  if (m_enable_visuals)
472  {
473  this->m_win->addTextMessage(
474  0.5, 0.3, "", mrpt::img::TColorf(1.0, 0, 0),
475  m_text_index_paused_message);
476  }
477 
478  MRPT_END;
479 } // end of initClass
480 
481 template <class GRAPH_T>
483  mrpt::obs::CObservation::Ptr& observation, size_t& rawlog_entry)
484 {
485  using namespace mrpt::obs;
486 
487  CActionCollection::Ptr action;
488  CSensoryFrame::Ptr observations;
489 
490  return this->_execGraphSlamStep(
491  action, observations, observation, rawlog_entry);
492 } // end of execGraphSlamStep
493 
494 template <class GRAPH_T>
497  mrpt::obs::CSensoryFrame::Ptr& observations,
498  mrpt::obs::CObservation::Ptr& observation, size_t& rawlog_entry)
499 {
500  MRPT_START;
501 
502  using namespace std;
503  using namespace mrpt;
504  using namespace mrpt::poses;
505  using namespace mrpt::obs;
506  using namespace mrpt::obs::utils;
507  using namespace mrpt::opengl;
508  using namespace mrpt::system;
509 
510  m_time_logger.enter("proc_time");
511 
513  m_has_read_config,
514  format("\nConfig file is not read yet.\nExiting... \n"));
515  // good to go..
516 
517  // read first measurement independently if we haven't already
518  if (m_init_timestamp == INVALID_TIMESTAMP)
519  {
520  m_init_timestamp = getTimeStamp(action, observations, observation);
521  MRPT_LOG_DEBUG_STREAM("execGraphSlamStep: first run");
522 
523  if (observation)
524  {
525  MRPT_LOG_DEBUG_STREAM("Observation only dataset!");
526  m_observation_only_dataset = true; // false by default
527  }
528  else
529  {
530  MRPT_LOG_DEBUG_STREAM("Action-observation dataset!");
531  ASSERTDEB_(action);
532  m_observation_only_dataset = false;
533 
534  CPose3D increment_pose_3d;
535  action->getFirstMovementEstimationMean(increment_pose_3d);
536  pose_t increment_pose(increment_pose_3d);
537  m_curr_odometry_only_pose += increment_pose;
538  }
539 
540  // TODO enable this and test this.
541  // return true;
542  }
543 
544  // NRD
545  bool registered_new_node;
546  {
547  std::lock_guard<std::mutex> graph_lock(m_graph_section);
548  m_time_logger.enter("node_registrar");
549  registered_new_node =
550  m_node_reg->updateState(action, observations, observation);
551  m_time_logger.leave("node_registrar");
552  }
553 
554  { // get the 2D laser scan, if there
556  getObservation<CObservation2DRangeScan>(observations, observation);
557  if (scan)
558  {
559  m_last_laser_scan2D = scan;
560 
561  if (!m_first_laser_scan2D)
562  { // capture first laser scan seperately
563  m_first_laser_scan2D = m_last_laser_scan2D;
564  }
565  }
566  }
567 
568  if (registered_new_node)
569  {
570  // At the first node registration, must have registered exactly 2 nodes
571  // (root + first)
572  if (m_is_first_time_node_reg)
573  {
574  std::lock_guard<std::mutex> graph_lock(m_graph_section);
575  m_nodeID_max = 0;
576  if (m_graph.nodeCount() != 2)
577  {
579  "Expected [2] new registered nodes"
580  << " but got [" << m_graph.nodeCount() << "]");
581  THROW_EXCEPTION(format("Illegal node registration"));
582  }
583 
584  m_nodes_to_laser_scans2D.insert(
585  make_pair(m_nodeID_max, m_first_laser_scan2D));
586 
587  m_is_first_time_node_reg = false;
588  }
589 
590  // going to be incremented in monitorNodeRegistration anyway.
591  m_nodeID_max++;
592 
593  // either way add just one odometry edge
594  m_edge_counter.addEdge("Odometry");
595  }
596  this->monitorNodeRegistration(
597  registered_new_node, "NodeRegistrationDecider");
598 
599  // Edge registration procedure - Optimization
600  // run this so that the ERD, GSO can be updated with the latest
601  // observations even when no new nodes have been added to the graph
602  { // ERD
603  std::lock_guard<std::mutex> graph_lock(m_graph_section);
604 
605  m_time_logger.enter("edge_registrar");
606  m_edge_reg->updateState(action, observations, observation);
607  m_time_logger.leave("edge_registrar");
608  }
609  this->monitorNodeRegistration(
610  registered_new_node, "EdgeRegistrationDecider");
611 
612  { // GSO
613  std::lock_guard<std::mutex> graph_lock(m_graph_section);
614 
615  m_time_logger.enter("optimizer");
616  m_optimizer->updateState(action, observations, observation);
617  m_time_logger.leave("optimizer");
618  }
619  this->monitorNodeRegistration(registered_new_node, "GraphSlamOptimizer");
620 
621  // current timestamp - to be filled depending on the format
622  m_curr_timestamp = getTimeStamp(action, observations, observation);
623 
624  if (observation)
625  {
626  // Read a single observation from the rawlog
627  // (Format #2 rawlog file)
628 
629  // odometry
630  if (IS_CLASS(observation, CObservationOdometry))
631  {
632  CObservationOdometry::Ptr obs_odometry =
633  std::dynamic_pointer_cast<CObservationOdometry>(observation);
634 
635  m_curr_odometry_only_pose = pose_t(obs_odometry->odometry);
636  m_odometry_poses.push_back(m_curr_odometry_only_pose);
637  }
638  else if (IS_CLASS(observation, CObservation3DRangeScan))
639  {
640  m_last_laser_scan3D =
641  std::dynamic_pointer_cast<mrpt::obs::CObservation3DRangeScan>(
642  observation);
643  }
644  }
645  else
646  {
647  // action, observations should contain a pair of valid data
648  // (Format #1 rawlog file)
649  ASSERTDEB_(observations);
650  ASSERTDEB_(action);
651 
652  CPose3D increment_pose_3d;
653  action->getFirstMovementEstimationMean(increment_pose_3d);
654  pose_t increment_pose(increment_pose_3d);
655  m_curr_odometry_only_pose += increment_pose;
656  m_odometry_poses.push_back(m_curr_odometry_only_pose);
657  } // ELSE FORMAT #1 - Action/Observations
658 
659  if (registered_new_node)
660  {
661  this->execDijkstraNodesEstimation();
662 
663  // keep track of the laser scans so that I can later visualize the map
664  m_nodes_to_laser_scans2D[m_nodeID_max] = m_last_laser_scan2D;
665 
666  if (m_enable_visuals && m_visualize_map)
667  {
668  std::lock_guard<std::mutex> graph_lock(m_graph_section);
669  // bool full_update = m_optimizer->justFullyOptimizedGraph();
670  bool full_update = true;
671  this->updateMapVisualization(m_nodes_to_laser_scans2D, full_update);
672  }
673 
674  // query node/edge deciders for visual objects update
675  if (m_enable_visuals)
676  {
677  this->updateAllVisuals();
678  }
679 
680  // update the edge counter
681  std::map<std::string, int> edge_types_to_nums;
682  m_edge_reg->getEdgesStats(&edge_types_to_nums);
683  if (edge_types_to_nums.size())
684  {
685  for (auto it = edge_types_to_nums.begin();
686  it != edge_types_to_nums.end(); ++it)
687  {
688  // loop closure
689  if (mrpt::system::strCmpI(it->first, "lc"))
690  {
691  m_edge_counter.setLoopClosureEdgesManually(it->second);
692  }
693  else
694  {
695  m_edge_counter.setEdgesManually(it->first, it->second);
696  }
697  }
698  }
699 
700  // update the graph visualization
701 
702  if (m_enable_curr_pos_viewport)
703  {
704  updateCurrPosViewport();
705  }
706  // update visualization of estimated trajectory
707  if (m_enable_visuals)
708  {
709  bool full_update = true; // don't care, do it anyway.
710  m_time_logger.enter("Visuals");
711  this->updateEstimatedTrajectoryVisualization(full_update);
712  m_time_logger.leave("Visuals");
713  }
714 
715  // refine the SLAM metric and update its corresponding visualization
716  // This is done only when the GT is available.
717  if (m_use_GT)
718  {
719  m_time_logger.enter("SLAM_metric");
720  this->computeSlamMetric(m_nodeID_max, m_GT_poses_index);
721  m_time_logger.leave("SLAM_metric");
722  if (m_visualize_SLAM_metric)
723  {
724  m_time_logger.enter("Visuals");
725  this->updateSlamMetricVisualization();
726  m_time_logger.leave("Visuals");
727  }
728  }
729 
730  // mark the map outdated
731  m_map_is_cached = false;
732 
733  } // IF REGISTERED_NEW_NODE
734 
735  //
736  // Visualization related actions
737  //
738  m_time_logger.enter("Visuals");
739  // Timestamp textMessage
740  // Use the dataset timestamp otherwise fallback to
741  // mrpt::system::getCurrentTime
742  if (m_enable_visuals)
743  {
744  if (m_curr_timestamp != INVALID_TIMESTAMP)
745  {
746  m_win_manager->addTextMessage(
747  m_offset_x_left, -m_offset_y_timestamp,
748  format(
749  "Simulated time: %s",
750  timeToString(m_curr_timestamp).c_str()),
751  mrpt::img::TColorf(1.0, 1.0, 1.0),
752  /* unique_index = */ m_text_index_timestamp);
753  }
754  else
755  {
756  m_win_manager->addTextMessage(
757  m_offset_x_left, -m_offset_y_timestamp,
758  format(
759  "Wall time: %s",
761  mrpt::img::TColorf(1.0, 1.0, 1.0),
762  /* unique_index = */ m_text_index_timestamp);
763  }
764  }
765 
766  // Odometry visualization
767  if (m_visualize_odometry_poses && m_odometry_poses.size())
768  {
769  this->updateOdometryVisualization();
770  }
771 
772  // ensure that the GT is visualized at the same rate as the SLAM procedure
773  // handle RGBD-TUM datasets manually. Advance the GT index accordingly
774  if (m_use_GT)
775  {
776  if (mrpt::system::strCmpI(m_GT_file_format, "rgbd_tum"))
777  { // 1/loop
778  if (m_enable_visuals)
779  {
780  this->updateGTVisualization(); // I have already taken care of
781  // the step
782  }
783  m_GT_poses_index += m_GT_poses_step;
784  }
785  else if (mrpt::system::strCmpI(m_GT_file_format, "navsimul"))
786  {
787  if (m_observation_only_dataset)
788  { // 1/2loops
789  if (rawlog_entry % 2 == 0)
790  {
791  if (m_enable_visuals)
792  {
793  this->updateGTVisualization(); // I have already taken
794  // care of the step
795  }
796  m_GT_poses_index += m_GT_poses_step;
797  }
798  }
799  else
800  { // 1/loop
801  // get both action and observation at a single step - same rate
802  // as GT
803  if (m_enable_visuals)
804  {
805  this->updateGTVisualization(); // I have already taken care
806  // of the step
807  }
808  m_GT_poses_index += m_GT_poses_step;
809  }
810  }
811  }
812 
813  // 3DRangeScans viewports update
814  if (mrpt::system::strCmpI(m_GT_file_format, "rgbd_tum"))
815  {
816  if (m_enable_range_viewport && m_last_laser_scan3D)
817  {
818  this->updateRangeImageViewport();
819  }
820 
821  if (m_enable_intensity_viewport && m_last_laser_scan3D)
822  {
823  this->updateIntensityImageViewport();
824  }
825  }
826 
827  // Query for events and take corresponding actions
828  if (m_enable_visuals)
829  {
830  this->queryObserverForEvents();
831  }
832  m_time_logger.leave("Visuals");
833 
834  m_dataset_grab_time =
835  mrpt::system::timeDifference(m_init_timestamp, m_curr_timestamp);
836  m_time_logger.leave("proc_time");
837 
838  return !m_request_to_exit;
839  MRPT_END;
840 } // end of _execGraphSlamStep
841 
842 template <class GRAPH_T>
844 {
845  {
846  std::lock_guard<std::mutex> graph_lock(m_graph_section);
847  m_time_logger.enter("dijkstra_nodes_estimation");
848  m_graph.dijkstra_nodes_estimate();
849  m_time_logger.leave("dijkstra_nodes_estimation");
850  }
851 }
852 
853 template <class GRAPH_T>
855  bool registered /*=false*/, std::string class_name /*="Class"*/)
856 {
857  MRPT_START;
858  using namespace mrpt;
859 
860  std::lock_guard<std::mutex> graph_lock(m_graph_section);
861  size_t listed_nodeCount =
862  (m_nodeID_max == INVALID_NODEID ? 0 : m_nodeID_max + 1);
863 
864  if (!registered)
865  { // just check that it's the same.
867  listed_nodeCount == m_graph.nodeCount(),
868  format(
869  "listed_nodeCount [%lu] != nodeCount() [%lu]",
870  static_cast<unsigned long>(listed_nodeCount),
871  static_cast<unsigned long>(m_graph.nodeCount())));
872  }
873  else
874  {
875  if (listed_nodeCount != m_graph.nodeCount())
876  {
878  class_name << " illegally added new nodes to the graph "
879  << ", wanted to see [" << listed_nodeCount
880  << "] but saw [" << m_graph.nodeCount() << "]");
882  format("Illegal node registration by %s.", class_name.c_str()));
883  }
884  }
885  MRPT_END;
886 }
887 
888 template <class GRAPH_T>
891  mrpt::system::TTimeStamp* acquisition_time) const
892 {
893  MRPT_START;
894 
895  if (!map)
896  {
897  map = mrpt::make_aligned_shared<mrpt::maps::COccupancyGridMap2D>();
898  }
899  ASSERTDEB_(map);
900 
901  if (!m_map_is_cached)
902  {
903  this->computeMap();
904  }
905  map->copyMapContentFrom(*m_gridmap_cached);
906 
907  // fill the timestamp if this is given
908  if (acquisition_time)
909  {
910  *acquisition_time = m_map_acq_time;
911  }
912  MRPT_END;
913 }
914 
915 template <class GRAPH_T>
918  mrpt::system::TTimeStamp* acquisition_time) const
919 {
920  MRPT_START;
921  THROW_EXCEPTION("Not Implemented Yet.");
922 
923  if (!m_map_is_cached)
924  {
925  this->computeMap();
926  }
927  // map =
928  // dynamic_cast<mrpt::maps::COctoMap::Ptr>(m_octomap_cached->clone());
929  ASSERTDEB_(map);
930 
931  // fill the timestamp if this is given
932  if (acquisition_time)
933  {
934  *acquisition_time = m_map_acq_time;
935  }
936 
937  MRPT_END;
938 }
939 
940 template <class GRAPH_T>
942 {
943  MRPT_START;
944  using namespace std;
945  using namespace mrpt;
946  using namespace mrpt::maps;
947  using namespace mrpt::poses;
948 
949  std::lock_guard<std::mutex> graph_lock(m_graph_section);
950 
951  if (!constraint_t::is_3D())
952  { // 2D Poses
953  // MRPT_LOG_DEBUG_STREAM("Computing the occupancy gridmap...");
954  mrpt::maps::COccupancyGridMap2D::Ptr gridmap = m_gridmap_cached;
955  gridmap->clear();
956 
957  // traverse all the nodes - add their laser scans at their corresponding
958  // poses
959  for (auto it = m_nodes_to_laser_scans2D.begin();
960  it != m_nodes_to_laser_scans2D.end(); ++it)
961  {
962  const mrpt::graphs::TNodeID& curr_node = it->first;
963 
964  // fetch LaserScan
965  const mrpt::obs::CObservation2DRangeScan::Ptr& curr_laser_scan =
966  it->second;
968  curr_laser_scan, format(
969  "LaserScan of nodeID: %lu is not present.",
970  static_cast<unsigned long>(curr_node)));
971 
972  // Fetch pose at which to display the LaserScan
973  CPose3D scan_pose = getLSPoseForGridMapVisualization(curr_node);
974 
975  // Add all to gridmap
976  gridmap->insertObservation(curr_laser_scan.get(), &scan_pose);
977  }
978 
979  m_map_is_cached = true;
980  m_map_acq_time = mrpt::system::now();
981  }
982  else
983  { // 3D Pose
984  // MRPT_LOG_DEBUG_STREAM("Computing the Octomap...");
985  THROW_EXCEPTION("Not Implemented Yet. Method is to compute a COctoMap");
986  // MRPT_LOG_DEBUG_STREAM("Computed COctoMap successfully.");
987  }
988 
989  MRPT_END;
990 } // end of computeMap
991 
992 template <class GRAPH_T>
994 {
995  MRPT_START;
996 
999  mrpt::format("\nConfiguration file not found: \n%s\n", fname.c_str()));
1000 
1001  mrpt::config::CConfigFile cfg_file(fname);
1002 
1003  // Section: GeneralConfiguration
1004 
1005  m_user_decides_about_output_dir = cfg_file.read_bool(
1006  "GeneralConfiguration", "user_decides_about_output_dir", false, false);
1007  m_GT_file_format = cfg_file.read_string(
1008  "GeneralConfiguration", "ground_truth_file_format", "NavSimul", false);
1009 
1010  // Minimum verbosity level of the logger
1011  int min_verbosity_level =
1012  cfg_file.read_int("GeneralConfiguration", "class_verbosity", 1, false);
1013  this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));
1014 
1015  // Section: VisualizationParameters
1016 
1017  // map visualization
1018  m_visualize_map = cfg_file.read_bool(
1019  "VisualizationParameters", "visualize_map", true, false);
1020 
1021  // odometry-only visualization
1022  m_visualize_odometry_poses = cfg_file.read_bool(
1023  "VisualizationParameters", "visualize_odometry_poses", true, false);
1024  m_visualize_estimated_trajectory = cfg_file.read_bool(
1025  "VisualizationParameters", "visualize_estimated_trajectory", true,
1026  false);
1027 
1028  // GT configuration visualization
1029  m_visualize_GT = cfg_file.read_bool(
1030  "VisualizationParameters", "visualize_ground_truth", true, false);
1031  // SLAM metric plot
1032  m_visualize_SLAM_metric = cfg_file.read_bool(
1033  "VisualizationParameters", "visualize_SLAM_metric", true, false);
1034 
1035  // Viewports flags
1036  m_enable_curr_pos_viewport = cfg_file.read_bool(
1037  "VisualizationParameters", "enable_curr_pos_viewport", true, false);
1038  m_enable_range_viewport = cfg_file.read_bool(
1039  "VisualizationParameters", "enable_range_viewport", false, false);
1040  m_enable_intensity_viewport = cfg_file.read_bool(
1041  "VisualizationParameters", "enable_intensity_viewport", false, false);
1042 
1043  m_node_reg->loadParams(fname);
1044  m_edge_reg->loadParams(fname);
1045  m_optimizer->loadParams(fname);
1046 
1047  m_has_read_config = true;
1048  MRPT_END;
1049 }
1050 template <class GRAPH_T>
1052 {
1053  MRPT_START;
1054 
1055  std::string str;
1056  this->getParamsAsString(&str);
1057  return str;
1058 
1059  MRPT_END;
1060 }
1061 template <class GRAPH_T>
1063 {
1064  MRPT_START;
1065  ASSERTDEB_(m_has_read_config);
1066  using namespace std;
1067 
1068  stringstream ss_out;
1069 
1070  ss_out
1071  << "\n------------[ Graphslam_engine Problem Parameters ]------------"
1072  << std::endl;
1073  ss_out << "Config filename = " << m_config_fname
1074  << std::endl;
1075 
1076  ss_out << "Ground Truth File format = " << m_GT_file_format
1077  << std::endl;
1078  ss_out << "Ground Truth filename = " << m_fname_GT << std::endl;
1079 
1080  ss_out << "Visualize odometry = "
1081  << (m_visualize_odometry_poses ? "TRUE" : "FALSE") << std::endl;
1082  ss_out << "Visualize estimated trajectory = "
1083  << (m_visualize_estimated_trajectory ? "TRUE" : "FALSE")
1084  << std::endl;
1085  ss_out << "Visualize map = "
1086  << (m_visualize_map ? "TRUE" : "FALSE") << std::endl;
1087  ss_out << "Visualize Ground Truth = "
1088  << (m_visualize_GT ? "TRUE" : "FALSE") << std::endl;
1089 
1090  ss_out << "Visualize SLAM metric plot = "
1091  << (m_visualize_SLAM_metric ? "TRUE" : "FALSE") << std::endl;
1092 
1093  ss_out << "Enable curr. position viewport = "
1094  << (m_enable_curr_pos_viewport ? "TRUE" : "FALSE") << endl;
1095  ss_out << "Enable range img viewport = "
1096  << (m_enable_range_viewport ? "TRUE" : "FALSE") << endl;
1097  ss_out << "Enable intensity img viewport = "
1098  << (m_enable_intensity_viewport ? "TRUE" : "FALSE") << endl;
1099 
1100  ss_out << "-----------------------------------------------------------"
1101  << std::endl;
1102  ss_out << std::endl;
1103 
1104  // copy the stringstream contents to the passed in string
1105  *params_out = ss_out.str();
1106 
1107  MRPT_END;
1108 }
1109 
1110 template <class GRAPH_T>
1112 {
1113  MRPT_START;
1114  std::cout << getParamsAsString();
1115 
1116  m_node_reg->printParams();
1117  m_edge_reg->printParams();
1118  m_optimizer->printParams();
1119 
1120  MRPT_END;
1121 }
1122 
1123 template <class GRAPH_T>
1125 {
1126  MRPT_START;
1127  using namespace std;
1128  using namespace mrpt::system;
1129 
1130  MRPT_LOG_INFO_STREAM("Setting up file: " << fname);
1131 
1132  // current time vars
1134  std::string time_spec = "UTC Time";
1135  string cur_date_str(dateTimeToString(cur_date));
1136  string cur_date_validstr(fileNameStripInvalidChars(cur_date_str));
1137 
1138  m_out_streams[fname].open(fname);
1139  ASSERTDEBMSG_(
1140  m_out_streams[fname].fileOpenCorrectly(),
1141  mrpt::format("\nError while trying to open %s\n", fname.c_str()));
1142 
1143  const std::string sep(80, '#');
1144 
1145  m_out_streams[fname].printf("# Mobile Robot Programming Toolkit (MRPT)\n");
1146  m_out_streams[fname].printf("# http::/www.mrpt.org\n");
1147  m_out_streams[fname].printf("# GraphSlamEngine Application\n");
1148  m_out_streams[fname].printf(
1149  "# Automatically generated file - %s: %s\n", time_spec.c_str(),
1150  cur_date_str.c_str());
1151  m_out_streams[fname].printf("%s\n\n", sep.c_str());
1152 
1153  MRPT_END;
1154 }
1155 
1156 template <class GRAPH_T>
1158 {
1159  MRPT_START;
1160  ASSERTDEB_(m_enable_visuals);
1161  using namespace mrpt::opengl;
1162 
1163  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1164  COpenGLViewport::Ptr viewp_range;
1165 
1166  viewp_range = scene->createViewport("viewp_range");
1167  double x, y, h, w;
1168  m_win_manager->assignViewportParameters(&x, &y, &w, &h);
1169  viewp_range->setViewportPosition(x, y, h, w);
1170 
1171  m_win->unlockAccess3DScene();
1172  m_win->forceRepaint();
1173 
1174  MRPT_END;
1175 }
1176 
1177 template <class GRAPH_T>
1179 {
1180  MRPT_START;
1181  std::lock_guard<std::mutex> graph_lock(m_graph_section);
1182  m_time_logger.enter("Visuals");
1183 
1184  m_node_reg->updateVisuals();
1185  m_edge_reg->updateVisuals();
1186  m_optimizer->updateVisuals();
1187 
1188  m_time_logger.leave("Visuals");
1189  MRPT_END;
1190 }
1191 
1192 template <class GRAPH_T>
1194 {
1195  MRPT_START;
1196  ASSERTDEB_(m_enable_visuals);
1197  using namespace mrpt::math;
1198  using namespace mrpt::opengl;
1199 
1200  if (m_last_laser_scan3D->hasRangeImage)
1201  {
1202  // TODO - make this a static class member - or at least a private member
1203  // of the class
1204  CMatrixFloat range2D;
1206 
1207  // load the image if not already loaded..
1208  m_last_laser_scan3D->load();
1209  range2D = m_last_laser_scan3D->rangeImage *
1210  (1.0f / 5.0); // TODO - without the magic number?
1211  img.setFromMatrix(range2D);
1212 
1213  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1214  COpenGLViewport::Ptr viewp_range = scene->getViewport("viewp_range");
1215  viewp_range->setImageView(img);
1216  m_win->unlockAccess3DScene();
1217  m_win->forceRepaint();
1218  }
1219 
1220  MRPT_END;
1221 }
1222 
1223 template <class GRAPH_T>
1225 {
1226  MRPT_START;
1227  ASSERTDEB_(m_enable_visuals);
1228  using namespace mrpt::opengl;
1229 
1230  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1231  COpenGLViewport::Ptr viewp_intensity;
1232 
1233  viewp_intensity = scene->createViewport("viewp_intensity");
1234  double x, y, w, h;
1235  m_win_manager->assignViewportParameters(&x, &y, &w, &h);
1236  viewp_intensity->setViewportPosition(x, y, w, h);
1237 
1238  m_win->unlockAccess3DScene();
1239  m_win->forceRepaint();
1240 
1241  MRPT_END;
1242 }
1243 template <class GRAPH_T>
1245 {
1246  MRPT_START;
1247  ASSERTDEB_(m_enable_visuals);
1248  using namespace mrpt::opengl;
1249 
1250  if (m_last_laser_scan3D->hasIntensityImage)
1251  {
1253 
1254  // load the image if not already loaded..
1255  m_last_laser_scan3D->load();
1256  img = m_last_laser_scan3D->intensityImage;
1257 
1258  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1259  COpenGLViewport::Ptr viewp_intensity =
1260  scene->getViewport("viewp_intensity");
1261  viewp_intensity->setImageView(img);
1262  m_win->unlockAccess3DScene();
1263  m_win->forceRepaint();
1264  }
1265 
1266  MRPT_END;
1267 } // end of updateIntensityImageViewport
1268 
1269 template <class GRAPH_T>
1272 {
1273  pose_t p;
1274  return this->initRobotModelVisualizationInternal(p);
1275 } // end of initRobotModelVisualization
1276 
1277 template <class GRAPH_T>
1280  const mrpt::poses::CPose2D& p_unused)
1281 {
1283 
1284 } // end of initRobotModelVisualizationInternal - CPose2D
1285 
1286 template <class GRAPH_T>
1289  const mrpt::poses::CPose3D& p_unused)
1290 {
1292 } // end of initRobotModelVisualizationInternal - CPose3D
1293 
1294 template <class GRAPH_T>
1296 {
1297  MRPT_START;
1298  ASSERTDEB_(m_enable_visuals);
1299  using namespace mrpt::opengl;
1300 
1301  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1302  COpenGLViewport::Ptr viewp =
1303  scene->createViewport("curr_robot_pose_viewport");
1304  // Add a clone viewport, using [0,1] factor X,Y,Width,Height coordinates:
1305  viewp->setCloneView("main");
1306  double x, y, h, w;
1307  m_win_manager->assignViewportParameters(&x, &y, &w, &h);
1308  viewp->setViewportPosition(x, y, h, w);
1309  viewp->setTransparent(false);
1310  viewp->getCamera().setAzimuthDegrees(90);
1311  viewp->getCamera().setElevationDegrees(90);
1312  viewp->setCustomBackgroundColor(
1313  mrpt::img::TColorf(205, 193, 197, /*alpha = */ 255));
1314  viewp->getCamera().setZoomDistance(30);
1315  viewp->getCamera().setOrthogonal();
1316 
1317  m_win->unlockAccess3DScene();
1318  m_win->forceRepaint();
1319 
1320  MRPT_END;
1321 }
1322 
1323 template <class GRAPH_T>
1325 {
1326  MRPT_START;
1327  ASSERTDEB_(m_enable_visuals);
1328  using namespace mrpt::opengl;
1329  using namespace mrpt::poses;
1330 
1331  ASSERTDEB_(m_enable_visuals);
1332 
1333  global_pose_t curr_robot_pose = this->getCurrentRobotPosEstimation();
1334 
1335  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1336  COpenGLViewport::Ptr viewp = scene->getViewport("curr_robot_pose_viewport");
1337  viewp->getCamera().setPointingAt(CPose3D(curr_robot_pose));
1338 
1339  m_win->unlockAccess3DScene();
1340  m_win->forceRepaint();
1341  MRPT_LOG_DEBUG_STREAM("Updated the \"current_pos\" viewport");
1342 
1343  MRPT_END;
1344 }
1345 
1346 template <class GRAPH_T>
1348  const std::string& fname_GT, std::vector<mrpt::poses::CPose2D>* gt_poses,
1349  std::vector<mrpt::system::TTimeStamp>* gt_timestamps)
1350 {
1351  MRPT_START;
1352  using namespace std;
1353  using namespace mrpt::system;
1354 
1355  // make sure file exists
1356  ASSERTDEBMSG_(
1357  fileExists(fname_GT),
1358  format(
1359  "\nGround-truth file %s was not found.\n"
1360  "Either specify a valid ground-truth filename or set set the "
1361  "m_visualize_GT flag to false\n",
1362  fname_GT.c_str()));
1363 
1364  mrpt::io::CFileInputStream file_GT(fname_GT);
1365  ASSERTDEBMSG_(file_GT.fileOpenCorrectly(), "\nCouldn't open GT file\n");
1366  ASSERTDEBMSG_(gt_poses, "\nNo valid std::vector<pose_t>* was given\n");
1367 
1368  string curr_line;
1369 
1370  // parse the file - get timestamp and pose and fill in the pose_t vector
1371  for (size_t line_num = 0; file_GT.readLine(curr_line); line_num++)
1372  {
1373  vector<string> curr_tokens;
1374  system::tokenize(curr_line, " ", curr_tokens);
1375 
1376  // check the current pose dimensions
1377  ASSERTDEBMSG_(
1378  curr_tokens.size() == constraint_t::state_length + 1,
1379  "\nGround Truth File doesn't seem to contain data as generated by "
1380  "the "
1381  "GridMapNavSimul application.\n Either specify the GT file format "
1382  "or set "
1383  "visualize_ground_truth to false in the .ini file\n");
1384 
1385  // timestamp
1386  if (gt_timestamps)
1387  {
1388  auto timestamp =
1389  mrpt::Clock::fromDouble(atof(curr_tokens[0].c_str()));
1390  gt_timestamps->push_back(timestamp);
1391  }
1392 
1393  // pose
1394  pose_t curr_pose(
1395  atof(curr_tokens[1].c_str()), atof(curr_tokens[2].c_str()),
1396  atof(curr_tokens[3].c_str()));
1397  gt_poses->push_back(curr_pose);
1398  }
1399 
1400  file_GT.close();
1401 
1402  MRPT_END;
1403 }
1404 template <class GRAPH_T>
1406  const std::string& fname_GT, std::vector<mrpt::poses::CPose3D>* gt_poses,
1407  std::vector<mrpt::system::TTimeStamp>* gt_timestamps)
1408 {
1409  THROW_EXCEPTION("Not implemented.");
1410 }
1411 
1412 template <class GRAPH_T>
1414  const std::string& fname_GT, std::vector<mrpt::poses::CPose2D>* gt_poses,
1415  std::vector<mrpt::system::TTimeStamp>* gt_timestamps)
1416 {
1417  MRPT_START;
1418  using namespace std;
1419  using namespace mrpt::math;
1420  using namespace mrpt::system;
1421 
1422  // make sure file exists
1423  ASSERTDEBMSG_(
1424  fileExists(fname_GT),
1425  format(
1426  "\nGround-truth file %s was not found.\n"
1427  "Either specify a valid ground-truth filename or set set the "
1428  "m_visualize_GT flag to false\n",
1429  fname_GT.c_str()));
1430 
1431  mrpt::io::CFileInputStream file_GT(fname_GT);
1432  ASSERTDEBMSG_(
1433  file_GT.fileOpenCorrectly(),
1434  "\nreadGTFileRGBD_TUM: Couldn't openGT file\n");
1435  ASSERTDEBMSG_(gt_poses, "No valid std::vector<pose_t>* was given");
1436 
1437  string curr_line;
1438 
1439  // move to the first non-commented immediately - comments before this..
1440  for (size_t i = 0; file_GT.readLine(curr_line); i++)
1441  {
1442  if (curr_line.at(0) != '#')
1443  {
1444  break;
1445  }
1446  }
1447 
1448  // handle the first pose seperately
1449  // make sure that the ground-truth starts at 0.
1450  pose_t pose_diff;
1451  vector<string> curr_tokens;
1452  mrpt::system::tokenize(curr_line, " ", curr_tokens);
1453 
1454  // check the current pose dimensions
1455  ASSERTDEBMSG_(
1456  curr_tokens.size() == 8,
1457  "\nGround Truth File doesn't seem to contain data as specified in "
1458  "RGBD_TUM related "
1459  "datasets.\n Either specify correct the GT file format or set "
1460  "visualize_ground_truth to false in the .ini file\n");
1461 
1462  // quaternion
1463  CQuaternionDouble quat;
1464  quat.r(atof(curr_tokens[7].c_str()));
1465  quat.x(atof(curr_tokens[4].c_str()));
1466  quat.y(atof(curr_tokens[5].c_str()));
1467  quat.z(atof(curr_tokens[6].c_str()));
1468  double r, p, y;
1469  quat.rpy(r, p, y);
1470 
1471  CVectorDouble curr_coords(3);
1472  curr_coords[0] = atof(curr_tokens[1].c_str());
1473  curr_coords[1] = atof(curr_tokens[2].c_str());
1474  curr_coords[2] = atof(curr_tokens[3].c_str());
1475 
1476  // initial pose
1477  pose_t curr_pose(curr_coords[0], curr_coords[1], y);
1478  // pose_t curr_pose(0, 0, 0);
1479 
1480  pose_diff = curr_pose;
1481 
1482  // parse the file - get timestamp and pose and fill in the pose_t vector
1483  for (; file_GT.readLine(curr_line);)
1484  {
1485  system::tokenize(curr_line, " ", curr_tokens);
1486  ASSERTDEBMSG_(
1487  curr_tokens.size() == 8,
1488  "\nGround Truth File doesn't seem to contain data as specified in "
1489  "RGBD_TUM related "
1490  "datasets.\n Either specify correct the GT file format or set "
1491  "visualize_ground_truth to false in the .ini file\n");
1492 
1493  // timestamp
1494  if (gt_timestamps)
1495  {
1496  auto timestamp =
1497  mrpt::Clock::fromDouble(atof(curr_tokens[0].c_str()));
1498  gt_timestamps->push_back(timestamp);
1499  }
1500 
1501  // quaternion
1502  quat.r(atof(curr_tokens[7].c_str()));
1503  quat.x(atof(curr_tokens[4].c_str()));
1504  quat.y(atof(curr_tokens[5].c_str()));
1505  quat.z(atof(curr_tokens[6].c_str()));
1506  quat.rpy(r, p, y);
1507 
1508  // pose
1509  curr_coords[0] = atof(curr_tokens[1].c_str());
1510  curr_coords[1] = atof(curr_tokens[2].c_str());
1511  curr_coords[2] = atof(curr_tokens[3].c_str());
1512 
1513  // current ground-truth pose
1514  pose_t gt_pose(curr_coords[0], curr_coords[1], y);
1515 
1516  gt_pose.x() -= pose_diff.x();
1517  gt_pose.y() -= pose_diff.y();
1518  gt_pose.phi() -= pose_diff.phi();
1519  // curr_pose += -pose_diff;
1520  gt_poses->push_back(gt_pose);
1521  }
1522 
1523  file_GT.close();
1524 
1525  MRPT_END;
1526 }
1527 
1528 template <class GRAPH_T>
1530 {
1531  MRPT_START;
1532  using namespace std;
1533  using namespace mrpt::math;
1534 
1535  // aligning GT (optical) frame with the MRPT frame
1536  // Set the rotation matrix from the corresponding RPY angles
1537  // MRPT Frame: X->forward; Y->Left; Z->Upward
1538  // Optical Frame: X->Right; Y->Downward; Z->Forward
1539  ASSERTDEB_(m_has_read_config);
1540  // rotz
1541  double anglez = DEG2RAD(0.0);
1542  const double tmpz[] = {
1543  cos(anglez), -sin(anglez), 0, sin(anglez), cos(anglez), 0, 0, 0, 1};
1544  CMatrixDouble rotz(3, 3, tmpz);
1545 
1546  // roty
1547  double angley = DEG2RAD(0.0);
1548  // double angley = DEG2RAD(90.0);
1549  const double tmpy[] = {cos(angley), 0, sin(angley), 0, 1, 0,
1550  -sin(angley), 0, cos(angley)};
1551  CMatrixDouble roty(3, 3, tmpy);
1552 
1553  // rotx
1554  // double anglex = DEG2RAD(-90.0);
1555  double anglex = DEG2RAD(0.0);
1556  const double tmpx[] = {
1557  1, 0, 0, 0, cos(anglex), -sin(anglex), 0, sin(anglex), cos(anglex)};
1558  CMatrixDouble rotx(3, 3, tmpx);
1559 
1560  stringstream ss_out;
1561  ss_out << "\nConstructing the rotation matrix for the GroundTruth Data..."
1562  << endl;
1563  m_rot_TUM_to_MRPT = rotz * roty * rotx;
1564 
1565  ss_out << "Rotation matrices for optical=>MRPT transformation" << endl;
1566  ss_out << "rotz: " << endl << rotz << endl;
1567  ss_out << "roty: " << endl << roty << endl;
1568  ss_out << "rotx: " << endl << rotx << endl;
1569  ss_out << "Full rotation matrix: " << endl << m_rot_TUM_to_MRPT << endl;
1570 
1571  MRPT_LOG_DEBUG_STREAM(ss_out);
1572 
1573  MRPT_END;
1574 }
1575 
1576 template <class GRAPH_T>
1578 {
1579  MRPT_START;
1580  ASSERTDEB_(m_enable_visuals);
1581  ASSERTDEBMSG_(
1582  m_win_observer,
1583  "\nqueryObserverForEvents method was called even though no Observer "
1584  "object was provided\n");
1585 
1586  std::map<std::string, bool> events_occurred;
1587  m_win_observer->returnEventsStruct(&events_occurred);
1588  m_request_to_exit = events_occurred.find("Ctrl+c")->second;
1589 
1590  // odometry visualization
1591  if (events_occurred[m_keystroke_odometry])
1592  {
1593  this->toggleOdometryVisualization();
1594  }
1595  // GT visualization
1596  if (events_occurred[m_keystroke_GT])
1597  {
1598  this->toggleGTVisualization();
1599  }
1600  // Map visualization
1601  if (events_occurred[m_keystroke_map])
1602  {
1603  this->toggleMapVisualization();
1604  }
1605  // Estimated Trajectory Visualization
1606  if (events_occurred[m_keystroke_estimated_trajectory])
1607  {
1608  this->toggleEstimatedTrajectoryVisualization();
1609  }
1610  // pause/resume program execution
1611  if (events_occurred[m_keystroke_pause_exec])
1612  {
1613  this->togglePause();
1614  }
1615 
1616  // notify the deciders/optimizer of any events they may be interested in
1617  // MRPT_LOG_DEBUG_STREAM("Notifying deciders/optimizer for events");
1618  m_node_reg->notifyOfWindowEvents(events_occurred);
1619  m_edge_reg->notifyOfWindowEvents(events_occurred);
1620  m_optimizer->notifyOfWindowEvents(events_occurred);
1621 
1622  MRPT_END;
1623 }
1624 
1625 template <class GRAPH_T>
1627 {
1628  MRPT_START;
1629  ASSERTDEB_(m_enable_visuals);
1630  using namespace mrpt::opengl;
1631  MRPT_LOG_INFO_STREAM("Toggling Odometry visualization...");
1632 
1633  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1634 
1635  if (m_visualize_odometry_poses)
1636  {
1637  CRenderizable::Ptr obj = scene->getByName("odometry_poses_cloud");
1638  obj->setVisibility(!obj->isVisible());
1639 
1640  obj = scene->getByName("robot_odometry_poses");
1641  obj->setVisibility(!obj->isVisible());
1642  }
1643  else
1644  {
1645  dumpVisibilityErrorMsg("visualize_odometry_poses");
1646  }
1647 
1648  m_win->unlockAccess3DScene();
1649  m_win->forceRepaint();
1650 
1651  MRPT_END;
1652 }
1653 template <class GRAPH_T>
1655 {
1656  MRPT_START;
1657  ASSERTDEB_(m_enable_visuals);
1658  using namespace mrpt::opengl;
1659  MRPT_LOG_INFO_STREAM("Toggling Ground Truth visualization");
1660 
1661  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1662 
1663  if (m_visualize_GT)
1664  {
1665  CRenderizable::Ptr obj = scene->getByName("GT_cloud");
1666  obj->setVisibility(!obj->isVisible());
1667 
1668  obj = scene->getByName("robot_GT");
1669  obj->setVisibility(!obj->isVisible());
1670  }
1671  else
1672  {
1673  dumpVisibilityErrorMsg("visualize_ground_truth");
1674  }
1675 
1676  m_win->unlockAccess3DScene();
1677  m_win->forceRepaint();
1678 
1679  MRPT_END;
1680 }
1681 template <class GRAPH_T>
1683 {
1684  MRPT_START;
1685  ASSERTDEB_(m_enable_visuals);
1686  using namespace std;
1687  using namespace mrpt::opengl;
1688  MRPT_LOG_INFO_STREAM("Toggling Map visualization... ");
1689 
1690  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1691 
1692  // get total number of nodes
1693  int num_of_nodes;
1694  {
1695  std::lock_guard<std::mutex> graph_lock(m_graph_section);
1696  num_of_nodes = m_graph.nodeCount();
1697  }
1698 
1699  // name of gui object
1700  stringstream scan_name("");
1701 
1702  for (int node_cnt = 0; node_cnt != num_of_nodes; ++node_cnt)
1703  {
1704  // build the name of the potential corresponding object in the scene
1705  scan_name.str("");
1706  scan_name << "laser_scan_";
1707  scan_name << node_cnt;
1708 
1709  CRenderizable::Ptr obj = scene->getByName(scan_name.str());
1710  // current node may not have laserScans => may not have corresponding
1711  // obj
1712  if (obj)
1713  {
1714  obj->setVisibility(!obj->isVisible());
1715  }
1716  }
1717  m_win->unlockAccess3DScene();
1718  m_win->forceRepaint();
1719 
1720  MRPT_END;
1721 }
1722 template <class GRAPH_T>
1724 {
1725  MRPT_START;
1726  ASSERTDEB_(m_enable_visuals);
1727  using namespace mrpt::opengl;
1728  MRPT_LOG_INFO_STREAM("Toggling Estimated Trajectory visualization... ");
1729 
1730  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1731 
1732  if (m_visualize_estimated_trajectory)
1733  {
1734  CRenderizable::Ptr obj = scene->getByName("estimated_traj_setoflines");
1735  obj->setVisibility(!obj->isVisible());
1736 
1737  obj = scene->getByName("robot_estimated_traj");
1738  obj->setVisibility(!obj->isVisible());
1739  }
1740  else
1741  {
1742  dumpVisibilityErrorMsg("visualize_estimated_trajectory");
1743  }
1744 
1745  m_win->unlockAccess3DScene();
1746  m_win->forceRepaint();
1747 
1748  MRPT_END;
1749 }
1750 template <class GRAPH_T>
1752  std::string viz_flag, int sleep_time)
1753 {
1754  MRPT_START;
1755 
1757  "Cannot toggle visibility of specified object.\n"
1758  << "Make sure that the corresponding visualization flag (" << viz_flag
1759  << ") is set to true in the .ini file.");
1760  std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
1761 
1762  MRPT_END;
1763 }
1764 
1765 template <class GRAPH_T>
1767  const mrpt::obs::CActionCollection::Ptr action,
1768  const mrpt::obs::CSensoryFrame::Ptr observations,
1769  const mrpt::obs::CObservation::Ptr observation)
1770 {
1771  MRPT_START;
1772  using namespace mrpt::obs;
1773  using namespace mrpt::system;
1774 
1775  // make sure that adequate data is given.
1776  ASSERTDEBMSG_(
1777  action || observation,
1778  "Neither action or observation contains valid data.");
1779 
1781  if (observation)
1782  {
1783  timestamp = observation->timestamp;
1784  }
1785  else
1786  {
1787  // querry action part first
1788  timestamp = action->get(0)->timestamp;
1789 
1790  // if still not available query the observations in the CSensoryFrame
1791  if (timestamp == INVALID_TIMESTAMP)
1792  {
1793  for (auto sens_it = observations->begin();
1794  sens_it != observations->end(); ++sens_it)
1795  {
1796  timestamp = (*sens_it)->timestamp;
1797  if (timestamp != INVALID_TIMESTAMP)
1798  {
1799  break;
1800  }
1801  }
1802  }
1803  }
1804  return timestamp;
1805  MRPT_END;
1806 }
1807 
1808 template <class GRAPH_T>
1811  const mrpt::graphs::TNodeID nodeID) const
1812 {
1813  return mrpt::poses::CPose3D(m_graph.nodes.at(nodeID));
1814 }
1815 
1816 template <class GRAPH_T>
1818 {
1819  using namespace mrpt::opengl;
1820 
1821  CSetOfObjects::Ptr map_obj = mrpt::make_aligned_shared<CSetOfObjects>();
1822  map_obj->setName("map");
1823  COpenGLScene::Ptr& scene = this->m_win->get3DSceneAndLock();
1824  scene->insert(map_obj);
1825  this->m_win->unlockAccess3DScene();
1826  this->m_win->forceRepaint();
1827 }
1828 
1829 template <class GRAPH_T>
1831  const std::map<
1833  nodes_to_laser_scans2D,
1834  bool full_update)
1835 {
1836  MRPT_START;
1837  ASSERTDEB_(m_enable_visuals);
1838  using namespace mrpt::obs;
1839  using namespace mrpt::opengl;
1840  using namespace std;
1841  using namespace mrpt::poses;
1842  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
1843  CSetOfObjects::Ptr map_obj;
1844  {
1845  CRenderizable::Ptr obj = scene->getByName("map");
1846  map_obj = std::dynamic_pointer_cast<CSetOfObjects>(obj);
1847  ASSERTDEB_(map_obj);
1848  }
1849 
1850  mrpt::system::CTicTac map_update_timer;
1851  map_update_timer.Tic();
1852 
1853  // get the set of nodes for which to run the update
1854  std::set<mrpt::graphs::TNodeID> nodes_set;
1855  {
1856  if (full_update)
1857  {
1858  // for all the nodes get the node position and the corresponding
1859  // laser scan
1860  // if they were recorded and visualize them
1861  m_graph.getAllNodes(nodes_set);
1862  MRPT_LOG_DEBUG_STREAM("Executing full update of the map visuals");
1863  map_obj->clear();
1864 
1865  } // end if - full update
1866  else
1867  { // add only current nodeID
1868  nodes_set.insert(m_nodeID_max);
1869  } // end if - partial update
1870  }
1871 
1872  // for all the nodes in the previously populated set
1873  for (mrpt::graphs::TNodeID node_it : nodes_set)
1874  {
1875  // name of gui object
1876  stringstream scan_name("");
1877  scan_name << "laser_scan_";
1878  scan_name << node_it;
1879 
1880  // get the node laser scan
1881  CObservation2DRangeScan::Ptr scan_content;
1882  auto search = nodes_to_laser_scans2D.find(node_it);
1883 
1884  // make sure that the laser scan exists and is valid
1885  if (search != nodes_to_laser_scans2D.end() && search->second)
1886  {
1887  scan_content = search->second;
1888 
1889  CObservation2DRangeScan scan_decimated;
1890  this->decimateLaserScan(
1891  *scan_content, &scan_decimated,
1892  /*keep_every_n_entries = */ 5);
1893 
1894  // if the scan doesn't already exist, add it to the map object,
1895  // otherwise just
1896  // adjust its pose
1897  CRenderizable::Ptr obj = map_obj->getByName(scan_name.str());
1898  CSetOfObjects::Ptr scan_obj =
1899  std::dynamic_pointer_cast<CSetOfObjects>(obj);
1900  if (!scan_obj)
1901  {
1902  scan_obj = mrpt::make_aligned_shared<CSetOfObjects>();
1903 
1904  // creating and inserting the observation in the CSetOfObjects
1906  m.insertObservation(&scan_decimated);
1907  m.getAs3DObject(scan_obj);
1908 
1909  scan_obj->setName(scan_name.str());
1910  this->setObjectPropsFromNodeID(node_it, scan_obj);
1911 
1912  // set the visibility of the object the same value as the
1913  // visibility of
1914  // the previous - Needed for proper toggling of the visibility
1915  // of the
1916  // whole map
1917  {
1918  stringstream prev_scan_name("");
1919  prev_scan_name << "laser_scan_" << node_it - 1;
1920  CRenderizable::Ptr prev_obj =
1921  map_obj->getByName(prev_scan_name.str());
1922  if (prev_obj)
1923  {
1924  scan_obj->setVisibility(prev_obj->isVisible());
1925  }
1926  }
1927 
1928  map_obj->insert(scan_obj);
1929  }
1930  else
1931  {
1932  scan_obj = std::dynamic_pointer_cast<CSetOfObjects>(scan_obj);
1933  }
1934 
1935  // finally set the pose correctly - as computed by graphSLAM
1936  const CPose3D& scan_pose = CPose3D(m_graph.nodes.at(node_it));
1937  scan_obj->setPose(scan_pose);
1938  }
1939  else
1940  {
1942  "Laser scans of NodeID " << node_it << "are empty/invalid");
1943  }
1944 
1945  } // end for set of nodes
1946 
1947  m_win->unlockAccess3DScene();
1948  m_win->forceRepaint();
1949 
1950  double elapsed_time = map_update_timer.Tac();
1952  "updateMapVisualization took: " << elapsed_time << "s");
1953  MRPT_END;
1954 } // end of updateMapVisualization
1955 
1956 template <class GRAPH_T>
1958  const mrpt::graphs::TNodeID nodeID,
1960 {
1961  MRPT_START;
1962  viz_object->setColor_u8(m_optimized_map_color);
1963  MRPT_END;
1964 }
1965 
1966 template <class GRAPH_T>
1968  mrpt::obs::CObservation2DRangeScan& laser_scan_in,
1969  mrpt::obs::CObservation2DRangeScan* laser_scan_out,
1970  const int keep_every_n_entries /*= 2*/)
1971 {
1972  MRPT_START;
1973 
1974  size_t scan_size = laser_scan_in.scan.size();
1975 
1976  // assign the decimated scans, ranges
1977  std::vector<float> new_scan(
1978  scan_size); // Was [], but can't use non-constant argument with arrays
1979  std::vector<char> new_validRange(scan_size);
1980  size_t new_scan_size = 0;
1981  for (size_t i = 0; i != scan_size; i++)
1982  {
1983  if (i % keep_every_n_entries == 0)
1984  {
1985  new_scan[new_scan_size] = laser_scan_in.scan[i];
1986  new_validRange[new_scan_size] = laser_scan_in.validRange[i];
1987  new_scan_size++;
1988  }
1989  }
1990  laser_scan_out->loadFromVectors(
1991  new_scan_size, &new_scan[0], &new_validRange[0]);
1992 
1993  MRPT_END;
1994 }
1995 
1996 template <class GRAPH_T>
1998 {
1999  MRPT_START;
2000  ASSERTDEB_(m_enable_visuals);
2001  using namespace mrpt::opengl;
2002 
2003  // assertions
2004  ASSERTDEB_(m_has_read_config);
2005  ASSERTDEB_(
2006  m_win &&
2007  "Visualization of data was requested but no CDisplayWindow3D pointer "
2008  "was provided");
2009 
2010  // point cloud
2011  CPointCloud::Ptr GT_cloud = mrpt::make_aligned_shared<CPointCloud>();
2012  GT_cloud->setPointSize(1.0);
2013  GT_cloud->enablePointSmooth();
2014  GT_cloud->enableColorFromX(false);
2015  GT_cloud->enableColorFromY(false);
2016  GT_cloud->enableColorFromZ(false);
2017  GT_cloud->setColor_u8(m_GT_color);
2018  GT_cloud->setName("GT_cloud");
2019 
2020  // robot model
2021  CSetOfObjects::Ptr robot_model = this->setCurrentPositionModel(
2022  "robot_GT", m_GT_color, m_robot_model_size);
2023 
2024  // insert them to the scene
2025  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
2026  scene->insert(GT_cloud);
2027  scene->insert(robot_model);
2028  m_win->unlockAccess3DScene();
2029 
2030  m_win_manager->assignTextMessageParameters(
2031  &m_offset_y_GT, &m_text_index_GT);
2032  m_win_manager->addTextMessage(
2033  m_offset_x_left, -m_offset_y_GT, mrpt::format("Ground truth path"),
2034  mrpt::img::TColorf(m_GT_color), m_text_index_GT);
2035  m_win->forceRepaint();
2036 
2037  MRPT_END;
2038 } // end of initGTVisualization
2039 
2040 template <class GRAPH_T>
2042 {
2043  MRPT_START;
2044  ASSERTDEB_(m_enable_visuals);
2045  using namespace mrpt::opengl;
2046 
2047  // add to the GT PointCloud and visualize it
2048  // check that GT vector is not depleted
2049  if (m_visualize_GT && m_GT_poses_index < m_GT_poses.size())
2050  {
2051  ASSERTDEB_(
2052  m_win &&
2053  "Visualization of data was requested but no CDisplayWindow3D "
2054  "pointer was given");
2055 
2056  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
2057 
2058  CRenderizable::Ptr obj = scene->getByName("GT_cloud");
2059  CPointCloud::Ptr GT_cloud = std::dynamic_pointer_cast<CPointCloud>(obj);
2060 
2061  // add the latest GT pose
2062  mrpt::poses::CPose3D p(m_GT_poses[m_GT_poses_index]);
2063  GT_cloud->insertPoint(p.x(), p.y(), p.z());
2064 
2065  // robot model of GT trajectory
2066  obj = scene->getByName("robot_GT");
2067  CSetOfObjects::Ptr robot_obj =
2068  std::dynamic_pointer_cast<CSetOfObjects>(obj);
2069  robot_obj->setPose(p);
2070  m_win->unlockAccess3DScene();
2071  m_win->forceRepaint();
2072  }
2073 
2074  MRPT_END;
2075 } // end of updateGTVisualization
2076 
2077 template <class GRAPH_T>
2079 {
2080  MRPT_START;
2081  ASSERTDEB_(m_has_read_config);
2082  ASSERTDEB_(m_enable_visuals);
2083  using namespace mrpt::opengl;
2084 
2085  // point cloud
2086  CPointCloud::Ptr odometry_poses_cloud =
2087  mrpt::make_aligned_shared<CPointCloud>();
2088  odometry_poses_cloud->setPointSize(1.0);
2089  odometry_poses_cloud->enablePointSmooth();
2090  odometry_poses_cloud->enableColorFromX(false);
2091  odometry_poses_cloud->enableColorFromY(false);
2092  odometry_poses_cloud->enableColorFromZ(false);
2093  odometry_poses_cloud->setColor_u8(m_odometry_color);
2094  odometry_poses_cloud->setName("odometry_poses_cloud");
2095 
2096  // robot model
2097  CSetOfObjects::Ptr robot_model = this->setCurrentPositionModel(
2098  "robot_odometry_poses", m_odometry_color, m_robot_model_size);
2099 
2100  // insert them to the scene
2101  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
2102  scene->insert(odometry_poses_cloud);
2103  scene->insert(robot_model);
2104  m_win->unlockAccess3DScene();
2105 
2106  m_win_manager->assignTextMessageParameters(
2107  &m_offset_y_odometry, &m_text_index_odometry);
2108  m_win_manager->addTextMessage(
2109  m_offset_x_left, -m_offset_y_odometry, mrpt::format("Odometry path"),
2110  mrpt::img::TColorf(m_odometry_color), m_text_index_odometry);
2111 
2112  m_win->forceRepaint();
2113 
2114  MRPT_END;
2115 }
2116 
2117 template <class GRAPH_T>
2119 {
2120  MRPT_START;
2121  ASSERTDEB_(m_enable_visuals);
2122  ASSERTDEBMSG_(
2123  m_win,
2124  "Visualization of data was requested but no CDisplayWindow3D pointer "
2125  "was given");
2126  using namespace mrpt::opengl;
2127 
2128  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
2129 
2130  // point cloud
2131  CRenderizable::Ptr obj = scene->getByName("odometry_poses_cloud");
2132  CPointCloud::Ptr odometry_poses_cloud =
2133  std::dynamic_pointer_cast<CPointCloud>(obj);
2134  mrpt::poses::CPose3D p(m_odometry_poses.back());
2135 
2136  odometry_poses_cloud->insertPoint(p.x(), p.y(), p.z());
2137 
2138  // robot model
2139  obj = scene->getByName("robot_odometry_poses");
2140  CSetOfObjects::Ptr robot_obj =
2141  std::dynamic_pointer_cast<CSetOfObjects>(obj);
2142  robot_obj->setPose(p);
2143 
2144  m_win->unlockAccess3DScene();
2145  m_win->forceRepaint();
2146 
2147  MRPT_END;
2148 }
2149 
2150 template <class GRAPH_T>
2152 {
2153  MRPT_START;
2154  ASSERTDEB_(m_enable_visuals);
2155  using namespace mrpt::opengl;
2156 
2157  // SetOfLines
2158  CSetOfLines::Ptr estimated_traj_setoflines =
2159  mrpt::make_aligned_shared<CSetOfLines>();
2160  estimated_traj_setoflines->setColor_u8(m_estimated_traj_color);
2161  estimated_traj_setoflines->setLineWidth(1.5);
2162  estimated_traj_setoflines->setName("estimated_traj_setoflines");
2163  // append a dummy line so that you can later use append using
2164  // CSetOfLines::appendLienStrip method.
2165  estimated_traj_setoflines->appendLine(
2166  /* 1st */ 0, 0, 0,
2167  /* 2nd */ 0, 0, 0);
2168 
2169  // robot model
2170  // pose_t initial_pose;
2171  CSetOfObjects::Ptr robot_model = this->setCurrentPositionModel(
2172  "robot_estimated_traj", m_estimated_traj_color, m_robot_model_size);
2173 
2174  // insert objects in the graph
2175  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
2176  if (m_visualize_estimated_trajectory)
2177  {
2178  scene->insert(estimated_traj_setoflines);
2179  }
2180  scene->insert(robot_model);
2181  m_win->unlockAccess3DScene();
2182 
2183  if (m_visualize_estimated_trajectory)
2184  {
2185  m_win_manager->assignTextMessageParameters(
2186  &m_offset_y_estimated_traj, &m_text_index_estimated_traj);
2187  m_win_manager->addTextMessage(
2188  m_offset_x_left, -m_offset_y_estimated_traj,
2189  mrpt::format("Estimated trajectory"),
2190  mrpt::img::TColorf(m_estimated_traj_color),
2191  m_text_index_estimated_traj);
2192  }
2193 
2194  m_win->forceRepaint();
2195 
2196  MRPT_END;
2197 }
2198 
2199 template <class GRAPH_T>
2201  bool full_update)
2202 {
2203  MRPT_START;
2204  ASSERTDEB_(m_enable_visuals);
2205  using namespace mrpt::opengl;
2206 
2207  std::lock_guard<std::mutex> graph_lock(m_graph_section);
2208  ASSERTDEB_(m_graph.nodeCount() != 0);
2209 
2210  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
2211 
2213  if (m_visualize_estimated_trajectory)
2214  {
2215  // set of lines
2216  obj = scene->getByName("estimated_traj_setoflines");
2217  CSetOfLines::Ptr estimated_traj_setoflines =
2218  std::dynamic_pointer_cast<CSetOfLines>(obj);
2219 
2220  // gather set of nodes for which to append lines - all of the nodes in
2221  // the graph or just the last inserted..
2222  std::set<mrpt::graphs::TNodeID> nodes_set;
2223  {
2224  if (full_update)
2225  {
2226  this->getNodeIDsOfEstimatedTrajectory(&nodes_set);
2227  estimated_traj_setoflines->clear();
2228  // dummy way so that I can use appendLineStrip afterwards.
2229  estimated_traj_setoflines->appendLine(
2230  /* 1st */ 0, 0, 0,
2231  /* 2nd */ 0, 0, 0);
2232  }
2233  else
2234  {
2235  nodes_set.insert(m_graph.nodeCount() - 1);
2236  }
2237  }
2238  // append line for each node in the set
2239  for (unsigned long it : nodes_set)
2240  {
2241  mrpt::poses::CPose3D p(m_graph.nodes.at(it));
2242 
2243  estimated_traj_setoflines->appendLineStrip(p.x(), p.y(), p.z());
2244  }
2245  }
2246 
2247  // robot model
2248  // set the robot position to the last recorded pose in the graph
2249  obj = scene->getByName("robot_estimated_traj");
2250  CSetOfObjects::Ptr robot_obj =
2251  std::dynamic_pointer_cast<CSetOfObjects>(obj);
2252  pose_t curr_estimated_pos = m_graph.nodes[m_graph.nodeCount() - 1];
2253  robot_obj->setPose(curr_estimated_pos);
2254 
2255  m_win->unlockAccess3DScene();
2256  m_win->forceRepaint();
2257  MRPT_END;
2258 } // end of updateEstimatedTrajectoryVisualization
2259 
2260 template <class GRAPH_T>
2262  const std::string& rawlog_fname)
2263 {
2264  this->setRawlogFile(rawlog_fname);
2265  this->initTRGBDInfoFileParams();
2266 }
2267 template <class GRAPH_T>
2269 {
2270  this->initTRGBDInfoFileParams();
2271 }
2272 
2273 template <class GRAPH_T>
2275  const std::string& rawlog_fname)
2276 {
2277  // get the correct info filename from the rawlog_fname
2279  std::string rawlog_filename = mrpt::system::extractFileName(rawlog_fname);
2280  std::string name_prefix = "rawlog_";
2281  std::string name_suffix = "_info.txt";
2282  info_fname = dir + name_prefix + rawlog_filename + name_suffix;
2283 }
2284 
2285 template <class GRAPH_T>
2287 {
2288  // fields to use
2289  fields["Overall number of objects"] = "";
2290  fields["Observations format"] = "";
2291 }
2292 
2293 template <class GRAPH_T>
2295 {
2296  ASSERT_FILE_EXISTS_(info_fname);
2297  using namespace std;
2298 
2299  // open file
2300  mrpt::io::CFileInputStream info_file(info_fname);
2301  ASSERTDEBMSG_(
2302  info_file.fileOpenCorrectly(),
2303  "\nTRGBDInfoFileParams::parseFile: Couldn't open info file\n");
2304 
2305  string curr_line;
2306  size_t line_cnt = 0;
2307 
2308  // parse until you find an empty line.
2309  while (true)
2310  {
2311  info_file.readLine(curr_line);
2312  line_cnt++;
2313  if (curr_line.size() == 0) break;
2314  }
2315 
2316  // parse the meaningful data
2317  while (info_file.readLine(curr_line))
2318  {
2319  // split current line at ":"
2320  vector<string> curr_tokens;
2321  mrpt::system::tokenize(curr_line, ":", curr_tokens);
2322 
2323  ASSERTDEB_EQUAL_(curr_tokens.size(), 2);
2324 
2325  // evaluate the name. if name in info struct then fill the corresponding
2326  // info struct parameter with the value_part in the file.
2327  std::string literal_part = mrpt::system::trim(curr_tokens[0]);
2328  std::string value_part = mrpt::system::trim(curr_tokens[1]);
2329 
2330  for (auto it = fields.begin(); it != fields.end(); ++it)
2331  {
2332  if (mrpt::system::strCmpI(it->first, literal_part))
2333  {
2334  it->second = value_part;
2335  }
2336  }
2337 
2338  line_cnt++;
2339  }
2340 }
2341 
2342 template <class GRAPH_T>
2344 {
2345  MRPT_START;
2346 
2347  // what's the name of the file to be saved?
2348  std::string fname;
2349  if (fname_in)
2350  {
2351  fname = *fname_in;
2352  }
2353  else
2354  {
2355  fname = "output_graph.graph";
2356  }
2357 
2359  "Saving generated graph in VERTEX/EDGE format: " << fname);
2360  m_graph.saveToTextFile(fname);
2361 
2362  MRPT_END;
2363 }
2364 
2365 template <class GRAPH_T>
2367 {
2368  MRPT_START;
2369  ASSERTDEBMSG_(
2370  m_enable_visuals,
2371  "\nsave3DScene was called even though enable_visuals flag is "
2372  "off.\nExiting...\n");
2373  using namespace mrpt::opengl;
2374 
2375  COpenGLScene::Ptr scene = m_win->get3DSceneAndLock();
2376 
2377  if (!scene)
2378  {
2380  "Could not fetch 3D Scene. Display window must already be closed.");
2381  return;
2382  }
2383 
2384  // TODO - what else is there to be excluded from the scene?
2385  // remove the CPlanarLaserScan if it exists
2386  {
2387  CPlanarLaserScan::Ptr laser_scan;
2388  for (; (laser_scan = scene->getByClass<CPlanarLaserScan>());)
2389  {
2391  "Removing CPlanarLaserScan from generated 3DScene...");
2392  scene->removeObject(laser_scan);
2393  }
2394  }
2395 
2396  // what's the name of the file to be saved?
2397  std::string fname;
2398  if (fname_in)
2399  {
2400  fname = *fname_in;
2401  }
2402  else
2403  {
2404  fname = "output_scene.3DScene";
2405  }
2406 
2407  MRPT_LOG_INFO_STREAM("Saving generated scene to " << fname);
2408  scene->saveToFile(fname);
2409 
2410  m_win->unlockAccess3DScene();
2411  m_win->forceRepaint();
2412 
2413  MRPT_END;
2414 }
2415 
2416 template <class GRAPH_T>
2418  mrpt::graphs::TNodeID nodeID, size_t gt_index)
2419 {
2420  MRPT_START;
2421  using namespace mrpt::math;
2422  using namespace mrpt::poses;
2423 
2424  // start updating the metric after a certain number of nodes have been added
2425  if (m_graph.nodeCount() < 4)
2426  {
2427  return;
2428  }
2429 
2430  // add to the map - keep track of which gt index corresponds to which nodeID
2431  m_nodeID_to_gt_indices[nodeID] = gt_index;
2432 
2433  // initialize the loop variables only once
2434  pose_t curr_node_pos;
2435  pose_t curr_gt_pos;
2436  pose_t node_delta_pos;
2437  pose_t gt_delta;
2438  double trans_diff = 0;
2439  double rot_diff = 0;
2440 
2441  size_t indices_size = m_nodeID_to_gt_indices.size();
2442 
2443  // recompute the metric from scratch
2444  m_curr_deformation_energy = 0;
2445 
2446  // first element of map
2447  auto start_it = m_nodeID_to_gt_indices.begin();
2448  start_it++;
2449 
2450  // fetch the first node, gt positions separately
2451  auto prev_it = start_it;
2452  prev_it--;
2453  pose_t prev_node_pos = m_graph.nodes[prev_it->first];
2454  pose_t prev_gt_pos = m_GT_poses[prev_it->second];
2455 
2456  // temporary constraint type
2457  constraint_t c;
2458 
2459  for (auto index_it = start_it; index_it != m_nodeID_to_gt_indices.end();
2460  index_it++)
2461  {
2462  curr_node_pos = m_graph.nodes[index_it->first];
2463  curr_gt_pos = m_GT_poses[index_it->second];
2464 
2465  node_delta_pos = curr_node_pos - prev_node_pos;
2466  gt_delta = curr_gt_pos - prev_gt_pos;
2467 
2468  trans_diff = gt_delta.distanceTo(node_delta_pos);
2469  rot_diff = this->accumulateAngleDiffs(gt_delta, node_delta_pos);
2470 
2471  m_curr_deformation_energy += (pow(trans_diff, 2) + pow(rot_diff, 2));
2472  m_curr_deformation_energy /= indices_size;
2473 
2474  // add it to the overall vector
2475  m_deformation_energy_vec.push_back(m_curr_deformation_energy);
2476 
2477  prev_node_pos = curr_node_pos;
2478  prev_gt_pos = curr_gt_pos;
2479  }
2480 
2482  "Total deformation energy: " << m_curr_deformation_energy);
2483 
2484  MRPT_END;
2485 }
2486 
2487 template <class GRAPH_T>
2489  const mrpt::poses::CPose2D& p1, const mrpt::poses::CPose2D& p2)
2490 {
2491  return mrpt::math::wrapToPi(p1.phi() - p2.phi());
2492 }
2493 template <class GRAPH_T>
2495  const mrpt::poses::CPose3D& p1, const mrpt::poses::CPose3D& p2)
2496 {
2497  using mrpt::math::wrapToPi;
2498  double res = 0;
2499 
2500  res += wrapToPi(p1.roll() - p2.roll());
2501  res += wrapToPi(p1.pitch() - p2.pitch());
2502  res += wrapToPi(p1.yaw() - p2.yaw());
2503 
2504  return res;
2505 }
2506 
2507 template <class GRAPH_T>
2509 {
2510  ASSERTDEB_(m_enable_visuals);
2511 
2512  MRPT_START;
2513  MRPT_LOG_DEBUG_STREAM("In initializeSlamMetricVisualization...");
2514  ASSERTDEB_(m_visualize_SLAM_metric);
2515 
2516  // initialize the m_win_plot on the stack
2517  m_win_plot = std::make_unique<mrpt::gui::CDisplayWindowPlots>(
2518  "Evolution of SLAM metric - Deformation Energy (1:1000)", 400, 300);
2519 
2520  m_win_plot->setPos(20, 50);
2521  m_win_plot->clf();
2522  // just plot the deformation points from scratch every time
2523  m_win_plot->hold_off();
2524  m_win_plot->enableMousePanZoom(true);
2525 
2526  MRPT_END;
2527 }
2528 
2529 template <class GRAPH_T>
2531 {
2532  MRPT_START;
2533  ASSERTDEB_(m_enable_visuals);
2534  ASSERTDEB_(m_win_plot && m_visualize_SLAM_metric);
2535 
2536  // build the X, Y vectors for plotting - use log scale
2537  std::vector<double> x(m_deformation_energy_vec.size(), 0);
2538  std::vector<double> y(m_deformation_energy_vec.size(), 0);
2539  for (size_t i = 0; i != x.size(); i++)
2540  {
2541  x[i] = i;
2542  y[i] = m_deformation_energy_vec[i] * 1000;
2543  }
2544 
2545  m_win_plot->plot(x, y, "r-1", "Deformation Energy (x1000)");
2546 
2547  // set the limits so that he y-values can be monitored
2548  // set the xmin limit with respect to xmax, which is constantly growing
2550  xmax = std::max_element(x.begin(), x.end());
2551  ymax = std::max_element(y.begin(), y.end());
2552 
2553  m_win_plot->axis(
2554  /*x_min = */ xmax != x.end() ? -(*xmax / 12) : -1,
2555  /*x_max = */ (xmax != x.end() ? *xmax : 1),
2556  /*y_min = */ -0.4f,
2557  /*y_max = */ (ymax != y.end() ? *ymax : 1));
2558 
2559  MRPT_END;
2560 }
2561 
2562 template <class GRAPH_T>
2564  std::string* report_str) const
2565 {
2566  MRPT_START;
2567  using namespace std;
2568 
2569  // Summary of Results
2570  stringstream results_ss;
2571  results_ss << "Summary: " << std::endl;
2572  results_ss << this->header_sep << std::endl;
2573  results_ss << "\tProcessing time: "
2574  << m_time_logger.getMeanTime("proc_time") << std::endl;
2575  ;
2576  results_ss << "\tDataset Grab time: " << m_dataset_grab_time << std::endl;
2577  results_ss << "\tReal-time capable: "
2578  << (m_time_logger.getMeanTime("proc_time") < m_dataset_grab_time
2579  ? "TRUE"
2580  : "FALSE")
2581  << std::endl;
2582  results_ss << m_edge_counter.getAsString();
2583  results_ss << "\tNum of Nodes: " << m_graph.nodeCount() << std::endl;
2584  ;
2585 
2586  // Class configuration parameters
2587  std::string config_params = this->getParamsAsString();
2588 
2589  // time and output logging
2590  const std::string time_res = m_time_logger.getStatsAsText();
2591  const std::string output_res = this->getLogAsString();
2592 
2593  // merge the individual reports
2594  report_str->clear();
2595 
2596  *report_str += results_ss.str();
2597  *report_str += this->report_sep;
2598 
2599  *report_str += config_params;
2600  *report_str += this->report_sep;
2601 
2602  *report_str += time_res;
2603  *report_str += this->report_sep;
2604 
2605  *report_str += output_res;
2606  *report_str += this->report_sep;
2607 
2608  MRPT_END;
2609 }
2610 
2611 template <class GRAPH_T>
2613  std::map<std::string, int>* node_stats,
2614  std::map<std::string, int>* edge_stats, mrpt::system::TTimeStamp* timestamp)
2615 {
2616  MRPT_START;
2617  using namespace std;
2618  using namespace mrpt::graphslam::detail;
2619 
2620  ASSERTDEBMSG_(node_stats, "Invalid pointer to node_stats is given");
2621  ASSERTDEBMSG_(edge_stats, "Invalid pointer to edge_stats is given");
2622 
2623  if (m_nodeID_max == 0)
2624  {
2625  return false;
2626  }
2627 
2628  // fill the node stats
2629  (*node_stats)["nodes_total"] = m_nodeID_max + 1;
2630 
2631  // fill the edge stats
2632  for (auto it = m_edge_counter.cbegin(); it != m_edge_counter.cend(); ++it)
2633  {
2634  (*edge_stats)[it->first] = it->second;
2635  }
2636 
2637  (*edge_stats)["loop_closures"] = m_edge_counter.getLoopClosureEdges();
2638  (*edge_stats)["edges_total"] = m_edge_counter.getTotalNumOfEdges();
2639 
2640  // fill the timestamp
2641  if (timestamp)
2642  {
2643  *timestamp = m_curr_timestamp;
2644  }
2645 
2646  return true;
2647  MRPT_END;
2648 }
2649 
2650 template <class GRAPH_T>
2652  const std::string& output_dir_fname)
2653 {
2654  MRPT_START;
2655  using namespace mrpt::system;
2656  using namespace mrpt;
2657 
2658  ASSERTDEBMSG_(
2659  directoryExists(output_dir_fname),
2660  format(
2661  "Output directory \"%s\" doesn't exist", output_dir_fname.c_str()));
2662 
2663  MRPT_LOG_INFO_STREAM("Generating detailed class report...");
2664  std::lock_guard<std::mutex> graph_lock(m_graph_section);
2665 
2666  std::string report_str;
2667  std::string fname;
2668  const std::string ext = ".log";
2669 
2670  { // CGraphSlamEngine
2671  report_str.clear();
2672  fname = output_dir_fname + "/" + m_class_name + ext;
2673  // initialize the output file - refer to the stream through the
2674  // m_out_streams map
2675  this->initResultsFile(fname);
2676  this->getDescriptiveReport(&report_str);
2677  m_out_streams[fname].printf("%s", report_str.c_str());
2678 
2679  // write the timings into a separate file
2680  const std::string time_res = m_time_logger.getStatsAsText();
2681  fname = output_dir_fname + "/" + "timings" + ext;
2682  this->initResultsFile(fname);
2683  m_out_streams[fname].printf("%s", time_res.c_str());
2684  }
2685  { // node_registrar
2686  report_str.clear();
2687  fname = output_dir_fname + "/" + "node_registrar" + ext;
2688  this->initResultsFile(fname);
2689  m_node_reg->getDescriptiveReport(&report_str);
2690  m_out_streams[fname].printf("%s", report_str.c_str());
2691  }
2692  { // edge_registrar
2693  report_str.clear();
2694  fname = output_dir_fname + "/" + "edge_registrar" + ext;
2695  this->initResultsFile(fname);
2696  m_edge_reg->getDescriptiveReport(&report_str);
2697  m_out_streams[fname].printf("%s", report_str.c_str());
2698  }
2699  { // optimizer
2700  report_str.clear();
2701  fname = output_dir_fname + "/" + "optimizer" + ext;
2702  this->initResultsFile(fname);
2703  m_optimizer->getDescriptiveReport(&report_str);
2704  m_out_streams[fname].printf("%s", report_str.c_str());
2705  }
2706 
2707  if (m_use_GT)
2708  { // slam evaluation metric
2709  report_str.clear();
2710  const std::string desc(
2711  "# File includes the evolution of the SLAM metric. Implemented "
2712  "metric computes the \"deformation energy\" that is needed to "
2713  "transfer the estimated trajectory into the ground-truth "
2714  "trajectory (computed as sum of the difference between estimated "
2715  "trajectory and ground truth consecutive poses See \"A comparison "
2716  "of SLAM algorithms based on a graph of relations - W.Burgard et "
2717  "al.\", for more details on the metric.\n");
2718 
2719  fname = output_dir_fname + "/" + "SLAM_evaluation_metric" + ext;
2720  this->initResultsFile(fname);
2721 
2722  m_out_streams[fname].printf("%s\n", desc.c_str());
2723  for (auto vec_it = m_deformation_energy_vec.begin();
2724  vec_it != m_deformation_energy_vec.end(); ++vec_it)
2725  {
2726  m_out_streams[fname].printf("%f\n", *vec_it);
2727  }
2728  }
2729 
2730  MRPT_END;
2731 }
2732 template <class GRAPH_T>
2735  const std::string& model_name, const mrpt::img::TColor& model_color,
2736  const size_t model_size, const pose_t& init_pose)
2737 {
2739  this->initRobotModelVisualization();
2740  model->setName(model_name);
2741  model->setColor_u8(model_color);
2742  model->setScale(model_size);
2743  model->setPose(init_pose);
2744 
2745  return model;
2746 }
2747 
2748 template <class GRAPH_T>
2750  std::vector<double>* vec_out) const
2751 {
2752  *vec_out = m_deformation_energy_vec;
2753 }
2754 } // 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:86
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:282
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:51
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:213
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:108
void computeMap() const
Compute the map of the environment based on the recorded measurements.
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
Definition: CMetricMap.cpp:94
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.
double DEG2RAD(const double x)
Degrees to radians.
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.
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
global_pose_t getCurrentRobotPosEstimation() const
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:45
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:529
Interface for implementing edge registration classes.
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:523
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:239
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
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4199
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.
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< char > > validRange
It&#39;s false (=0) on no reflected rays, referenced to elements in scan.
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 coordinate of the quaternion.
Definition: CQuaternion.h:85
typename GRAPH_T::constraint_t::type_value pose_t
Type of underlying poses (2D/3D).
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< float > > scan
The range values of the scan, in meters.
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.
const GLubyte * c
Definition: glext.h:6406
_u8 model
Definition: rplidar_cmd.h:19
#define ASSERTDEB_EQUAL_(__A, __B)
Definition: exceptions.h:233
GLint GLvoid * img
Definition: glext.h:3769
bool readLine(std::string &str)
Reads one string line from the file (until a new-line character)
static uint64_t getCurrentTime()
Definition: Clock.cpp:22
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...
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);
GLsizei const GLchar ** string
Definition: glext.h:4116
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:535
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:432
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:232
void printParams() const
Print the problem parameters to the console for verification.
auto dir
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
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
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:38
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:82
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:148
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:231
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.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:102
#define MRPT_END
Definition: exceptions.h:286
A RGB color - floats in the range [0,1].
Definition: TColor.h:77
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:15
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)
GLenum GLint GLint y
Definition: glext.h:3542
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
GLuint res
Definition: glext.h:7385
A RGB color - 8bit.
Definition: TColor.h:20
An observation of the current (cumulative) odometry for a wheeled robot.
GLenum GLint x
Definition: glext.h:3542
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:75
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:21
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:31
#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)
GLfloat GLfloat p
Definition: glext.h:6398
static void setImagesPathBase(const std::string &path)
Definition: CImage.cpp:83
const Scalar * const_iterator
Definition: eigen_plugins.h:27
Internal auxiliary classes.
Definition: levmarq_impl.h:18
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...
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147
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:43
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:82



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019