MRPT  2.0.3
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"
mrpt::graphslam::CGraphSlamEngine::updateAllVisuals
void updateAllVisuals()
Wrapper around the deciders/optimizer updateVisuals methods.
Definition: CGraphSlamEngine_impl.h:1174
mrpt::graphslam::CGraphSlamEngine::resumeExec
void resumeExec() const
Definition: CGraphSlamEngine.h:397
mrpt::obs::CObservation::Ptr
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:45
mrpt::graphslam::CGraphSlamEngine::m_edge_reg
mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T > * m_edge_reg
Definition: CGraphSlamEngine.h:706
mrpt::graphslam::CGraphSlamEngine::m_last_laser_scan2D
mrpt::obs::CObservation2DRangeScan::Ptr m_last_laser_scan2D
Last laser scan that the current class instance received.
Definition: CGraphSlamEngine.h:839
mrpt::graphslam::CGraphSlamEngine::m_text_index_estimated_traj
int m_text_index_estimated_traj
Definition: CGraphSlamEngine.h:797
mrpt::graphslam::CGraphSlamEngine::m_offset_y_odometry
double m_offset_y_odometry
Definition: CGraphSlamEngine.h:788
mrpt::graphslam::deciders::CNodeRegistrationDecider
Interface for implementing node registration classes.
Definition: CNodeRegistrationDecider.h:32
mrpt::graphslam::CGraphSlamEngine::m_curr_odometry_only_pose
pose_t m_curr_odometry_only_pose
Current robot position based solely on odometry.
Definition: CGraphSlamEngine.h:925
mrpt::graphslam::CGraphSlamEngine::getMap
void getMap(mrpt::maps::COccupancyGridMap2D::Ptr map, mrpt::system::TTimeStamp *acquisition_time=nullptr) const
Definition: CGraphSlamEngine_impl.h:885
mrpt::graphslam::CGraphSlamEngine::m_estimated_traj_color
mrpt::img::TColor m_estimated_traj_color
Definition: CGraphSlamEngine.h:850
mrpt::graphslam::CGraphSlamEngine::~CGraphSlamEngine
~CGraphSlamEngine() override
Definition: CGraphSlamEngine_impl.h:58
CNetworkOfPoses.h
mrpt::graphslam::CGraphSlamEngine::m_fname_GT
std::string m_fname_GT
Definition: CGraphSlamEngine.h:721
COccupancyGridMap2D.h
mrpt::graphslam::CGraphSlamEngine::initIntensityImageViewport
void initIntensityImageViewport()
Definition: CGraphSlamEngine_impl.h:1219
mrpt::graphslam::CGraphSlamEngine::toggleMapVisualization
void toggleMapVisualization()
Definition: CGraphSlamEngine_impl.h:1677
mrpt::graphslam::CGraphSlamEngine::updateEstimatedTrajectoryVisualization
void updateEstimatedTrajectoryVisualization(bool full_update=false)
Update the Esstimated robot trajectory with the latest estimated robot position.
Definition: CGraphSlamEngine_impl.h:2192
mrpt::graphslam::CGraphSlamEngine::isPaused
bool isPaused() const
Definition: CGraphSlamEngine.h:385
mrpt::graphslam::CGraphSlamEngine::printParams
void printParams() const
Print the problem parameters to the console for verification.
Definition: CGraphSlamEngine_impl.h:1107
mrpt::graphslam::CGraphSlamEngine::m_node_reg
mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > * m_node_reg
Definition: CGraphSlamEngine.h:705
mrpt::graphslam::CGraphSlamEngine::monitorNodeRegistration
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.
Definition: CGraphSlamEngine_impl.h:850
mrpt::graphslam::CGraphSlamEngine::decimateLaserScan
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.
Definition: CGraphSlamEngine_impl.h:1962
mrpt::graphslam::CGraphSlamEngine::loadParams
void loadParams(const std::string &fname)
Read the configuration variables from the .ini file specified by the user.
Definition: CGraphSlamEngine_impl.h:989
mrpt::graphslam::CGraphSlamEngine::m_paused_message
const std::string m_paused_message
Message to be displayed while paused.
Definition: CGraphSlamEngine.h:773
obs_utils.h
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
Definition: CSetOfObjects.h:28
mrpt::graphslam::CGraphSlamEngine::header_sep
static const std::string header_sep
Separator string to be used in debugging messages.
Definition: CGraphSlamEngine.h:969
mrpt::graphslam::CGraphSlamEngine::m_graph
GRAPH_T m_graph
The graph object to be built and optimized.
Definition: CGraphSlamEngine.h:699
mrpt::graphslam::CGraphSlamEngine< mrpt::graphs::CNetworkOfPoses2DInf >::constraint_t
typename mrpt::graphs::CNetworkOfPoses2DInf ::constraint_t constraint_t
Type of graph constraints.
Definition: CGraphSlamEngine.h:149
mrpt::graphslam::CGraphSlamEngine::getLSPoseForGridMapVisualization
virtual mrpt::poses::CPose3D getLSPoseForGridMapVisualization(const mrpt::graphs::TNodeID nodeID) const
return the 3D Pose of a LaserScan that is to be visualized.
Definition: CGraphSlamEngine_impl.h:1805
mrpt::graphslam::CGraphSlamEngine::m_first_laser_scan2D
mrpt::obs::CObservation2DRangeScan::Ptr m_first_laser_scan2D
First recorded laser scan - assigned to the root.
Definition: CGraphSlamEngine.h:841
mrpt::graphslam::CGraphSlamEngine::updateCurrPosViewport
virtual void updateCurrPosViewport()
Update the viewport responsible for displaying the graph-building procedure in the estimated position...
Definition: CGraphSlamEngine_impl.h:1319
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
Definition: CSensoryFrame.h:53
mrpt::graphslam::CGraphSlamEngine::m_has_read_config
bool m_has_read_config
Definition: CGraphSlamEngine.h:730
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:54
mrpt::graphslam::CGraphSlamEngine::toggleOdometryVisualization
void toggleOdometryVisualization()
Definition: CGraphSlamEngine_impl.h:1621
mrpt::system::CTimeLogger
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X),...
Definition: system/CTimeLogger.h:50
mrpt::graphslam::CGraphSlamEngine::m_octomap_cached
mrpt::maps::COctoMap::Ptr m_octomap_cached
Definition: CGraphSlamEngine.h:939
mrpt::graphslam::CGraphSlamEngine::alignOpticalWithMRPTFrame
void alignOpticalWithMRPTFrame()
Definition: CGraphSlamEngine_impl.h:1524
mrpt::graphslam::CGraphSlamEngine::pauseExec
void pauseExec()
Definition: CGraphSlamEngine.h:413
mrpt::graphslam::deciders::CEdgeRegistrationDecider
Interface for implementing edge registration classes.
Definition: CEdgeRegistrationDecider.h:36
mrpt::graphslam::CGraphSlamEngine::m_is_paused
bool m_is_paused
Indicated if program is temporarily paused by the user.
Definition: CGraphSlamEngine.h:770
CSetOfObjects.h
mrpt::graphslam::CGraphSlamEngine::m_nodeID_max
mrpt::graphs::TNodeID m_nodeID_max
Internal counter for querying for the number of nodeIDs.
Definition: CGraphSlamEngine.h:865
mrpt::graphslam::CGraphSlamEngine< mrpt::graphs::CNetworkOfPoses2DInf >::pose_t
typename mrpt::graphs::CNetworkOfPoses2DInf ::constraint_t::type_value pose_t
Type of underlying poses (2D/3D).
Definition: CGraphSlamEngine.h:151
mrpt::graphslam::CGraphSlamEngine::m_offset_y_estimated_traj
double m_offset_y_estimated_traj
Definition: CGraphSlamEngine.h:790
mrpt::graphslam::optimizers::CGraphSlamOptimizer
Interface for implementing graphSLAM optimizer classes.
Definition: CGraphSlamOptimizer.h:34
mrpt::graphslam::CGraphSlamEngine::m_GT_poses
std::vector< pose_t > m_GT_poses
Definition: CGraphSlamEngine.h:830
mrpt::graphslam::CGraphSlamEngine::m_graph_section
std::mutex m_graph_section
Mark graph modification/accessing explicitly for multithreaded implementation.
Definition: CGraphSlamEngine.h:869
CFileOutputStream.h
mrpt::graphslam::CGraphSlamEngine::m_text_index_odometry
int m_text_index_odometry
Definition: CGraphSlamEngine.h:795
mrpt::graphslam::CGraphSlamEngine::m_robot_model_size
size_t m_robot_model_size
How big are the robots going to be in the scene.
Definition: CGraphSlamEngine.h:860
mrpt::graphslam::CGraphSlamEngine::getGraph
const GRAPH_T & getGraph() const
Return a reference to the underlying GRAPH_T instance.
Definition: CGraphSlamEngine.h:303
mrpt::graphslam::CGraphSlamEngine::m_enable_range_viewport
bool m_enable_range_viewport
Definition: CGraphSlamEngine.h:765
mrpt::graphslam::CGraphSlamEngine::m_current_constraint_type_color
mrpt::img::TColor m_current_constraint_type_color
Definition: CGraphSlamEngine.h:852
mrpt::maps::COctoMap::Ptr
std::shared_ptr< mrpt::maps ::COctoMap > Ptr
Definition: COctoMap.h:43
COctoMap.h
mrpt::graphslam::CGraphSlamEngine::m_nodeID_to_gt_indices
std::map< mrpt::graphs::TNodeID, size_t > m_nodeID_to_gt_indices
Map from nodeIDs to their corresponding closest GT pose index.
Definition: CGraphSlamEngine.h:882
mrpt::graphslam::CGraphSlamEngine::accumulateAngleDiffs
static double accumulateAngleDiffs(const mrpt::poses::CPose2D &p1, const mrpt::poses::CPose2D &p2)
Definition: CGraphSlamEngine_impl.h:2480
mrpt::graphslam::CGraphSlamEngine::execGraphSlamStep
bool execGraphSlamStep(mrpt::obs::CObservation::Ptr &observation, size_t &rawlog_entry)
Wrapper method around _execGraphSlamStep.
Definition: CGraphSlamEngine_impl.h:479
mrpt::graphs::TNodeID
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
mrpt::graphslam::CGraphSlamEngine::m_keystroke_pause_exec
std::string m_keystroke_pause_exec
Definition: CGraphSlamEngine.h:808
mrpt::graphslam::CGraphSlamEngine::m_text_index_paused_message
int m_text_index_paused_message
Definition: CGraphSlamEngine.h:800
mrpt::graphslam::CGraphSlamEngine::m_rot_TUM_to_MRPT
mrpt::math::CMatrixDouble33 m_rot_TUM_to_MRPT
Definition: CGraphSlamEngine.h:858
mrpt::graphslam::CGraphSlamEngine::initRobotModelVisualization
mrpt::opengl::CSetOfObjects::Ptr initRobotModelVisualization()
Definition: CGraphSlamEngine_impl.h:1266
mrpt::graphslam::CGraphSlamEngine::toggleGTVisualization
void toggleGTVisualization()
Definition: CGraphSlamEngine_impl.h:1649
mrpt::graphslam::CGraphSlamEngine::m_offset_y_paused_message
double m_offset_y_paused_message
Definition: CGraphSlamEngine.h:793
mrpt::graphslam::CGraphSlamEngine::_execGraphSlamStep
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.
Definition: CGraphSlamEngine_impl.h:492
CGraphSlamOptimizer.h
mrpt::graphslam::CGraphSlamEngine::m_map_is_cached
bool m_map_is_cached
Indicates if the map is cached.
Definition: CGraphSlamEngine.h:944
mrpt::obs::CActionCollection::Ptr
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
Definition: CActionCollection.h:28
CObservation3DRangeScan.h
CGraphSlamEngine_impl.h
mrpt::graphslam::CGraphSlamEngine::queryObserverForEvents
void queryObserverForEvents()
Query the observer instance for any user events.
Definition: CGraphSlamEngine_impl.h:1572
CEdgeRegistrationDecider.h
mrpt::graphslam::CGraphSlamEngine::m_keystroke_map
std::string m_keystroke_map
Definition: CGraphSlamEngine.h:812
mrpt::graphslam::CGraphSlamEngine::getDescriptiveReport
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the class state.
Definition: CGraphSlamEngine_impl.h:2555
mrpt::graphslam::CGraphSlamEngine::m_GT_file_format
std::string m_GT_file_format
Definition: CGraphSlamEngine.h:832
mrpt::gui::CDisplayWindow3D::addTextMessage
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 ...
Definition: CDisplayWindow3D.h:312
MRPT_LOG_WARN_STREAM
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
Definition: system/COutputLogger.h:475
mrpt::graphslam::CGraphSlamEngine::m_visualize_GT
bool m_visualize_GT
Definition: CGraphSlamEngine.h:759
CPose2D.h
mrpt::graphslam::CWindowObserver
Monitor events in the visualization window.
Definition: CWindowObserver.h:24
mrpt::graphslam::CGraphSlamEngine::toggleEstimatedTrajectoryVisualization
void toggleEstimatedTrajectoryVisualization()
Definition: CGraphSlamEngine_impl.h:1718
mrpt::graphslam::CGraphSlamEngine::TRGBDInfoFileParams
Struct responsible for keeping the parameters of the .info file in RGBD related datasets.
Definition: CGraphSlamEngine.h:890
mrpt::graphslam::CGraphSlamEngine::getRawlogFname
std::string getRawlogFname()
Return the filename of the used rawlog file.
Definition: CGraphSlamEngine.h:305
mrpt::graphslam::CGraphSlamEngine::report_sep
static const std::string report_sep
Definition: CGraphSlamEngine.h:970
mrpt::graphslam::CGraphSlamEngine::m_offset_y_GT
double m_offset_y_GT
Definition: CGraphSlamEngine.h:789
mrpt::graphslam::CGraphSlamEngine::TRGBDInfoFileParams::initTRGBDInfoFileParams
void initTRGBDInfoFileParams()
Definition: CGraphSlamEngine_impl.h:2278
CDisplayWindow3D.h
mrpt::graphslam::CWindowManager
Class acts as a container for storing pointers to mrpt::gui::CDisplayWindow3D, mrpt::graphslam::CWind...
Definition: CWindowManager.h:27
mrpt::graphslam::CGraphSlamEngine::initRobotModelVisualizationInternal
mrpt::opengl::CSetOfObjects::Ptr initRobotModelVisualizationInternal(const mrpt::poses::CPose2D &p_unused)
Method to help overcome the partial template specialization restriction of C++.
Definition: CGraphSlamEngine_impl.h:1274
mrpt::graphslam::CGraphSlamEngine::m_optimized_map_color
mrpt::img::TColor m_optimized_map_color
Definition: CGraphSlamEngine.h:851
mrpt::graphslam::CGraphSlamEngine::initSlamMetricVisualization
void initSlamMetricVisualization()
Definition: CGraphSlamEngine_impl.h:2500
mrpt::graphslam::CGraphSlamEngine::getParamsAsString
std::string getParamsAsString() const
Wrapper around getParamsAsString.
Definition: CGraphSlamEngine_impl.h:1047
mrpt::obs::CObservation2DRangeScan::Ptr
std::shared_ptr< mrpt::obs ::CObservation2DRangeScan > Ptr
Definition: CObservation2DRangeScan.h:56
mrpt::graphslam::CGraphSlamEngine::TRGBDInfoFileParams::info_fname
std::string info_fname
Definition: CGraphSlamEngine.h:910
mrpt::graphslam::CGraphSlamEngine::m_img_prev_path_base
std::string m_img_prev_path_base
Definition: CGraphSlamEngine.h:874
mrpt::graphslam::CGraphSlamEngine::m_offset_x_left
double m_offset_x_left
Offset from the left side of the canvas.
Definition: CGraphSlamEngine.h:786
mrpt::graphslam::CGraphSlamEngine::getNodeIDsOfEstimatedTrajectory
virtual void getNodeIDsOfEstimatedTrajectory(std::set< mrpt::graphs::TNodeID > *nodes_set) const
Return the list of nodeIDs which make up robot trajectory.
Definition: CGraphSlamEngine_impl.h:85
mrpt::graphslam::CGraphSlamEngine::m_config_fname
std::string m_config_fname
Definition: CGraphSlamEngine.h:713
mrpt::graphslam::CGraphSlamEngine::updateMapVisualization
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.
Definition: CGraphSlamEngine_impl.h:1825
mrpt::graphslam::CGraphSlamEngine::initOdometryVisualization
void initOdometryVisualization()
Definition: CGraphSlamEngine_impl.h:2073
mrpt::graphslam::CGraphSlamEngine::m_current_constraint_type
std::string m_current_constraint_type
Type of constraint currently in use.
Definition: CGraphSlamEngine.h:965
mrpt::graphslam::CGraphSlamEngine::updateOdometryVisualization
void updateOdometryVisualization()
Update odometry-only cloud with latest odometry estimation.
Definition: CGraphSlamEngine_impl.h:2111
mrpt::graphslam
SLAM methods related to graphs of pose constraints.
Definition: TUserOptionsChecker.h:30
mrpt::graphslam::CGraphSlamEngine::getGraphSlamStats
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.
Definition: CGraphSlamEngine_impl.h:2604
mrpt::graphslam::CGraphSlamEngine::m_nodes_to_laser_scans2D
nodes_to_scans2D_t m_nodes_to_laser_scans2D
Map of NodeIDs to their corresponding LaserScans.
Definition: CGraphSlamEngine.h:836
mrpt::graphslam::CGraphSlamEngine::readGTFileRGBD_TUM
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.
Definition: CGraphSlamEngine_impl.h:1408
mrpt::graphslam::CGraphSlamEngine::m_simple_map_cached
mrpt::maps::CSimpleMap m_simple_map_cached
Acquired map in CSimpleMap representation.
Definition: CGraphSlamEngine.h:938
mrpt::graphslam::CGraphSlamEngine::computeMap
void computeMap() const
Compute the map of the environment based on the recorded measurements.
Definition: CGraphSlamEngine_impl.h:937
mrpt::graphslam::CGraphSlamEngine::m_text_index_GT
int m_text_index_GT
Definition: CGraphSlamEngine.h:796
mrpt::graphslam::CGraphSlamEngine::initMapVisualization
void initMapVisualization()
Definition: CGraphSlamEngine_impl.h:1812
mrpt::math::CMatrixFixed< double, 3, 3 >
mrpt::graphslam::CGraphSlamEngine::m_keystroke_odometry
std::string m_keystroke_odometry
Definition: CGraphSlamEngine.h:809
mrpt::graphslam::CGraphSlamEngine::m_offset_y_current_constraint_type
double m_offset_y_current_constraint_type
Definition: CGraphSlamEngine.h:792
mrpt::graphslam::CGraphSlamEngine::m_optimizer
mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T > * m_optimizer
Definition: CGraphSlamEngine.h:707
mrpt::graphslam::CGraphSlamEngine::m_dataset_grab_time
double m_dataset_grab_time
Time it took to record the dataset.
Definition: CGraphSlamEngine.h:918
COutputLogger.h
mrpt::graphslam::CGraphSlamEngine::m_GT_poses_step
size_t m_GT_poses_step
Rate at which to read the GT poses.
Definition: CGraphSlamEngine.h:726
mrpt::graphslam::CGraphSlamEngine::generateReportFiles
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...
Definition: CGraphSlamEngine_impl.h:2643
mrpt::graphslam::CGraphSlamEngine::initEstimatedTrajectoryVisualization
void initEstimatedTrajectoryVisualization()
Definition: CGraphSlamEngine_impl.h:2144
mrpt::graphslam::CGraphSlamEngine::readGTFile
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.
Definition: CGraphSlamEngine_impl.h:1342
mrpt::graphslam::CGraphSlamEngine::m_time_logger
mrpt::system::CTimeLogger m_time_logger
Time logger instance.
Definition: CGraphSlamEngine.h:696
mrpt::graphslam::CGraphSlamEngine::initGTVisualization
void initGTVisualization()
Definition: CGraphSlamEngine_impl.h:1993
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::graphslam::CGraphSlamEngine::m_win
mrpt::gui::CDisplayWindow3D * m_win
Definition: CGraphSlamEngine.h:741
mrpt::graphslam::CGraphSlamEngine::TRGBDInfoFileParams::parseFile
void parseFile()
Parse the RGBD information file to gain information about the rawlog file contents.
Definition: CGraphSlamEngine_impl.h:2286
mrpt::graphslam::CGraphSlamEngine::m_keystroke_GT
std::string m_keystroke_GT
Definition: CGraphSlamEngine.h:810
mrpt::graphslam::CGraphSlamEngine::m_last_laser_scan3D
mrpt::obs::CObservation3DRangeScan::Ptr m_last_laser_scan3D
Last laser scan that the current class instance received.
Definition: CGraphSlamEngine.h:844
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::graphslam::CGraphSlamEngine::m_visualize_SLAM_metric
bool m_visualize_SLAM_metric
Definition: CGraphSlamEngine.h:762
mrpt::graphslam::CGraphSlamEngine::m_enable_curr_pos_viewport
bool m_enable_curr_pos_viewport
Definition: CGraphSlamEngine.h:763
mrpt::graphslam::CGraphSlamEngine::m_request_to_exit
bool m_request_to_exit
Indicate whether the user wants to exit the application (e.g.
Definition: CGraphSlamEngine.h:930
mrpt::graphslam::CGraphSlamEngine::m_supported_constraint_types
std::vector< std::string > m_supported_constraint_types
MRPT CNetworkOfPoses constraint classes that are currently supported.
Definition: CGraphSlamEngine.h:962
mrpt::graphslam::CGraphSlamEngine::setObjectPropsFromNodeID
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.
Definition: CGraphSlamEngine_impl.h:1952
mrpt::img::TColor
A RGB color - 8bit.
Definition: TColor.h:25
mrpt::graphslam::CGraphSlamEngine::getDeformationEnergyVector
void getDeformationEnergyVector(std::vector< double > *vec_out) const
Fill the given vector with the deformation energy values computed for the SLAM evaluation metric.
Definition: CGraphSlamEngine_impl.h:2741
mrpt::system::TTimeStamp
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
mrpt::graphslam::CGraphSlamEngine::m_out_streams
fstreams_out m_out_streams
keeps track of the out fstreams so that they can be closed (if still open) in the class Dtor.
Definition: CGraphSlamEngine.h:736
mrpt::graphslam::CGraphSlamEngine::m_curr_deformation_energy
double m_curr_deformation_energy
Definition: CGraphSlamEngine.h:883
CNodeRegistrationDecider.h
mrpt::graphslam::CGraphSlamEngine::TRGBDInfoFileParams::setRawlogFile
void setRawlogFile(const std::string &rawlog_fname)
Definition: CGraphSlamEngine_impl.h:2266
mrpt::graphslam::CGraphSlamEngine::m_info_params
struct mrpt::graphslam::CGraphSlamEngine::TRGBDInfoFileParams m_info_params
mrpt::graphslam::CGraphSlamEngine::m_visualize_odometry_poses
bool m_visualize_odometry_poses
Definition: CGraphSlamEngine.h:758
mrpt::graphslam::CGraphSlamEngine::saveGraph
void saveGraph(const std::string *fname_in=nullptr) const
Wrapper method around the GRAPH_T::saveToTextFile method.
Definition: CGraphSlamEngine_impl.h:2335
mrpt::graphslam::CGraphSlamEngine::m_use_GT
bool m_use_GT
Flag for specifying if we are going to use ground truth data at all.
Definition: CGraphSlamEngine.h:826
mrpt::graphslam::CGraphSlamEngine::m_observation_only_dataset
bool m_observation_only_dataset
Definition: CGraphSlamEngine.h:731
mrpt::graphslam::CGraphSlamEngine::TRGBDInfoFileParams::fields
std::map< std::string, std::string > fields
Format for the parameters in the info file: string literal - related value (kept in a string represen...
Definition: CGraphSlamEngine.h:908
mrpt::graphslam::CGraphSlamEngine::getTimeStamp
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.
Definition: CGraphSlamEngine_impl.h:1761
mrpt::graphslam::CGraphSlamEngine::TRGBDInfoFileParams::~TRGBDInfoFileParams
~TRGBDInfoFileParams()=default
mrpt::graphslam::CGraphSlamEngine::initCurrPosViewport
void initCurrPosViewport()
Definition: CGraphSlamEngine_impl.h:1290
mrpt::graphslam::CGraphSlamEngine::m_gridmap_cached
mrpt::maps::COccupancyGridMap2D::Ptr m_gridmap_cached
Definition: CGraphSlamEngine.h:936
mrpt::graphslam::CGraphSlamEngine::updateGTVisualization
void updateGTVisualization()
Display the next ground truth position in the visualization window.
Definition: CGraphSlamEngine_impl.h:2036
mrpt::system::COutputLogger
Versatile class for consistent logging and management of output messages.
Definition: system/COutputLogger.h:117
mrpt::graphslam::CGraphSlamEngine< mrpt::graphs::CNetworkOfPoses2DInf >::global_pose_t
typename mrpt::graphs::CNetworkOfPoses2DInf ::global_pose_t global_pose_t
Definition: CGraphSlamEngine.h:152
CPose3D.h
CSensoryFrame.h
mrpt::graphslam::CGraphSlamEngine::TRGBDInfoFileParams::TRGBDInfoFileParams
TRGBDInfoFileParams()
Definition: CGraphSlamEngine_impl.h:2260
mrpt::graphslam::CGraphSlamEngine::togglePause
void togglePause()
Definition: CGraphSlamEngine.h:386
mrpt::graphslam::CGraphSlamEngine::m_keystroke_estimated_trajectory
std::string m_keystroke_estimated_trajectory
Definition: CGraphSlamEngine.h:811
CAction.h
mrpt::graphslam::CGraphSlamEngine::m_visualize_map
bool m_visualize_map
Definition: CGraphSlamEngine.h:760
mrpt::graphslam::CGraphSlamEngine::initClass
void initClass()
General initialization method to call from the Class Constructors.
Definition: CGraphSlamEngine_impl.h:93
mrpt::graphslam::CGraphSlamEngine::m_deformation_energy_vec
std::vector< double > m_deformation_energy_vec
Definition: CGraphSlamEngine.h:884
mrpt::graphslam::CGraphSlamEngine::execDijkstraNodesEstimation
void execDijkstraNodesEstimation()
Wrapper around the GRAPH_T::dijkstra_nodes_estimate.
Definition: CGraphSlamEngine_impl.h:839
mrpt::graphslam::CGraphSlamEngine::updateSlamMetricVisualization
void updateSlamMetricVisualization()
Update the displayPlots window with the new information with regards to the metric.
Definition: CGraphSlamEngine_impl.h:2522
mrpt::graphslam::CGraphSlamEngine::m_is_first_time_node_reg
bool m_is_first_time_node_reg
Track the first node registration occurance.
Definition: CGraphSlamEngine.h:957
mrpt::graphslam::CGraphSlamEngine::updateRangeImageViewport
void updateRangeImageViewport()
In RGB-D TUM Datasets update the Range image displayed in a seperate viewport.
Definition: CGraphSlamEngine_impl.h:1189
mrpt::system::upperCase
std::string upperCase(const std::string &str)
Returns a upper-case version of a string.
Definition: string_utils.cpp:40
mrpt::graphslam::CGraphSlamEngine::computeSlamMetric
void computeSlamMetric(mrpt::graphs::TNodeID nodeID, size_t gt_index)
Compare the SLAM result (estimated trajectory) with the GT path.
Definition: CGraphSlamEngine_impl.h:2409
mrpt::graphslam::CGraphSlamEngine::m_edge_counter
mrpt::graphslam::detail::CEdgeCounter m_edge_counter
Instance to keep track of all the edges + visualization related operations.
Definition: CGraphSlamEngine.h:818
mrpt::graphslam::CGraphSlamEngine::m_user_decides_about_output_dir
bool m_user_decides_about_output_dir
Definition: CGraphSlamEngine.h:728
mrpt::graphslam::CGraphSlamEngine::m_text_index_timestamp
int m_text_index_timestamp
Definition: CGraphSlamEngine.h:798
mrpt::maps::CSimpleMap
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
mrpt::graphslam::CGraphSlamEngine::m_class_name
std::string m_class_name
Definition: CGraphSlamEngine.h:952
mrpt::graphslam::CGraphSlamEngine::m_enable_visuals
const bool m_enable_visuals
Determine if we are to enable visualization support or not.
Definition: CGraphSlamEngine.h:711
mrpt::graphslam::CGraphSlamEngine::initResultsFile
void initResultsFile(const std::string &fname)
Automate the creation and initialization of a results file relevant to the application.
Definition: CGraphSlamEngine_impl.h:1120
MRPT_LOG_INFO_STREAM
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
Definition: system/COutputLogger.h:473
mrpt::graphslam::CGraphSlamEngine::m_img_external_storage_dir
std::string m_img_external_storage_dir
Definition: CGraphSlamEngine.h:873
mrpt::graphslam::CGraphSlamEngine::m_win_observer
mrpt::graphslam::CWindowObserver * m_win_observer
Definition: CGraphSlamEngine.h:742
mrpt::maps::COccupancyGridMap2D::Ptr
std::shared_ptr< mrpt::maps ::COccupancyGridMap2D > Ptr
Definition: COccupancyGridMap2D.h:57
mrpt::graphslam::CGraphSlamEngine::m_GT_poses_index
size_t m_GT_poses_index
Counter for reading back the GT_poses.
Definition: CGraphSlamEngine.h:724
mrpt::graphslam::CGraphSlamEngine::setCurrentPositionModel
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.
Definition: CGraphSlamEngine_impl.h:2726
CEdgeCounter.h
mrpt::graphslam::CGraphSlamEngine< mrpt::graphs::CNetworkOfPoses2DInf >::fstreams_out_it
typename fstreams_out::iterator fstreams_out_it
Map for iterating over output file streams.
Definition: CGraphSlamEngine.h:146
mrpt::graphslam::CGraphSlamEngine::m_enable_intensity_viewport
bool m_enable_intensity_viewport
Definition: CGraphSlamEngine.h:764
mrpt::graphslam::CGraphSlamEngine::CGraphSlamEngine
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.
Definition: CGraphSlamEngine_impl.h:28
mrpt::graphslam::CGraphSlamEngine::m_win_plot
std::unique_ptr< mrpt::gui::CDisplayWindowPlots > m_win_plot
DisplayPlots instance for visualizing the evolution of the SLAM metric.
Definition: CGraphSlamEngine.h:746
CDisplayWindowPlots.h
CSimpleMap.h
mrpt::graphslam::CGraphSlamEngine::m_rawlog_fname
std::string m_rawlog_fname
Rawlog file from which to read the measurements.
Definition: CGraphSlamEngine.h:719
mrpt::graphslam::CGraphSlamEngine::m_offset_y_timestamp
double m_offset_y_timestamp
Definition: CGraphSlamEngine.h:791
mrpt::graphslam::CGraphSlamEngine::m_init_timestamp
mrpt::system::TTimeStamp m_init_timestamp
First recorded timestamp in the dataset.
Definition: CGraphSlamEngine.h:921
mrpt::graphslam::CGraphSlamEngine::getRobotEstimatedTrajectory
virtual GRAPH_T::global_poses_t getRobotEstimatedTrajectory() const
Definition: CGraphSlamEngine_impl.h:78
mrpt::graphslam::CGraphSlamEngine::save3DScene
void save3DScene(const std::string *fname_in=nullptr) const
Wrapper method around the COpenGLScene::saveToFile method.
Definition: CGraphSlamEngine_impl.h:2358
mrpt::graphslam::CGraphSlamEngine< mrpt::graphs::CNetworkOfPoses2DInf >::fstreams_out
std::map< std::string, mrpt::io::CFileOutputStream > fstreams_out
Map for managing output file streams.
Definition: CGraphSlamEngine.h:144
mrpt::graphslam::CGraphSlamEngine::m_visualize_estimated_trajectory
bool m_visualize_estimated_trajectory
Definition: CGraphSlamEngine.h:761
mrpt::obs::CObservation3DRangeScan::Ptr
std::shared_ptr< mrpt::obs ::CObservation3DRangeScan > Ptr
Definition: CObservation3DRangeScan.h:170
mrpt::graphslam::CGraphSlamEngine::m_text_index_current_constraint_type
int m_text_index_current_constraint_type
Definition: CGraphSlamEngine.h:799
mrpt::graphslam::CGraphSlamEngine::m_odometry_poses
std::vector< pose_t > m_odometry_poses
Definition: CGraphSlamEngine.h:829
mrpt::gui::CDisplayWindow3D
A graphical user interface (GUI) for efficiently rendering 3D scenes in real-time.
Definition: CDisplayWindow3D.h:117
mrpt::graphslam::CGraphSlamEngine< mrpt::graphs::CNetworkOfPoses2DInf >::nodes_to_scans2D_t
std::map< mrpt::graphs::TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr > nodes_to_scans2D_t
Definition: CGraphSlamEngine.h:154
TColor.h
mrpt::graphslam::CGraphSlamEngine::m_curr_timestamp
mrpt::system::TTimeStamp m_curr_timestamp
Current dataset timestamp.
Definition: CGraphSlamEngine.h:923
mrpt::graphslam::CGraphSlamEngine::m_odometry_color
mrpt::img::TColor m_odometry_color
Definition: CGraphSlamEngine.h:848
mrpt::graphslam::CGraphSlamEngine::initRangeImageViewport
void initRangeImageViewport()
Definition: CGraphSlamEngine_impl.h:1153
mrpt::graphslam::CGraphSlamEngine
Main file for the GraphSlamEngine.
Definition: CGraphSlamEngine.h:140
CObservation2DRangeScan.h
mrpt::graphslam::CGraphSlamEngine::m_map_acq_time
mrpt::system::TTimeStamp m_map_acq_time
Timestamp at which the map was computed.
Definition: CGraphSlamEngine.h:949
mrpt::graphslam::CGraphSlamEngine::getCurrentRobotPosEstimation
global_pose_t getCurrentRobotPosEstimation() const
Definition: CGraphSlamEngine_impl.h:70
mrpt::graphslam::CGraphSlamEngine::dumpVisibilityErrorMsg
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
Wrapper method that used for printing error messages in a consistent manner.
Definition: CGraphSlamEngine_impl.h:1746
mrpt::graphslam::CGraphSlamEngine::m_GT_color
mrpt::img::TColor m_GT_color
Definition: CGraphSlamEngine.h:849
mrpt::graphslam::CGraphSlamEngine::updateIntensityImageViewport
void updateIntensityImageViewport()
In RGB-D TUM Datasets update the Intensity image displayed in a seperate viewport.
Definition: CGraphSlamEngine_impl.h:1239
mrpt::graphslam::detail::CEdgeCounter
Generic class for tracking the total number of edges for different tpes of edges and for storing visu...
Definition: CEdgeCounter.h:27
mrpt::graphslam::CGraphSlamEngine::initVisualization
void initVisualization()
mrpt::graphslam::CGraphSlamEngine::m_win_manager
mrpt::graphslam::CWindowManager * m_win_manager
Definition: CGraphSlamEngine.h:740



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Thu May 21 21:53:32 UTC 2020