MRPT  2.0.5
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-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #pragma once
11 
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  */
244  std::string getParamsAsString() const;
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.*/
305  inline std::string getRawlogFname() { return m_rawlog_fname; }
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, "", m_text_index_paused_message);
410  }
411  }
412 
413  void pauseExec()
414  {
415  if (isPaused())
416  {
417  return;
418  }
420  "Program is paused. "
421  << "Press \"" << m_keystroke_pause_exec << " or \""
423  << "\" in the dipslay window to resume");
424  m_is_paused = true;
425  if (m_enable_visuals)
426  {
427  this->m_win->addTextMessage(
429  }
430 
431  while (this->isPaused())
432  {
433  std::this_thread::sleep_for(1s);
434  this->queryObserverForEvents();
435  }
436  }
437  /**\} */
438 
439  protected:
440  // Private function definitions
441  //////////////////////////////////////////////////////////////
442  /**\brief General initialization method to call from the Class
443  * Constructors.
444  *
445  * \note Method is automatically called in the class constructor
446  */
447  void initClass();
448  /**\brief Automate the creation and initialization of a results file
449  * relevant to
450  * the application.
451  *
452  * Open the file (corresponding to the provided filename) and write an
453  * introductory message.
454  *
455  * \sa CGraphSlamHandler::initOutputDir
456  */
457  void initResultsFile(const std::string& fname);
458  /**\brief Fill the provided string with a detailed report of the class state
459  *
460  * Report includes the following:
461  * - Timing of important methods
462  * - Properties fo class at the current time
463  * - Logging of commands until current time
464  *
465  * \note Decider/Optimizer classes should also implement a
466  * getDescriptiveReport
467  * method for printing information on their part of the execution.
468  */
469  void getDescriptiveReport(std::string* report_str) const;
470  /** \name Initialization of Visuals
471  * Methods used for initializing various visualization features relevant to
472  * the application at hand. If the visual feature is specified by the user
473  * (via the .ini file) and if it is relevant to the application then the
474  * corresponding method is called in the initClass class method
475  */
476  /**\{*/
477  void initVisualization();
478 
479  void initRangeImageViewport();
481 
483  /**\brief Method to help overcome the partial template specialization
484  * restriction of C++. Apply polymorphism by overloading function arguments
485  * instead
486  */
487  /**\{ */
489  const mrpt::poses::CPose2D& p_unused);
491  const mrpt::poses::CPose3D& p_unused);
492  /**\} */
493 
494  void initCurrPosViewport();
495  void initGTVisualization();
499  /**\}*/
500 
501  /** \name Update of Visuals
502  * Methods used for updating various visualization features relevant to
503  * the application at hand. If relevant to the application at hand update
504  * is periodically scheduled inside the execGraphSlam method
505  */
506  /**\{*/
507  /**\brief Wrapper around the deciders/optimizer updateVisuals methods
508  */
509  void updateAllVisuals();
510  /**\brief In RGB-D TUM Datasets update the Range image displayed in a
511  * seperate viewport
512  */
514  /**\brief In RGB-D TUM Datasets update the Intensity image displayed in a
515  * seperate viewport
516  */
518  /**\brief Update the viewport responsible for displaying the graph-building
519  * procedure in the estimated position of the robot
520  */
521  virtual void updateCurrPosViewport();
522  /**\brief return the 3D Pose of a LaserScan that is to be visualized.
523  *
524  * Used during the computeMap call for the occupancy gridmap
525  */
527  const mrpt::graphs::TNodeID nodeID) const;
528  /**\brief Set the properties of the map visual object based on the nodeID
529  * that
530  * it was produced by.
531  * Derived classes may override this method if they want to have different
532  * visual properties (color, shape etc.) for different nodeIDs.
533  *
534  * \note Base class method sets only the color of the object
535  */
536  virtual void setObjectPropsFromNodeID(
537  const mrpt::graphs::TNodeID nodeID,
539  void initMapVisualization();
540  /**\brief Update the map visualization based on the current graphSLAM
541  * state.
542  *
543  * Map is produced by arranging the range scans based on the estimated
544  * robot trajectory.
545  *
546  * \sa updateEstimatedTrajectoryVisualization
547  */
549  const std::map<
551  nodes_to_laser_scans2D,
552  bool full_update = false);
553  /**\brief Display the next ground truth position in the visualization
554  * window.
555  *
556  * \sa updateOdometryVisualization
557  */
558  void updateGTVisualization();
559  /**\brief Update odometry-only cloud with latest odometry estimation.
560  *
561  * \sa updateGTVisualization
562  * */
564  /**\brief Update the Esstimated robot trajectory with the latest estimated
565  * robot position.
566  *
567  * Update CSetOfLines visualization object with the latest graph node
568  * position. If full update is asked, method clears the CSetOfLines
569  * object and redraws all the lines based on the updated (optimized)
570  * positions of the nodes
571  */
572  void updateEstimatedTrajectoryVisualization(bool full_update = false);
573  /**\brief Update the displayPlots window with the new information with
574  * regards to the metric
575  */
577  /**\}*/
578  /** \name Toggling of Visuals
579  * Methods used for toggling various visualization features relevant to
580  * the application at hand.
581  */
582  /**\{*/
584  void toggleGTVisualization();
585  void toggleMapVisualization();
587  /**\}*/
588 
589  /**\brief Cut down on the size of the given laser scan.
590  *
591  * Handy for reducing the size of the resulting mrpt::opengl::CSetOfObjects
592  * that would be inserted in the visualization scene. Increase the
593  * decimation rate - keep-every_n_entries - to reduce the computational
594  * cost of updating the map visualization
595  *
596  * \sa updateMapVisualization
597  */
598  void decimateLaserScan(
599  mrpt::obs::CObservation2DRangeScan& laser_scan_in,
600  mrpt::obs::CObservation2DRangeScan* laser_scan_out,
601  const int keep_every_n_entries = 2);
602  void alignOpticalWithMRPTFrame(); // TODO - either use it or remove it
603  /**\brief Query the observer instance for any user events.
604  *
605  * Query the given observer for any events (keystrokes, mouse clicks,
606  * that may have occurred in the CDisplayWindow3D and fill in the
607  * corresponding class variables
608  */
609  inline void queryObserverForEvents();
610 
611  /** \brief Compare the SLAM result (estimated trajectory) with the GT path.
612  *
613  * See <a
614  * href="http://europa.informatik.uni-freiburg.de/files/burgard09iros.pdf">
615  * A Comparison of SLAM Algorithms Based on a Graph of Relations</a>
616  * for more details on this.
617  */
618  void computeSlamMetric(mrpt::graphs::TNodeID nodeID, size_t gt_index);
619 
620  /**\brief Wrapper method that used for printing error messages in a
621  * consistent manner
622  *
623  * Makes use of the COutputLogger instance. Prints error message when
624  * toggling illegal visual features in the display window
625  */
627  std::string viz_flag, int sleep_time = 500 /* ms */);
628  // TODO - move this to a different module - to be accessed globally
629  /**\brief Fill the TTimestamp in a consistent manner.
630  *
631  * Method can be used in both MRPT Rawlog formats
632  *
633  * \param[in] action_ptr Pointer to the action (action-observations format)
634  * \param[in] observations Pointer to list of observations
635  * (action-observations format)
636  * \param[in] observation Pointer to single observation (observation-only
637  * format)
638  *
639  * \note if both action_ptr and observation_ptr contains valid timestamps,
640  * the
641  * action is preferred.
642  *
643  * \return mrpt::system::TTimeStamp
644  */
647  const mrpt::obs::CSensoryFrame::Ptr observations,
648  const mrpt::obs::CObservation::Ptr observation);
649  // TODO - move these somewhere else.
650  /**\name Class specific supplementary functions.
651  */
652  /**\{*/
653  static double accumulateAngleDiffs(
654  const mrpt::poses::CPose2D& p1, const mrpt::poses::CPose2D& p2);
655  static double accumulateAngleDiffs(
656  const mrpt::poses::CPose3D& p1, const mrpt::poses::CPose3D& p2);
657  /**\}*/
658  /**\brief Set the opengl model that indicates the latest position of the
659  * trajectory at hand
660  *
661  * \param[in] model_name Name of the resulting opengl object.
662  * \param[in] model_color Color of the object.
663  * \param[in] model_size Scaling of the object.
664  * \param[in] init_pose Initial position of the object.
665  *
666  * \todo Use an airplane/quad model for 3D operations
667  *
668  * \returns CSetOfObjects::Ptr instance.
669  *
670  * \note Different model is used depending on whether we are running 2D or
671  * 3D SLAM.
672  */
674  const std::string& model_name,
675  const mrpt::img::TColor& model_color = mrpt::img::TColor(0, 0, 0),
676  const size_t model_size = 1, const pose_t& init_pose = pose_t());
677 
678  /**\brief Assert that the given nodes number matches the registered graph
679  * nodes, otherwise throw exception
680  *
681  * \note Method locks the graph internally.
682  *
683  * \raise logic_error if the expected node count mismatches with the
684  * graph current node count.
685  */
686  virtual void monitorNodeRegistration(
687  bool registered = false, std::string class_name = "Class");
688  /**\brief Wrapper around the GRAPH_T::dijkstra_nodes_estimate
689  *
690  * Update the global position of the nodes
691  */
693 
694  // VARIABLES
695  //////////////////////////////////////////////////////////////
696  mrpt::system::CTimeLogger m_time_logger; /**<Time logger instance */
697 
698  /**\brief The graph object to be built and optimized. */
699  GRAPH_T m_graph;
700 
701  /**\name Decider/Optimizer instances. Delegating the GRAPH_T tasks to these
702  * classes makes up for a modular and configurable design
703  */
704  /**\{*/
708  /**\}*/
709 
710  /**\brief Determine if we are to enable visualization support or not. */
711  const bool m_enable_visuals;
712 
713  std::string m_config_fname;
714 
715  /**\brief Rawlog file from which to read the measurements.
716  *
717  * If string is empty, process is to be run online
718  */
719  std::string m_rawlog_fname;
720 
721  std::string m_fname_GT;
722 
723  /**\brief Counter for reading back the GT_poses. */
725  /**\brief Rate at which to read the GT poses. */
727 
729 
732 
733  /**\brief keeps track of the out fstreams so that they can be closed (if
734  * still open) in the class Dtor.
735  */
737 
738  /**\name Visualization - related objects */
739  /**\{*/
743  /**\brief DisplayPlots instance for visualizing the evolution of the SLAM
744  * metric
745  */
746  std::unique_ptr<mrpt::gui::CDisplayWindowPlots> m_win_plot = nullptr;
747  /**\}*/
748 
749  /** \name Visualization - related flags
750  * \brief Flags for visualizing various trajectories/objects of interest.
751  *
752  * These are set from the .ini configuration file. The actual visualization
753  * of these objects can be overriden if the user issues the corresponding
754  * keystrokes in the CDisplayWindow3D. In order for them to have any
755  * effect, a pointer to CDisplayWindow3D has to be given first.
756  */
757  /**\{*/
766  /**\}*/
767 
768  /**\brief Indicated if program is temporarily paused by the user
769  */
770  mutable bool m_is_paused;
771 
772  /**\brief Message to be displayed while paused. */
773  const std::string m_paused_message;
774 
775  /**\name textMessage - related Parameters
776  * Parameters relevant to the textMessages appearing in the visualization
777  * window. These are divided into
778  * - Y offsets: vertical position of the textMessage, starting from the top
779  * side.
780  * - Indices: Unique ID number of each textMessage, used for updating it
781  */
782  /**\{*/
783  /**\brief Offset from the \a left side of the canvas.
784  * Common for all the messages that are displayed on that side.
785  */
787 
794 
801  /**\}*/
802 
803  /**\name User available keystrokes
804  * Keystrokes for toggling the corresponding objects in the CDisplayWindow
805  * upon user press
806  */
807  /**\{*/
809  std::string m_keystroke_odometry;
810  std::string m_keystroke_GT;
812  std::string m_keystroke_map;
813  /**\}*/
814 
815  /**\brief Instance to keep track of all the edges + visualization related
816  * operations
817  */
819 
820  /**\brief Flag for specifying if we are going to use ground truth data at
821  * all.
822  *
823  * This is set to true either if the evolution of the SLAM metric or the
824  * ground truth visualization is set to true.
825  */
826  bool m_use_GT;
827 
828  // pose_t vectors
829  std::vector<pose_t> m_odometry_poses;
830  std::vector<pose_t> m_GT_poses;
831 
832  std::string m_GT_file_format;
833 
834  /**\brief Map of NodeIDs to their corresponding LaserScans.
835  */
837  /**\brief Last laser scan that the current class instance received.
838  */
840  /**\brief First recorded laser scan - assigned to the root */
842  /**\brief Last laser scan that the current class instance received.
843  */
845 
846  /**\name Trajectories colors */
847  /**\{*/
853  /**\}*/
854 
855  // frame transformation from the RGBD_TUM GrountTruth to the MRPT
856  // reference frame
857  // TODO - either use it or lose it...
859  /** How big are the robots going to be in the scene */
861  /**\brief Internal counter for querying for the number of nodeIDs.
862  *
863  * Handy for not locking the m_graph resource
864  */
866  /** Mark graph modification/accessing explicitly for multithreaded
867  * implementation
868  */
869  mutable std::mutex m_graph_section;
870 
871  // keep track of the storage directory for the 3DRangeScan depth/range
872  // images
874  std::string m_img_prev_path_base;
875 
876  /**\name Slam Metric related variables */
877  /**\{*/
878  /**\brief Map from nodeIDs to their corresponding closest GT pose index.
879  * Keep track of the nodeIDs instead of the node positions as the latter
880  * are about to change in the Edge Registration / Loop closing procedures
881  */
882  std::map<mrpt::graphs::TNodeID, size_t> m_nodeID_to_gt_indices;
884  std::vector<double> m_deformation_energy_vec;
885  /**\}*/
886 
887  /**\brief Struct responsible for keeping the parameters of the .info file
888  * in RGBD related datasets
889  */
891  {
893  TRGBDInfoFileParams(const std::string& rawlog_fname);
894  ~TRGBDInfoFileParams() = default;
895 
897  /**\brief Parse the RGBD information file to gain information about the
898  * rawlog
899  * file contents
900  */
901  void parseFile();
902  void setRawlogFile(const std::string& rawlog_fname);
903 
904  /**\brief Format for the parameters in the info file:
905  * <em>string literal - related value</em> (kept in a string
906  * representation)
907  */
908  std::map<std::string, std::string> fields;
909 
910  std::string info_fname;
911 
912  } m_info_params;
913 
914  /**\brief Time it took to record the dataset.
915  * Processing time should (at least) be equal to the grab time for the
916  * algorithm to run in real-time
917  */
919 
920  /**\brief First recorded timestamp in the dataset. */
922  /**\brief Current dataset timestamp */
924  /**\brief Current robot position based solely on odometry */
926 
927  /**\brief Indicate whether the user wants to exit the application (e.g.
928  * pressed by pressign ctrl-c)
929  */
931 
932  /**\name Map-related objects
933  * \brief Cached version and corresponding flag of map
934  */
935  /**\{*/
937  /**\brief Acquired map in CSimpleMap representation */
940  /**\brief Indicates if the map is cached.
941  *
942  *\note Common var for both 2D and 3D maps.
943  */
944  mutable bool m_map_is_cached;
945  /**\brief Timestamp at which the map was computed
946  *
947  *\note Common var for both 2D and 3D maps.
948  */
950  /**\}*/
951 
952  std::string m_class_name;
953  /**\brief Track the first node registration occurance
954  *
955  * Handy so that we can assign a measurement to the root node as well.
956  */
958 
959  /**\brief MRPT CNetworkOfPoses constraint classes that are currently
960  * supported
961  */
962  std::vector<std::string> m_supported_constraint_types;
963  /**\brief Type of constraint currently in use.
964  */
966 
967  /**\brief Separator string to be used in debugging messages
968  */
969  static const std::string header_sep;
970  static const std::string report_sep;
971 };
972 } // namespace mrpt::graphslam
973 // pseudo-split the definition and implementation of template
974 #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...
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.
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.
void addTextMessage(const double x_frac, const double y_frac, const std::string &text, const size_t unique_index=0, const mrpt::opengl::TFontParams &fontParams=mrpt::opengl::TFontParams())
A shortcut for calling mrpt::opengl::COpenGLViewport::addTextMessage() in the "main" viewport of the ...
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:85
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.
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
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:25
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 2.0.5 Git: 40e60e732 Thu Jul 9 08:38:35 2020 +0200 at jue jul 9 08:45:11 CEST 2020