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



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020