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