Main MRPT website > C++ reference for MRPT 1.5.6
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 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 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...
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.
void printParams() const
Print the problem parameters to the console for verification.
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.
Interface for implementing edge registration classes.
virtual mrpt::poses::CPose3D getLSPoseForGridMapVisualization(const mrpt::utils::TNodeID nodeID) const
return the 3D Pose of a LaserScan that is to be visualized.
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
void getMap(mrpt::maps::COccupancyGridMap2DPtr map, mrpt::system::TTimeStamp *acquisition_time=NULL) const
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.
void saveGraph(const std::string *fname_in=NULL) const
Wrapper method around the GRAPH_T::saveToTextFile method.
mrpt::utils::TNodeID m_nodeID_max
Internal counter for querying for the number of nodeIDs.
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the class state.
std::string m_rawlog_fname
Rawlog file from which to read the measurements.
void computeMap() const
Compute the map of the environment based on the recorded measurements.
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 save3DScene(const std::string *fname_in=NULL) const
Wrapper method around the COpenGLScene::saveToFile method.
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.
virtual void getRobotEstimatedTrajectory(typename GRAPH_T::global_poses_t *graph_poses) const
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.
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.
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 getDeformationEnergyVector(std::vector< double > *vec_out) const
Fill the given vector with the deformation energy values computed for the SLAM evaluation metric...
mrpt::opengl::CSetOfObjectsPtr initRobotModelVisualization()
mrpt::graphslam::CWindowManager * m_win_manager
std::string getParamsAsString() const
Wrapper around getParamsAsString.
mrpt::graphslam::detail::CEdgeCounter m_edge_counter
Instance to keep track of all the edges + visualization related operations.
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)
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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.
virtual void getNodeIDsOfEstimatedTrajectory(std::set< mrpt::utils::TNodeID > *nodes_set) const
Return the list of nodeIDs which make up robot trajectory.
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.
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.
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.
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
global_pose_t getCurrentRobotPosEstimation() const
Query CGraphSlamEngine instance for the current estimated robot position.
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...
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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018