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



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