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



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018