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