Main MRPT website > C++ reference for MRPT 1.5.7
CICPCriteriaERD.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 CICPCRITERIAERD_H
11 #define CICPCRITERIAERD_H
12 
13 #include <mrpt/math/CMatrix.h>
14 #include <mrpt/math/utils.h>
15 #include <mrpt/utils/CImage.h>
17 #include <mrpt/utils/CConfigFile.h>
19 #include <mrpt/utils/CTimeLogger.h>
20 #include <mrpt/utils/CStream.h>
22 #include <mrpt/utils/TColor.h>
26 #include <mrpt/obs/CSensoryFrame.h>
27 #include <mrpt/obs/CRawlog.h>
28 #include <mrpt/opengl/CDisk.h>
31 #include <mrpt/slam/CICP.h>
32 #include <mrpt/system/os.h>
33 #include <mrpt/system/filesystem.h>
34 #include <mrpt/system/threads.h>
35 
36 #include <stdlib.h> // abs
37 
38 #include <iostream>
39 #include <map>
40 #include <string>
41 
43 
44 
45 namespace mrpt { namespace graphslam { namespace deciders {
46 
47 /**\brief ICP-based Edge Registration.
48  *
49  * ## Description
50  *
51  * Register new edges in the graph with the last inserted node. Criterion for
52  * adding new edges should be the goodness of the candidate ICP edge. The
53  * nodes for ICP are picked <em>based on the distance from the last
54  * inserted node</em>.
55  * \sa getNearbyNodesOf
56  *
57  * ### Specifications
58  *
59  * - Map type: 2D
60  * - MRPT rawlog format: #1, #2
61  * - Graph Type: CPosePDFGaussianInf
62  * - Observations: CObservation2DRangeScan, CObservation3DRangeScan
63  * - Edge Registration Strategy: ICP Goodnesss threshold
64  *
65  * ### .ini Configuration Parameters
66  *
67  * \htmlinclude graphslam-engine_config_params_preamble.txt
68  *
69  * - \b class_verbosity
70  * + \a Section : EdgeRegistrationDeciderParameters
71  * + \a default value : 1 (LVL_INFO)
72  * + \a Required : FALSE
73  *
74  * - \b LC_min_nodeid_diff
75  * + \a Section : GeneralConfiguration
76  * + \a Default value : 30
77  * + \a Required : FALSE
78  * + \a Description : Minimum NodeID difference for an edge to be considered
79  * a loop closure.
80  *
81  * - \b ICP_max_distance
82  * + \a Section : EdgeRegistrationDeciderParameters
83  * + \a Default value : 10 // meters
84  * + \a Required : FALSE
85  * + \a Description : Maximum distance for scan-matching. Decider tries to
86  * align the laser scans of the current node and each of the previous nodes
87  * that are found within the designated ICP_max_distance.
88  *
89  * - \b ICP_goodness_thresh
90  * + \a Section : EdgeRegistrationDeciderParameters
91  * + \a Default value : 0.75
92  * + \a Required : FALSE
93  * + \a Description : Threshold for accepting a scan-matching edge between
94  * the current and previous nodes
95  *
96  * - \b visualize_laser_scans
97  * + \a Section : VisualizationParameters
98  * + \a Default value : TRUE
99  * + \a Required : FALSE
100  *
101  * - \b scans_img_external_dir
102  * + \a Section : EdgeRegistrationDeciderParameters
103  * + \a Default value : . (current directory)
104  * + \a Required : FALSE
105  * + \a Description : Only applicable in datasets with 3DRangeScans that
106  * are externally stored (not stored in the given .rawlog file).
107  *
108  * \ingroup mrpt_graphslam_grp
109  */
110 template<class GRAPH_T=typename mrpt::graphs::CNetworkOfPoses2DInf >
113 {
114  public:
115  /**\brief Handy typedefs */
116  /**\{*/
117  /**\brief type of graph constraints */
118  typedef typename GRAPH_T::constraint_t constraint_t;
119  /**\brief type of underlying poses (2D/3D). */
120  typedef typename GRAPH_T::constraint_t::type_value pose_t;
123  typedef CICPCriteriaERD<GRAPH_T> decider_t; /**< self type - Handy typedef */
125  /**\}*/
126 
127  // Public methods
128  //////////////////////////////////////////////////////////////
129  CICPCriteriaERD();
131 
132  bool updateState(
133  mrpt::obs::CActionCollectionPtr action,
134  mrpt::obs::CSensoryFramePtr observations,
135  mrpt::obs::CObservationPtr observation );
136 
138  const std::map<std::string, bool>& events_occurred);
139  void getEdgesStats(
140  std::map<std::string, int>* edge_types_to_num) const;
141 
142  void initializeVisuals();
143  void updateVisuals();
144  void loadParams(const std::string& source_fname);
145  void printParams() const;
146 
148  public:
149  TParams(decider_t& d);
150  ~TParams();
151 
152 
153  void loadFromConfigFile(
155  const std::string &section);
156  void dumpToTextStream(mrpt::utils::CStream &out) const;
157 
159  // maximum distance for checking other nodes for ICP constraints
161  // threshold for accepting an ICP constraint in the graph
165  // keystroke to be used for the user to toggle the LaserScans from
166  // the CDisplayWindow
168 
170 
172  };
173  void getDescriptiveReport(std::string* report_str) const;
174 
175  // Public variables
176  // ////////////////////////////
178 
179  protected:
180  // protected functions
181  //////////////////////////////////////////////////////////////
183  const std::set<mrpt::utils::TNodeID>& nodes_set);
185  const std::set<mrpt::utils::TNodeID>& nodes_set);
186  void registerNewEdge(
187  const mrpt::utils::TNodeID& from,
188  const mrpt::utils::TNodeID& to,
189  const constraint_t& rel_edge );
190  void checkIfInvalidDataset(mrpt::obs::CActionCollectionPtr action,
191  mrpt::obs::CSensoryFramePtr observations,
192  mrpt::obs::CObservationPtr observation );
193  /**\brief Get a list of the nodeIDs whose position is within a certain
194  * distance to the specified nodeID
195  */
196  void getNearbyNodesOf(
197  std::set<mrpt::utils::TNodeID> *nodes_set,
198  const mrpt::utils::TNodeID& cur_nodeID,
199  double distance );
200  /**\brief togle the LaserScans visualization on and off
201  */
203  void dumpVisibilityErrorMsg(std::string viz_flag,
204  int sleep_time=500 /* ms */);
205 
206  // protected variables
207  //////////////////////////////////////////////////////////////
208 
209 
210  mrpt::utils::TColor m_search_disk_color; //!< see Constructor for initialization
211  mrpt::utils::TColor m_laser_scans_color; //!< see Constructor for initialization
214 
215 
216  std::map<mrpt::utils::TNodeID,
217  mrpt::obs::CObservation2DRangeScanPtr> m_nodes_to_laser_scans2D;
218  std::map<mrpt::utils::TNodeID,
219  mrpt::obs::CObservation3DRangeScanPtr> m_nodes_to_laser_scans3D;
220  std::map<std::string, int> m_edge_types_to_nums;
222 
223  mrpt::obs::CObservation2DRangeScanPtr m_last_laser_scan2D;
224  mrpt::obs::CObservation3DRangeScanPtr m_last_laser_scan3D;
225  // fake 2D laser scan generated from corresponding 3DRangeScan for
226  // visualization reasons
227  mrpt::obs::CObservation2DRangeScanPtr m_fake_laser_scan2D;
228 
229 };
230 
231 } } } // end of namespaces
232 
233 #include "CICPCriteriaERD_impl.h"
234 #endif /* end of include guard: CICPCRITERIAERD_H */
std::map< mrpt::utils::TNodeID, mrpt::obs::CObservation2DRangeScanPtr > m_nodes_to_laser_scans2D
std::map< mrpt::utils::TNodeID, mrpt::obs::CObservation2DRangeScanPtr > nodes_to_scans2D_t
void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred)
Get a list of the window events that happened since the last call.
bool updateState(mrpt::obs::CActionCollectionPtr action, mrpt::obs::CSensoryFramePtr observations, mrpt::obs::CObservationPtr observation)
Generic method for fetching the incremental action/observation readings from the calling function...
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
void checkRegistrationCondition2D(const std::set< mrpt::utils::TNodeID > &nodes_set)
GRAPH_T::constraint_t constraint_t
Handy typedefs.
mrpt::utils::TColor m_search_disk_color
see Constructor for initialization
Edge Registration Decider Interface from which RangeScanner-based ERDs can inherit from...
void toggleLaserScansVisualization()
togle the LaserScans visualization on and off
This class allows loading and storing values and vectors of different types from a configuration text...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void checkIfInvalidDataset(mrpt::obs::CActionCollectionPtr action, mrpt::obs::CSensoryFramePtr observations, mrpt::obs::CObservationPtr observation)
uint64_t TNodeID
The type for node IDs in graphs of different types.
mrpt::obs::CObservation2DRangeScanPtr m_last_laser_scan2D
mrpt::obs::CObservation2DRangeScanPtr m_fake_laser_scan2D
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
A RGB color - 8bit.
Definition: TColor.h:26
GLsizei const GLchar ** string
Definition: glext.h:3919
void dumpToTextStream(mrpt::utils::CStream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
mrpt::utils::TColor m_laser_scans_color
see Constructor for initialization
Class for keeping together all the RangeScanner-related functions.
Definition: CRangeScanOps.h:81
std::map< std::string, int > m_edge_types_to_nums
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void registerNewEdge(const mrpt::utils::TNodeID &from, const mrpt::utils::TNodeID &to, const constraint_t &rel_edge)
Register a new constraint/edge in the current graph.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
CRangeScanEdgeRegistrationDecider< GRAPH_T > parent_t
parent_t::nodes_to_scans2D_t nodes_to_scans2D_t
std::map< mrpt::utils::TNodeID, mrpt::obs::CObservation3DRangeScanPtr > m_nodes_to_laser_scans3D
void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
void getNearbyNodesOf(std::set< mrpt::utils::TNodeID > *nodes_set, const mrpt::utils::TNodeID &cur_nodeID, double distance)
Get a list of the nodeIDs whose position is within a certain distance to the specified nodeID...
CICPCriteriaERD< GRAPH_T > decider_t
self type - Handy typedef
GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
void updateVisuals()
Update the relevant visual features in CDisplayWindow.
void getEdgesStats(std::map< std::string, int > *edge_types_to_num) const
Fill the given map with the type of registered edges as well as the corresponding number of registrat...
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1504
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
void loadParams(const std::string &source_fname)
Fetch the latest observation that the current instance received (most probably during a call to the u...
mrpt::obs::CObservation3DRangeScanPtr m_last_laser_scan3D
void initializeVisuals()
Initialize visual objects in CDisplayWindow (e.g.
void checkRegistrationCondition3D(const std::set< mrpt::utils::TNodeID > &nodes_set)



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019