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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019