Main MRPT website > C++ reference for MRPT 1.9.9
CGraphSlamEngine.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #ifndef GRAPHSLAMENGINE_H
11 #define GRAPHSLAMENGINE_H
12 
13 #include <mrpt/maps/CSimpleMap.h>
15 #include <mrpt/maps/COctoMap.h>
20 #include <mrpt/obs/CAction.h>
23 #include <mrpt/obs/CSensoryFrame.h>
24 #include <mrpt/obs/obs_utils.h>
25 #include <mrpt/poses/CPose2D.h>
26 #include <mrpt/poses/CPose3D.h>
28 #include <mrpt/img/TColor.h>
30 
35 
36 #include <string>
37 #include <map>
38 #include <set>
39 
40 namespace mrpt
41 {
42 namespace graphslam
43 {
44 /**\brief Main file for the GraphSlamEngine.
45  *
46  * ## Description
47  *
48  * Given a dataset of measurements build a graph of nodes (keyframes) and
49  * constraints (edges) and solve it to find an estimation of the actual robot
50  * trajectory.
51  *
52  * // TODO - change this description
53  * The template arguments are listed below:
54  * - \em GRAPH_T: The type of Graph to be constructed and optimized. Currently
55  * CGraphSlamEngine works only with CPosePDFGaussianInf GRAPH_T instances.
56  * - \em NODE_REGISTRAR: Class responsible of adding new nodes in the graph.
57  * Class should at least implement the deciders::CNodeRegistrationDecider
58  * interface provided in CNodeRegistrationDecider.h file.
59  * - \em EDGE_REGISTRAR: Class responsible of adding new edges in the graph.
60  * Class should at least implement the deciders::CEdgeRegistrationDecider
61  * interface provided in CEdgeRegistrationDecider.h file.
62  * - \em OPTIMIZER: Class responsible of optimizing the graph. Class should at
63  * least implement the optimizers::CGraphSlamOptimizer interface provided
64  * in CGraphSlamOptimizer.h file.
65  *
66  * \note The GRAPH_T resource is accessed after having locked the relevant
67  * section
68  * \em m_graph_section. Critical section is also <em> locked prior to the calls
69  * to the deciders/optimizers </em>.
70  *
71  * ### .ini Configuration Parameters
72  *
73  * \htmlinclude graphslam-engine_config_params_preamble.txt
74  *
75  * - \b output_dir_fname
76  * + \a Section : GeneralConfiguration
77  * + \a Default value : 1 (mrpt::system::LVL_INFO)
78  * + \a Required : FALSE
79  *
80  * - \b user_decides_about_output_dir
81  * + \a Section : GeneralConfiguration
82  * + \a Default value : FALSE
83  * + \a Required : FALSE
84  * + \a Description : If flag true and in case of name conflict with output
85  * directory of the
86  * previous execution, a command-line is presented to the user to decide what
87  * to do about the new output directory. By default output directory from
88  * previous run is overwritten by the directory of the current run.
89  *
90  * - \b ground_truth_file_format
91  * + \a Section : GeneralConfiguration
92  * + \a Default value : NavSimul
93  * + \a Required : FALSE
94  * + \a Description : Specify the format of the ground-truth file if one is
95  * provided. Currently CGraphSlamEngine supports ground truth files generated
96  * by the GridMapNavSimul tool or ground truth files corresponding to
97  * RGBD-TUM datasets.
98  * + \a Available Options: NavSimul, RGBD_TUM
99  *
100  * - \b class_verbosity
101  * + \a Section : GeneralConfiguration
102  * + \a Default value : 1 (mrpt::system::LVL_INFO)
103  * + \a Required : FALSE
104  *
105  *
106  * - \b visualize_map
107  * + \a Section : VisualizationParameters
108  * + \a Default value : TRUE
109  * + \a Required : FALSE
110  *
111  * - \b visualize_odometry_poses
112  * + \a Section : VisualizationParameters
113  * + \a Default value : TRUE
114  * + \a Required : FALSE
115  *
116  * - \b visualize_estimated_trajectory
117  * + \a Section : VisualizationParameters
118  * + \a Default value : TRUE
119  * + \a Required : FALSE
120  *
121  * - \b visualize_GT
122  * + \a Section : VisualizationParameters
123  * + \a Default value : TRUE
124  * + \a Required : FALSE
125  *
126  * - \b visualize_SLAM_metric
127  * + \a Section : VisualizationParameters
128  * + \a Default value : TRUE
129  * + \a Required : FALSE
130  *
131  * - \b enable_curr_pos_viewport
132  * + \a Section : VisualizationParameters
133  * + \a Default value : TRUE
134  * + \a Required : FALSE
135  * + \a Description : Applicable only when dealing with RGB-D datasets
136  *
137  * - \b enable_range_viewport
138  * + \a Section : VisualizationParameters
139  * + \a Default value : TRUE
140  * + \a Required : FALSE
141  * + \a Description : Applicable only when dealing with RGB-D datasets
142  *
143  * - \b enable_intensity_viewport
144  * + \a Section : VisualizationParameters
145  * + \a Default value : FALSE
146  * + \a Required : FALSE
147  * + \a Description : Applicable only when dealing with RGB-D datasets
148  *
149  *
150  * \note Implementation can be found in the file \em CGraphSlamEngine_impl.h
151  * \ingroup mrpt_graphslam_grp
152  */
153 template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
155 {
156  public:
157  /**\brief Handy typedefs */
158  /**\{*/
159  /**\brief Map for managing output file streams.*/
160  using fstreams_out = std::map<std::string, mrpt::io::CFileOutputStream>;
161  /**\brief Map for iterating over output file streams.*/
163 
164  /**\brief Type of graph constraints */
165  using constraint_t = typename GRAPH_T::constraint_t;
166  /**\brief Type of underlying poses (2D/3D). */
167  using pose_t = typename GRAPH_T::constraint_t::type_value;
168  using global_pose_t = typename GRAPH_T::global_pose_t;
169  using nodes_to_scans2D_t = std::map<
171  /**\}*/
172 
173  /**\brief Constructor of CGraphSlamEngine class template.
174  *
175  * // TODO - remove the deprecated arguments
176  * \param[in] config_file .ini file containing the configuration
177  * parameters for the CGraphSlamEngine as well as the deciders/optimizer
178  * classes that CGraphSlamEngine is using
179  * \param[in] win_manager CwindowManager instance that includes a pointer
180  * to a CDisplayWindow3D and a CWindowObserver instance for properly
181  * interacting with the display window
182  * \param[in] rawlog_fname .rawlog dataset file, containing the robot
183  * measurements. CGraphSlamEngine supports both
184  * <a href="http://www.mrpt.org/Rawlog_Format"> MRPT rwalog formats </a>
185  * but in order for graphSLAM to work as expected the rawlog foromat has to
186  * be supported by the every decider/optimizer class that
187  * CGraphSlamEngine makes use of.
188  * \param[in] fname_GT Textfile containing the ground truth path of the
189  * robot. Currently the class can read ground truth files corresponding
190  * either to <em>RGBD - TUM datasets</em> or to rawlog files generated with
191  * the \em GridMapNavSimul MRPT application.
192  * // TODO add the deciders/optimizer
193  *
194  *
195  * \note If a nullptr CWindowManager pointer is porovided, the application
196  * runs on <em> headless mode </em>. In this case, no visual feedback is
197  * given but application receives a big boost in performance
198  */
200  const std::string& config_file, const std::string& rawlog_fname = "",
201  const std::string& fname_GT = "",
202  mrpt::graphslam::CWindowManager* win_manager = NULL,
204  NULL,
206  NULL,
208  NULL);
209  /**\brief Default Destructor. */
210  virtual ~CGraphSlamEngine();
211 
212  // Public function definitions
213  //////////////////////////////////////////////////////////////
214  /**\brief Query CGraphSlamEngine instance for the current estimated robot
215  * position
216  */
218  /***\brief Get the estimated trajectory of the robot given by the running
219  * graphSLAM algorithm.
220  * @returns Current list of nodes registered in the graph
221  */
222  virtual typename GRAPH_T::global_poses_t getRobotEstimatedTrajectory()
223  const;
224  /**\brief Return the list of nodeIDs which make up robot trajectory
225  * \sa updateEstimatedTrajectoryVisualization
226  */
227  virtual void getNodeIDsOfEstimatedTrajectory(
228  std::set<mrpt::graphs::TNodeID>* nodes_set) const;
229  /**\brief Wrapper method around the GRAPH_T::saveToTextFile method.
230  * Method saves the graph in the format used by TORO & HoG-man strategies
231  *
232  * \param[in] fname_in Name of the generated graph file - Defaults to
233  * "output_graph" if not
234  * set by the user
235  *
236  * \sa save3DScene, http://www.mrpt.org/Robotics_file_formats
237  */
238  void saveGraph(const std::string* fname_in = nullptr) const;
239  /**\brief Wrapper method around the COpenGLScene::saveToFile method.
240  *
241  * \param[in] Name of the generated graph file - Defaults to "output_graph"
242  * if not
243  * set by the user
244  *
245  * \sa saveGraph
246  */
247  void save3DScene(const std::string* fname_in = nullptr) const;
248  /**\brief Read the configuration variables from the <em>.ini file</em>
249  * specified by
250  * the user.
251  * Method is automatically called, upon CGraphSlamEngine initialization
252  *
253  */
254  void loadParams(const std::string& fname);
255  /**\brief Fill in the provided string with the class configuration
256  * parameters.
257  *
258  * \sa printParams
259  */
260  void getParamsAsString(std::string* params_out) const;
261  /**\brief Wrapper around getParamsAsString.
262  * Returns the generated string instead of passing it as an argument to the
263  * call
264  *
265  * \sa printParams
266  */
268  /** \name Map computation and acquisition methods
269  * \brief Fill the given map based on the observations that have been
270  * recorded so far.
271  */
272  /**\{*/
273  /* \brief Method is a wrapper around the computeMap method
274  * \param[out] map Pointer to the map instance that is
275  * to be filled
276  * \param[out] acquisition_time Timestamp that the map was computed at.
277  * This does not (necessarily) matches with the query time since the
278  * cached version is used as long as a new node has not been registered
279  * since the last time the gridmap was computed.
280  *
281  * \sa computeMap
282  */
283  void getMap(
285  mrpt::system::TTimeStamp* acquisition_time = NULL) const;
286  void getMap(
288  mrpt::system::TTimeStamp* acquisition_time = NULL) const;
289  /**\brief Compute the map of the environment based on the
290  * recorded measurements.
291  *
292  * \warning Currently only mrpt::obs::2DRangeScans are supported
293  * \sa getMap
294  */
295  void computeMap() const;
296  /**\}*/
297  /**\brief Print the problem parameters to the console for verification.
298  *
299  * Method is a wrapper around CGraphSlamEngine::getParamsAsString method
300  * \sa getParamsAsString
301  */
302  void printParams() const;
303  /**\brief Wrapper method around _execGraphSlamStep.
304  *
305  * Handy for not having to specify any action/observations objects
306  * \return False if the user has requested to exit the graphslam execution
307  * (e.g. pressed ctrl-c), True otherwise
308  */
309  bool execGraphSlamStep(
310  mrpt::obs::CObservation::Ptr& observation, size_t& rawlog_entry);
311  /**\brief Main class method responsible for parsing each measurement and
312  * for executing graphSLAM.
313  *
314  * \note Method reads each measurement seperately, so the application that
315  * invokes it is responsibe for fetching the measurements (e.g. from a
316  * rawlog file).
317  *
318  * \return False if the user has requested to exit the graphslam execution
319  * (e.g. pressed ctrl-c), True otherwise
320  **/
321  virtual bool _execGraphSlamStep(
323  mrpt::obs::CSensoryFrame::Ptr& observations,
324  mrpt::obs::CObservation::Ptr& observation, size_t& rawlog_entry);
325 
326  /**\brief Return a reference to the underlying GRAPH_T instance. */
327  const GRAPH_T& getGraph() const { return m_graph; }
328  /**\brief Return the filename of the used rawlog file.*/
330  /**\name ground-truth parsing methods */
331  /**\{*/
332  /**\brief Parse the ground truth .txt file and fill in the corresponding
333  * gt_poses vector.
334  *
335  * It is assumed that the rawlog, thererfore the groundtruth file has been
336  * generated using the <em>GridMapNavSimul</em> MRPT application. If not
337  * user should abide the ground-truth file format to that of the files
338  * generated by the GridMapNavSimul app.
339  *
340  * \sa readGTFileRGBD_TUM
341  *
342  * \param[in] fname_GT Ground truth filename from which the measurements
343  * are to be read
344  * \param[out] gt_poses std::vector which is to contain the 2D ground truth
345  * poses.
346  * \param[out] gt_timestamps std::vector which is to contain the timestamps
347  * for the corresponding ground truth poses. Ignore this argument if
348  * timestamps are not needed.
349  */
350  static void readGTFile(
351  const std::string& fname_GT,
352  std::vector<mrpt::poses::CPose2D>* gt_poses,
353  std::vector<mrpt::system::TTimeStamp>* gt_timestamps = NULL);
354  static void readGTFile(
355  const std::string& fname_GT,
356  std::vector<mrpt::poses::CPose3D>* gt_poses,
357  std::vector<mrpt::system::TTimeStamp>* gt_timestamps = NULL);
358  /**\brief Parse the ground truth .txt file and fill in the corresponding
359  * m_GT_poses vector. The poses returned are given with regards to the
360  * MRPT reference frame.
361  *
362  * It is assumed that the groundtruth file has been generated using the
363  * <em>rgbd_dataset2rawlog</em> MRPT tool.
364  *
365  * \param[in] fname_GT Ground truth filename from which the measurements
366  * are to be read
367  * \param[out] gt_poses std::vector which is to contain the
368  * 2D ground truth poses.
369  * \param[out] gt_timestamps std::vector which is to contain the timestamps
370  * for the corresponding ground truth poses. Ignore this argument if
371  * timestamps are not needed.
372  *
373  * \sa readGTFile,
374  * http://www.mrpt.org/Collection_of_Kinect_RGBD_datasets_with_ground_truth_CVPR_TUM_2011
375  */
376  static void readGTFileRGBD_TUM(
377  const std::string& fname_GT,
378  std::vector<mrpt::poses::CPose2D>* gt_poses,
379  std::vector<mrpt::system::TTimeStamp>* gt_timestamps = NULL);
380 
381  /**\}*/
382 
383  /**\brief Generate and write to a corresponding report for each of the
384  * respective self/decider/optimizer classes.
385  *
386  * \param[in] output_dir_fname directory name to generate the files in.
387  * Directory must be crated prior to this call
388  *
389  * \sa getDescriptiveReport, CGraphSlamHandler::initOutputDir
390  */
391  void generateReportFiles(const std::string& output_dir_fname_in);
392  /**\brief Fill the given vector with the deformation energy values computed
393  * for the SLAM evaluation metric
394  *
395  * \param[out] vec_out deformation energy vector to be filled
396  * \sa m_deformation_energy_vec
397  */
398  void getDeformationEnergyVector(std::vector<double>* vec_out) const;
399  /**\brief Fill the given maps with stats regarding the overall execution of
400  * graphslam.
401  */
402  bool getGraphSlamStats(
403  std::map<std::string, int>* node_stats,
404  std::map<std::string, int>* edge_stats,
405  mrpt::system::TTimeStamp* timestamp = NULL);
406 
407  /**\name pause/resume execution */
408  /**\{ */
409  bool isPaused() const { return m_is_paused; }
410  void togglePause()
411  {
412  if (isPaused())
413  {
414  this->resumeExec();
415  }
416  else
417  {
418  this->pauseExec();
419  }
420  }
421  void resumeExec() const
422  {
423  if (!isPaused())
424  {
425  return;
426  }
427  MRPT_LOG_INFO_STREAM("Program resumed.");
428  m_is_paused = false;
429 
430  if (m_enable_visuals)
431  {
432  this->m_win->addTextMessage(
433  0.3, 0.8, "", mrpt::img::TColorf(1.0, 0, 0),
435  }
436  }
437 
438  void pauseExec()
439  {
440  if (isPaused())
441  {
442  return;
443  }
445  "Program is paused. "
446  << "Press \"" << m_keystroke_pause_exec << " or \""
448  << "\" in the dipslay window to resume");
449  m_is_paused = true;
450  if (m_enable_visuals)
451  {
452  this->m_win->addTextMessage(
453  0.3, 0.8, m_paused_message, mrpt::img::TColorf(1.0, 0, 0),
455  }
456 
457  while (this->isPaused())
458  {
459  std::this_thread::sleep_for(1s);
460  this->queryObserverForEvents();
461  }
462  }
463  /**\} */
464 
465  protected:
466  // Private function definitions
467  //////////////////////////////////////////////////////////////
468  /**\brief General initialization method to call from the Class
469  * Constructors.
470  *
471  * \note Method is automatically called in the class constructor
472  */
473  void initClass();
474  /**\brief Automate the creation and initialization of a results file
475  * relevant to
476  * the application.
477  *
478  * Open the file (corresponding to the provided filename) and write an
479  * introductory message.
480  *
481  * \sa CGraphSlamHandler::initOutputDir
482  */
483  void initResultsFile(const std::string& fname);
484  /**\brief Fill the provided string with a detailed report of the class state
485  *
486  * Report includes the following:
487  * - Timing of important methods
488  * - Properties fo class at the current time
489  * - Logging of commands until current time
490  *
491  * \note Decider/Optimizer classes should also implement a
492  * getDescriptiveReport
493  * method for printing information on their part of the execution.
494  */
495  void getDescriptiveReport(std::string* report_str) const;
496  /** \name Initialization of Visuals
497  * Methods used for initializing various visualization features relevant to
498  * the application at hand. If the visual feature is specified by the user
499  * (via the .ini file) and if it is relevant to the application then the
500  * corresponding method is called in the initClass class method
501  */
502  /**\{*/
503  void initVisualization();
504 
505  void initRangeImageViewport();
507 
509  /**\brief Method to help overcome the partial template specialization
510  * restriction of C++. Apply polymorphism by overloading function arguments
511  * instead
512  */
513  /**\{ */
515  const mrpt::poses::CPose2D& p_unused);
517  const mrpt::poses::CPose3D& p_unused);
518  /**\} */
519 
520  void initCurrPosViewport();
521  void initGTVisualization();
525  /**\}*/
526 
527  /** \name Update of Visuals
528  * Methods used for updating various visualization features relevant to
529  * the application at hand. If relevant to the application at hand update
530  * is periodically scheduled inside the execGraphSlam method
531  */
532  /**\{*/
533  /**\brief Wrapper around the deciders/optimizer updateVisuals methods
534  */
535  void updateAllVisuals();
536  /**\brief In RGB-D TUM Datasets update the Range image displayed in a
537  * seperate viewport
538  */
540  /**\brief In RGB-D TUM Datasets update the Intensity image displayed in a
541  * seperate viewport
542  */
544  /**\brief Update the viewport responsible for displaying the graph-building
545  * procedure in the estimated position of the robot
546  */
547  virtual void updateCurrPosViewport();
548  /**\brief return the 3D Pose of a LaserScan that is to be visualized.
549  *
550  * Used during the computeMap call for the occupancy gridmap
551  */
553  const mrpt::graphs::TNodeID nodeID) const;
554  /**\brief Set the properties of the map visual object based on the nodeID
555  * that
556  * it was produced by.
557  * Derived classes may override this method if they want to have different
558  * visual properties (color, shape etc.) for different nodeIDs.
559  *
560  * \note Base class method sets only the color of the object
561  */
562  virtual void setObjectPropsFromNodeID(
563  const mrpt::graphs::TNodeID nodeID,
565  void initMapVisualization();
566  /**\brief Update the map visualization based on the current graphSLAM
567  * state.
568  *
569  * Map is produced by arranging the range scans based on the estimated
570  * robot trajectory.
571  *
572  * \sa updateEstimatedTrajectoryVisualization
573  */
575  const std::map<
577  nodes_to_laser_scans2D,
578  bool full_update = false);
579  /**\brief Display the next ground truth position in the visualization
580  * window.
581  *
582  * \sa updateOdometryVisualization
583  */
584  void updateGTVisualization();
585  /**\brief Update odometry-only cloud with latest odometry estimation.
586  *
587  * \sa updateGTVisualization
588  * */
590  /**\brief Update the Esstimated robot trajectory with the latest estimated
591  * robot position.
592  *
593  * Update CSetOfLines visualization object with the latest graph node
594  * position. If full update is asked, method clears the CSetOfLines
595  * object and redraws all the lines based on the updated (optimized)
596  * positions of the nodes
597  */
598  void updateEstimatedTrajectoryVisualization(bool full_update = false);
599  /**\brief Update the displayPlots window with the new information with
600  * regards to the metric
601  */
603  /**\}*/
604  /** \name Toggling of Visuals
605  * Methods used for toggling various visualization features relevant to
606  * the application at hand.
607  */
608  /**\{*/
610  void toggleGTVisualization();
611  void toggleMapVisualization();
613  /**\}*/
614 
615  /**\brief Cut down on the size of the given laser scan.
616  *
617  * Handy for reducing the size of the resulting mrpt::opengl::CSetOfObjects
618  * that would be inserted in the visualization scene. Increase the
619  * decimation rate - keep-every_n_entries - to reduce the computational
620  * cost of updating the map visualization
621  *
622  * \sa updateMapVisualization
623  */
624  void decimateLaserScan(
625  mrpt::obs::CObservation2DRangeScan& laser_scan_in,
626  mrpt::obs::CObservation2DRangeScan* laser_scan_out,
627  const int keep_every_n_entries = 2);
628  void alignOpticalWithMRPTFrame(); // TODO - either use it or remove it
629  /**\brief Query the observer instance for any user events.
630  *
631  * Query the given observer for any events (keystrokes, mouse clicks,
632  * that may have occurred in the CDisplayWindow3D and fill in the
633  * corresponding class variables
634  */
635  inline void queryObserverForEvents();
636 
637  /** \brief Compare the SLAM result (estimated trajectory) with the GT path.
638  *
639  * See <a
640  * href="http://europa.informatik.uni-freiburg.de/files/burgard09iros.pdf">
641  * A Comparison of SLAM Algorithms Based on a Graph of Relations</a>
642  * for more details on this.
643  */
644  void computeSlamMetric(mrpt::graphs::TNodeID nodeID, size_t gt_index);
645 
646  /**\brief Wrapper method that used for printing error messages in a
647  * consistent manner
648  *
649  * Makes use of the COutputLogger instance. Prints error message when
650  * toggling illegal visual features in the display window
651  */
653  std::string viz_flag, int sleep_time = 500 /* ms */);
654  // TODO - move this to a different module - to be accessed globally
655  /**\brief Fill the TTimestamp in a consistent manner.
656  *
657  * Method can be used in both MRPT Rawlog formats
658  *
659  * \param[in] action_ptr Pointer to the action (action-observations format)
660  * \param[in] observations Pointer to list of observations
661  * (action-observations format)
662  * \param[in] observation Pointer to single observation (observation-only
663  * format)
664  *
665  * \note if both action_ptr and observation_ptr contains valid timestamps,
666  * the
667  * action is preferred.
668  *
669  * \return mrpt::system::TTimeStamp
670  */
673  const mrpt::obs::CSensoryFrame::Ptr observations,
674  const mrpt::obs::CObservation::Ptr observation);
675  // TODO - move these somewhere else.
676  /**\name Class specific supplementary functions.
677  */
678  /**\{*/
679  static double accumulateAngleDiffs(
680  const mrpt::poses::CPose2D& p1, const mrpt::poses::CPose2D& p2);
681  static double accumulateAngleDiffs(
682  const mrpt::poses::CPose3D& p1, const mrpt::poses::CPose3D& p2);
683  /**\}*/
684  /**\brief Set the opengl model that indicates the latest position of the
685  * trajectory at hand
686  *
687  * \param[in] model_name Name of the resulting opengl object.
688  * \param[in] model_color Color of the object.
689  * \param[in] model_size Scaling of the object.
690  * \param[in] init_pose Initial position of the object.
691  *
692  * \todo Use an airplane/quad model for 3D operations
693  *
694  * \returns CSetOfObjects::Ptr instance.
695  *
696  * \note Different model is used depending on whether we are running 2D or
697  * 3D SLAM.
698  */
700  const std::string& model_name,
701  const mrpt::img::TColor& model_color = mrpt::img::TColor(0, 0, 0),
702  const size_t model_size = 1, const pose_t& init_pose = pose_t());
703 
704  /**\brief Assert that the given nodes number matches the registered graph
705  * nodes, otherwise throw exception
706  *
707  * \note Method locks the graph internally.
708  *
709  * \raise logic_error if the expected node count mismatches with the
710  * graph current node count.
711  */
712  virtual void monitorNodeRegistration(
713  bool registered = false, std::string class_name = "Class");
714  /**\brief Wrapper around the GRAPH_T::dijkstra_nodes_estimate
715  *
716  * Update the global position of the nodes
717  */
719 
720  // VARIABLES
721  //////////////////////////////////////////////////////////////
722  mrpt::system::CTimeLogger m_time_logger; /**<Time logger instance */
723 
724  /**\brief The graph object to be built and optimized. */
725  GRAPH_T m_graph;
726 
727  /**\name Decider/Optimizer instances. Delegating the GRAPH_T tasks to these
728  * classes makes up for a modular and configurable design
729  */
730  /**\{*/
734  /**\}*/
735 
736  /**\brief Determine if we are to enable visualization support or not. */
737  const bool m_enable_visuals;
738 
740 
741  /**\brief Rawlog file from which to read the measurements.
742  *
743  * If string is empty, process is to be run online
744  */
746 
748 
749  /**\brief Counter for reading back the GT_poses. */
751  /**\brief Rate at which to read the GT poses. */
753 
755 
758 
759  /**\brief keeps track of the out fstreams so that they can be closed (if
760  * still open) in the class Dtor.
761  */
763 
764  /**\name Visualization - related objects */
765  /**\{*/
769  /**\brief DisplayPlots instance for visualizing the evolution of the SLAM
770  * metric
771  */
773  /**\}*/
774 
775  /** \name Visualization - related flags
776  * \brief Flags for visualizing various trajectories/objects of interest.
777  *
778  * These are set from the .ini configuration file. The actual visualization
779  * of these objects can be overriden if the user issues the corresponding
780  * keystrokes in the CDisplayWindow3D. In order for them to have any
781  * effect, a pointer to CDisplayWindow3D has to be given first.
782  */
783  /**\{*/
792  /**\}*/
793 
794  /**\brief Indicated if program is temporarily paused by the user
795  */
796  mutable bool m_is_paused;
797 
798  /**\brief Message to be displayed while paused. */
800 
801  /**\name textMessage - related Parameters
802  * Parameters relevant to the textMessages appearing in the visualization
803  * window. These are divided into
804  * - Y offsets: vertical position of the textMessage, starting from the top
805  * side.
806  * - Indices: Unique ID number of each textMessage, used for updating it
807  */
808  /**\{*/
809  /**\brief Offset from the \a left side of the canvas.
810  * Common for all the messages that are displayed on that side.
811  */
813 
820 
827  /**\}*/
828 
829  /**\name User available keystrokes
830  * Keystrokes for toggling the corresponding objects in the CDisplayWindow
831  * upon user press
832  */
833  /**\{*/
839  /**\}*/
840 
841  /**\brief Instance to keep track of all the edges + visualization related
842  * operations
843  */
845 
846  /**\brief Flag for specifying if we are going to use ground truth data at
847  * all.
848  *
849  * This is set to true either if the evolution of the SLAM metric or the
850  * ground truth visualization is set to true.
851  */
852  bool m_use_GT;
853 
854  // pose_t vectors
855  std::vector<pose_t> m_odometry_poses;
856  std::vector<pose_t> m_GT_poses;
857 
859 
860  /**\brief Map of NodeIDs to their corresponding LaserScans.
861  */
863  /**\brief Last laser scan that the current class instance received.
864  */
866  /**\brief First recorded laser scan - assigned to the root */
868  /**\brief Last laser scan that the current class instance received.
869  */
871 
872  /**\name Trajectories colors */
873  /**\{*/
879  /**\}*/
880 
881  // frame transformation from the RGBD_TUM GrountTruth to the MRPT
882  // reference frame
883  // TODO - either use it or lose it...
885  /** How big are the robots going to be in the scene */
887  /**\brief Internal counter for querying for the number of nodeIDs.
888  *
889  * Handy for not locking the m_graph resource
890  */
892  /** Mark graph modification/accessing explicitly for multithreaded
893  * implementation
894  */
895  mutable std::mutex m_graph_section;
896 
897  // keep track of the storage directory for the 3DRangeScan depth/range
898  // images
901 
902  /**\name Slam Metric related variables */
903  /**\{*/
904  /**\brief Map from nodeIDs to their corresponding closest GT pose index.
905  * Keep track of the nodeIDs instead of the node positions as the latter
906  * are about to change in the Edge Registration / Loop closing procedures
907  */
908  std::map<mrpt::graphs::TNodeID, size_t> m_nodeID_to_gt_indices;
910  std::vector<double> m_deformation_energy_vec;
911  /**\}*/
912 
913  /**\brief Struct responsible for keeping the parameters of the .info file
914  * in RGBD related datasets
915  */
917  {
919  TRGBDInfoFileParams(const std::string& rawlog_fname);
921 
923  /**\brief Parse the RGBD information file to gain information about the
924  * rawlog
925  * file contents
926  */
927  void parseFile();
928  void setRawlogFile(const std::string& rawlog_fname);
929 
930  /**\brief Format for the parameters in the info file:
931  * <em>string literal - related value</em> (kept in a string
932  * representation)
933  */
934  std::map<std::string, std::string> fields;
935 
937 
938  } m_info_params;
939 
940  /**\brief Time it took to record the dataset.
941  * Processing time should (at least) be equal to the grab time for the
942  * algorithm to run in real-time
943  */
945 
946  /**\brief First recorded timestamp in the dataset. */
948  /**\brief Current dataset timestamp */
950  /**\brief Current robot position based solely on odometry */
952 
953  /**\brief Indicate whether the user wants to exit the application (e.g.
954  * pressed by pressign ctrl-c)
955  */
957 
958  /**\name Map-related objects
959  * \brief Cached version and corresponding flag of map
960  */
961  /**\{*/
963  /**\brief Acquired map in CSimpleMap representation */
966  /**\brief Indicates if the map is cached.
967  *
968  *\note Common var for both 2D and 3D maps.
969  */
970  mutable bool m_map_is_cached;
971  /**\brief Timestamp at which the map was computed
972  *
973  *\note Common var for both 2D and 3D maps.
974  */
976  /**\}*/
977 
979  /**\brief Track the first node registration occurance
980  *
981  * Handy so that we can assign a measurement to the root node as well.
982  */
984 
985  /**\brief MRPT CNetworkOfPoses constraint classes that are currently
986  * supported
987  */
988  std::vector<std::string> m_supported_constraint_types;
989  /**\brief Type of constraint currently in use.
990  */
992 
993  /**\brief Separator string to be used in debugging messages
994  */
995  static const std::string header_sep;
996  static const std::string report_sep;
997 };
998 } // end of namespace graphslam
999 } // end of namespace mrpt
1000 
1001 // pseudo-split the definition and implementation of template
1002 #include "CGraphSlamEngine_impl.h"
1003 
1004 #endif /* end of include guard: GRAPHSLAMENGINE_H */
void updateOdometryVisualization()
Update odometry-only cloud with latest odometry estimation.
mrpt::opengl::CSetOfObjects::Ptr setCurrentPositionModel(const std::string &model_name, const mrpt::img::TColor &model_color=mrpt::img::TColor(0, 0, 0), const size_t model_size=1, const pose_t &init_pose=pose_t())
Set the opengl model that indicates the latest position of the trajectory at hand.
Scalar * iterator
Definition: eigen_plugins.h:26
typename GRAPH_T::global_pose_t global_pose_t
virtual void getNodeIDsOfEstimatedTrajectory(std::set< mrpt::graphs::TNodeID > *nodes_set) const
Return the list of nodeIDs which make up robot trajectory.
void queryObserverForEvents()
Query the observer instance for any user events.
mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T > * m_edge_reg
void updateMapVisualization(const std::map< mrpt::graphs::TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr > &nodes_to_laser_scans2D, bool full_update=false)
Update the map visualization based on the current graphSLAM state.
mrpt::system::TTimeStamp m_init_timestamp
First recorded timestamp in the dataset.
void parseFile()
Parse the RGBD information file to gain information about the rawlog file contents.
void save3DScene(const std::string *fname_in=nullptr) const
Wrapper method around the COpenGLScene::saveToFile method.
mrpt::gui::CDisplayWindow3D * m_win
mrpt::obs::CObservation3DRangeScan::Ptr m_last_laser_scan3D
Last laser scan that the current class instance received.
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.
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:35
void updateSlamMetricVisualization()
Update the displayPlots window with the new information with regards to the metric.
Interface for implementing node registration classes.
double m_dataset_grab_time
Time it took to record the dataset.
bool m_use_GT
Flag for specifying if we are going to use ground truth data at all.
double m_offset_x_left
Offset from the left side of the canvas.
std::string getRawlogFname()
Return the filename of the used rawlog file.
mrpt::maps::CSimpleMap m_simple_map_cached
Acquired map in CSimpleMap representation.
mrpt::maps::COccupancyGridMap2D::Ptr m_gridmap_cached
mrpt::gui::CDisplayWindowPlots * m_win_plot
DisplayPlots instance for visualizing the evolution of the SLAM metric.
global_pose_t getCurrentRobotPosEstimation() const
Query CGraphSlamEngine instance for the current estimated robot position.
std::map< mrpt::graphs::TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr > nodes_to_scans2D_t
Interface for implementing edge registration classes.
bool m_request_to_exit
Indicate whether the user wants to exit the application (e.g.
virtual void setObjectPropsFromNodeID(const mrpt::graphs::TNodeID nodeID, mrpt::opengl::CSetOfObjects::Ptr &viz_object)
Set the properties of the map visual object based on the nodeID that it was produced by...
GLdouble s
Definition: glext.h:3676
mrpt::math::CMatrixDouble33 m_rot_TUM_to_MRPT
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
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.
void computeSlamMetric(mrpt::graphs::TNodeID nodeID, size_t gt_index)
Compare the SLAM result (estimated trajectory) with the GT path.
std::string m_rawlog_fname
Rawlog file from which to read the measurements.
void updateAllVisuals()
Wrapper around the deciders/optimizer updateVisuals methods.
static double accumulateAngleDiffs(const mrpt::poses::CPose2D &p1, const mrpt::poses::CPose2D &p2)
mrpt::obs::CObservation2DRangeScan::Ptr m_first_laser_scan2D
First recorded laser scan - assigned to the root.
static const std::string report_sep
typename GRAPH_T::constraint_t::type_value pose_t
Type of underlying poses (2D/3D).
Generic class for tracking the total number of edges for different tpes of edges and for storing visu...
Definition: CEdgeCounter.h:32
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.
struct mrpt::graphslam::CGraphSlamEngine::TRGBDInfoFileParams m_info_params
Versatile class for consistent logging and management of output messages.
size_t m_GT_poses_step
Rate at which to read the GT poses.
bool m_is_first_time_node_reg
Track the first node registration occurance.
std::map< std::string, std::string > fields
Format for the parameters in the info file: string literal - related value (kept in a string represen...
Main file for the GraphSlamEngine.
const bool m_enable_visuals
Determine if we are to enable visualization support or not.
std::vector< pose_t > m_odometry_poses
virtual void updateCurrPosViewport()
Update the viewport responsible for displaying the graph-building procedure in the estimated position...
mrpt::opengl::CSetOfObjects::Ptr initRobotModelVisualizationInternal(const mrpt::poses::CPose2D &p_unused)
Method to help overcome the partial template specialization restriction of C++.
CGraphSlamEngine(const std::string &config_file, const std::string &rawlog_fname="", const std::string &fname_GT="", mrpt::graphslam::CWindowManager *win_manager=NULL, mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > *node_reg=NULL, mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T > *edge_reg=NULL, mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T > *optimizer=NULL)
Constructor of CGraphSlamEngine class template.
mrpt::graphslam::CWindowObserver * m_win_observer
void updateEstimatedTrajectoryVisualization(bool full_update=false)
Update the Esstimated robot trajectory with the latest estimated robot position.
mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T > * m_optimizer
mrpt::img::TColor m_current_constraint_type_color
const GRAPH_T & getGraph() const
Return a reference to the underlying GRAPH_T instance.
mrpt::obs::CObservation2DRangeScan::Ptr m_last_laser_scan2D
Last laser scan that the current class instance received.
GLsizei const GLchar ** string
Definition: glext.h:4101
void saveGraph(const std::string *fname_in=nullptr) const
Wrapper method around the GRAPH_T::saveToTextFile method.
virtual ~CGraphSlamEngine()
Default Destructor.
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the class state.
static const std::string header_sep
Separator string to be used in debugging messages.
virtual mrpt::poses::CPose3D getLSPoseForGridMapVisualization(const mrpt::graphs::TNodeID nodeID) const
return the 3D Pose of a LaserScan that is to be visualized.
std::vector< double > m_deformation_energy_vec
virtual bool _execGraphSlamStep(mrpt::obs::CActionCollection::Ptr &action, mrpt::obs::CSensoryFrame::Ptr &observations, mrpt::obs::CObservation::Ptr &observation, size_t &rawlog_entry)
Main class method responsible for parsing each measurement and for executing graphSLAM.
mrpt::opengl::CSetOfObjects::Ptr initRobotModelVisualization()
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
void printParams() const
Print the problem parameters to the console for verification.
mrpt::graphslam::CWindowManager * m_win_manager
mrpt::graphslam::detail::CEdgeCounter m_edge_counter
Instance to keep track of all the edges + visualization related operations.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
static void readGTFile(const std::string &fname_GT, std::vector< mrpt::poses::CPose2D > *gt_poses, std::vector< mrpt::system::TTimeStamp > *gt_timestamps=NULL)
Parse the ground truth .txt file and fill in the corresponding gt_poses vector.
pose_t m_curr_odometry_only_pose
Current robot position based solely on odometry.
GRAPH_T m_graph
The graph object to be built and optimized.
virtual GRAPH_T::global_poses_t getRobotEstimatedTrajectory() const
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
static mrpt::system::TTimeStamp getTimeStamp(const mrpt::obs::CActionCollection::Ptr action, const mrpt::obs::CSensoryFrame::Ptr observations, const mrpt::obs::CObservation::Ptr observation)
Fill the TTimestamp in a consistent manner.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
Monitor events in the visualization window.
bool m_is_paused
Indicated if program is temporarily paused by the user.
size_t m_GT_poses_index
Counter for reading back the GT_poses.
mrpt::graphs::TNodeID m_nodeID_max
Internal counter for querying for the number of nodeIDs.
mrpt::system::TTimeStamp m_curr_timestamp
Current dataset timestamp.
std::string upperCase(const std::string &str)
Returns a upper-case version of a string.
A RGB color - floats in the range [0,1].
Definition: TColor.h:79
void decimateLaserScan(mrpt::obs::CObservation2DRangeScan &laser_scan_in, mrpt::obs::CObservation2DRangeScan *laser_scan_out, const int keep_every_n_entries=2)
Cut down on the size of the given laser scan.
void getDeformationEnergyVector(std::vector< double > *vec_out) const
Fill the given vector with the deformation energy values computed for the SLAM evaluation metric...
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
const std::string m_paused_message
Message to be displayed while paused.
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:17
typename GRAPH_T::constraint_t constraint_t
Type of graph constraints.
void getMap(mrpt::maps::COccupancyGridMap2D::Ptr map, mrpt::system::TTimeStamp *acquisition_time=NULL) const
mrpt::maps::COctoMap::Ptr m_octomap_cached
void addTextMessage(const double x, const double y, const std::string &text, const mrpt::img::TColorf &color=mrpt::img::TColorf(1.0, 1.0, 1.0), const size_t unique_index=0, const mrpt::opengl::TOpenGLFont font=mrpt::opengl::MRPT_GLUT_BITMAP_TIMES_ROMAN_24)
Add 2D text messages overlapped to the 3D rendered scene.
bool execGraphSlamStep(mrpt::obs::CObservation::Ptr &observation, size_t &rawlog_entry)
Wrapper method around _execGraphSlamStep.
fstreams_out m_out_streams
keeps track of the out fstreams so that they can be closed (if still open) in the class Dtor...
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::vector< std::string > m_supported_constraint_types
MRPT CNetworkOfPoses constraint classes that are currently supported.
std::string getParamsAsString() const
Wrapper around getParamsAsString.
size_t m_robot_model_size
How big are the robots going to be in the scene.
mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > * m_node_reg
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. ...
A RGB color - 8bit.
Definition: TColor.h:22
Struct responsible for keeping the parameters of the .info file in RGBD related datasets.
std::string m_current_constraint_type
Type of constraint currently in use.
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
Wrapper method that used for printing error messages in a consistent manner.
mrpt::system::TTimeStamp m_map_acq_time
Timestamp at which the map was computed.
typename fstreams_out::iterator fstreams_out_it
Map for iterating over output file streams.
bool m_map_is_cached
Indicates if the map is cached.
std::mutex m_graph_section
Mark graph modification/accessing explicitly for multithreaded implementation.
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...
mrpt::system::CTimeLogger m_time_logger
Time logger instance.
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...
std::map< mrpt::graphs::TNodeID, size_t > m_nodeID_to_gt_indices
Map from nodeIDs to their corresponding closest GT pose index.
std::map< std::string, mrpt::io::CFileOutputStream > fstreams_out
Handy typedefs.
nodes_to_scans2D_t m_nodes_to_laser_scans2D
Map of NodeIDs to their corresponding LaserScans.
void initResultsFile(const std::string &fname)
Automate the creation and initialization of a results file relevant to the application.
A graphical user interface (GUI) for efficiently rendering 3D scenes in real-time.
void initClass()
General initialization method to call from the Class Constructors.



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