Main MRPT website > C++ reference for MRPT 1.5.7
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>
17 template<class GRAPH_T>
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
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.
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
void queryObserverForEvents()
Query the observer instance for any user events.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
void parseFile()
Parse the RGBD information file to gain information about the rawlog file contents.
void save3DScene(const std::string *fname_in=NULL) const
Wrapper method around the COpenGLScene::saveToFile method.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
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 computeMap() const
Compute the map of the environment based on the recorded measurements.
void computeSlamMetric(mrpt::utils::TNodeID nodeID, size_t gt_index)
Compare the SLAM result (estimated trajectory) with the GT path.
virtual void getNodeIDsOfEstimatedTrajectory(std::set< mrpt::utils::TNodeID > *nodes_set) const
Return the list of nodeIDs which make up robot trajectory.
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.
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:77
Interface for implementing node registration classes.
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
#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.
Scalar * iterator
Definition: eigen_plugins.h:23
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.
global_pose_t getCurrentRobotPosEstimation() const
Query CGraphSlamEngine instance for the current estimated robot position.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
#define INVALID_NODEID
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
Interface for implementing edge registration classes.
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
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
STL namespace.
std::string BASE_IMPEXP fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores (&#39;_&#39;) or any other user-given char. ...
Definition: filesystem.cpp:315
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.
const_iterator find(const KEY &key) const
Definition: ts_hash_map.h:139
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:77
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
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.
mrpt::system::TTimeStamp BASE_IMPEXP getCurrentLocalTime()
Returns the current (local) time.
Definition: datetime.cpp:173
virtual void getRobotEstimatedTrajectory(typename GRAPH_T::global_poses_t *graph_poses) const
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void updateAllVisuals()
Wrapper around the deciders/optimizer updateVisuals methods.
GRAPH_T::constraint_t constraint_t
Type of graph constraints.
static double accumulateAngleDiffs(const mrpt::poses::CPose2D &p1, const mrpt::poses::CPose2D &p2)
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
static const std::string report_sep
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...
T r() const
Return r coordinate of the quaternion.
Definition: CQuaternion.h:75
GRAPH_T::constraint_t::type_value pose_t
Type of underlying poses (2D/3D).
void close()
Close the stream.
uint64_t TNodeID
The type for node IDs in graphs of different types.
#define MRPT_END
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.
const GLubyte * c
Definition: glext.h:5590
_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
GLint GLvoid * img
Definition: glext.h:3645
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
A RGB color - 8bit.
Definition: TColor.h:26
This namespace contains representation of robot actions and observations.
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&#39;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...
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
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei const GLchar ** string
Definition: glext.h:3919
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.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the class state.
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
static const std::string header_sep
Separator string to be used in debugging messages.
#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.
static COccupancyGridMap2DPtr Create()
void saveGraph(const std::string *fname_in=NULL) const
Wrapper method around the GRAPH_T::saveToTextFile method.
mrpt::opengl::CSetOfObjectsPtr initRobotModelVisualization()
bool readLine(std::string &str)
Reads one string line from the file (until a new-line character)
#define MRPT_START
void printParams() const
Print the problem parameters to the console for verification.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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.
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:76
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
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
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
std::map< std::string, int >::const_iterator const_iterator
Definition: CEdgeCounter.h:35
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::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.
void getDeformationEnergyVector(std::vector< double > *vec_out) const
Fill the given vector with the deformation energy values computed for the SLAM evaluation metric...
The namespace for 3D scene representation and rendering.
double Tac()
Stops the stopwatch.
Definition: CTicTac.cpp:92
#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)
GLenum GLint GLint y
Definition: glext.h:3516
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.
virtual mrpt::poses::CPose3D getLSPoseForGridMapVisualization(const mrpt::utils::TNodeID nodeID) const
return the 3D Pose of a LaserScan that is to be visualized.
Classes for creating GUI windows for 2D and 3D visualization.
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.
std::string getParamsAsString() const
Wrapper around getParamsAsString.
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. ...
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
GLuint res
Definition: glext.h:6298
An observation of the current (cumulative) odometry for a wheeled robot.
GLenum GLint x
Definition: glext.h:3516
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:43
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
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:78
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)
GLfloat GLfloat p
Definition: glext.h:5587
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
Internal auxiliary classes.
Definition: levmarq_impl.h:23
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
Wrapper method that used for printing error messages in a consistent manner.
void rpy(T &roll, T &pitch, T &yaw) const
Return the yaw, pitch & roll angles associated to quaternion.
Definition: CQuaternion.h:317
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...
void getMap(mrpt::maps::COccupancyGridMap2DPtr map, mrpt::system::TTimeStamp *acquisition_time=NULL) const
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.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019