MRPT  2.0.1
CICPCriteriaERD.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #pragma once
11 
14 #include <mrpt/img/TColor.h>
18 #include <mrpt/obs/CSensoryFrame.h>
19 #include <mrpt/slam/CICP.h>
20 
21 #include <map>
22 #include <set>
23 #include <string>
24 
26 
28 {
29 /**\brief ICP-based Edge Registration.
30  *
31  * ## Description
32  *
33  * Register new edges in the graph with the last inserted node. Criterion for
34  * adding new edges should be the goodness of the candidate ICP edge. The
35  * nodes for ICP are picked <em>based on the distance from the last
36  * inserted node</em>.
37  * \sa getNearbyNodesOf
38  *
39  * ### Specifications
40  *
41  * - Map type: 2D
42  * - MRPT rawlog format: #1, #2
43  * - Graph Type: CPosePDFGaussianInf
44  * - Observations: CObservation2DRangeScan, CObservation3DRangeScan
45  * - Edge Registration Strategy: ICP Goodnesss threshold
46  *
47  * ### .ini Configuration Parameters
48  *
49  * \htmlinclude graphslam-engine_config_params_preamble.txt
50  *
51  * - \b class_verbosity
52  * + \a Section : EdgeRegistrationDeciderParameters
53  * + \a default value : 1 (mrpt::system::LVL_INFO)
54  * + \a Required : FALSE
55  *
56  * - \b LC_min_nodeid_diff
57  * + \a Section : GeneralConfiguration
58  * + \a Default value : 30
59  * + \a Required : FALSE
60  * + \a Description : Minimum NodeID difference for an edge to be considered
61  * a loop closure.
62  *
63  * - \b ICP_max_distance
64  * + \a Section : EdgeRegistrationDeciderParameters
65  * + \a Default value : 10 // meters
66  * + \a Required : FALSE
67  * + \a Description : Maximum distance for scan-matching. Decider tries to
68  * align the laser scans of the current node and each of the previous nodes
69  * that are found within the designated ICP_max_distance.
70  *
71  * - \b ICP_goodness_thresh
72  * + \a Section : EdgeRegistrationDeciderParameters
73  * + \a Default value : 0.75
74  * + \a Required : FALSE
75  * + \a Description : Threshold for accepting a scan-matching edge between
76  * the current and previous nodes
77  *
78  * - \b visualize_laser_scans
79  * + \a Section : VisualizationParameters
80  * + \a Default value : TRUE
81  * + \a Required : FALSE
82  *
83  * - \b scans_img_external_dir
84  * + \a Section : EdgeRegistrationDeciderParameters
85  * + \a Default value : . (current directory)
86  * + \a Required : FALSE
87  * + \a Description : Only applicable in datasets with 3DRangeScans that
88  * are externally stored (not stored in the given .rawlog file).
89  *
90  * \ingroup mrpt_graphslam_grp
91  */
92 template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
95  GRAPH_T>
96 {
97  public:
98  /**\brief Handy typedefs */
99  /**\{*/
100  /**\brief type of graph constraints */
101  using constraint_t = typename GRAPH_T::constraint_t;
102  /**\brief type of underlying poses (2D/3D). */
103  using pose_t = typename GRAPH_T::constraint_t::type_value;
106  /** self type - Handy type */
109  /**\}*/
110 
111  // Public methods
112  //////////////////////////////////////////////////////////////
113  CICPCriteriaERD();
114  ~CICPCriteriaERD() override = default;
115 
116  bool updateState(
118  mrpt::obs::CSensoryFrame::Ptr observations,
119  mrpt::obs::CObservation::Ptr observation) override;
120 
122  const std::map<std::string, bool>& events_occurred) override;
123  void getEdgesStats(
124  std::map<std::string, int>* edge_types_to_num) const override;
125 
126  void initializeVisuals() override;
127  void updateVisuals() override;
128  void loadParams(const std::string& source_fname) override;
129  void printParams() const override;
130 
132  {
133  public:
134  TParams(decider_t& d);
135  ~TParams() override;
136 
137  void loadFromConfigFile(
138  const mrpt::config::CConfigFileBase& source,
139  const std::string& section) override;
140  void dumpToTextStream(std::ostream& out) const override;
141 
143  // maximum distance for checking other nodes for ICP constraints
145  // threshold for accepting an ICP constraint in the graph
149  // keystroke to be used for the user to toggle the LaserScans from
150  // the CDisplayWindow
152 
154 
156  };
157  void getDescriptiveReport(std::string* report_str) const override;
158 
159  // Public variables
160  // ////////////////////////////
162 
163  protected:
164  // protected functions
165  //////////////////////////////////////////////////////////////
167  const std::set<mrpt::graphs::TNodeID>& nodes_set);
169  const std::set<mrpt::graphs::TNodeID>& nodes_set);
170  void registerNewEdge(
171  const mrpt::graphs::TNodeID& from, const mrpt::graphs::TNodeID& to,
172  const constraint_t& rel_edge) override;
175  mrpt::obs::CSensoryFrame::Ptr observations,
176  mrpt::obs::CObservation::Ptr observation);
177  /**\brief Get a list of the nodeIDs whose position is within a certain
178  * distance to the specified nodeID
179  */
180  void getNearbyNodesOf(
181  std::set<mrpt::graphs::TNodeID>* nodes_set,
182  const mrpt::graphs::TNodeID& cur_nodeID, double distance);
183  /**\brief togle the LaserScans visualization on and off
184  */
187  std::string viz_flag, int sleep_time = 500 /* ms */);
188 
189  // protected variables
190  //////////////////////////////////////////////////////////////
191 
192  /** see Constructor for initialization */
194  /** see Constructor for initialization */
198 
199  std::map<mrpt::graphs::TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr>
201  std::map<mrpt::graphs::TNodeID, mrpt::obs::CObservation3DRangeScan::Ptr>
203  std::map<std::string, int> m_edge_types_to_nums;
204  bool m_is_using_3DScan{false};
205 
208  // fake 2D laser scan generated from corresponding 3DRangeScan for
209  // visualization reasons
211 };
212 } // namespace mrpt::graphslam::deciders
213 #include "CICPCriteriaERD_impl.h"
mrpt::img::TColor m_search_disk_color
see Constructor for initialization
typename parent_t::range_ops_t range_ops_t
void checkRegistrationCondition2D(const std::set< mrpt::graphs::TNodeID > &nodes_set)
typename GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
mrpt::img::TColor m_laser_scans_color
see Constructor for initialization
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
void loadParams(const std::string &source_fname) override
Load the necessary for the decider/optimizer configuration parameters.
void updateVisuals() override
Update the relevant visual features in CDisplayWindow.
void printParams() const override
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
mrpt::obs::CObservation2DRangeScan::Ptr m_last_laser_scan2D
Edge Registration Decider Interface from which RangeScanner-based ERDs can inherit from...
mrpt::obs::CObservation3DRangeScan::Ptr m_last_laser_scan3D
void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred) override
Get a list of the window events that happened since the last call.
void toggleLaserScansVisualization()
togle the LaserScans visualization on and off
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
This class allows loading and storing values and vectors of different types from a configuration text...
std::map< mrpt::graphs::TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr > m_nodes_to_laser_scans2D
void checkRegistrationCondition3D(const std::set< mrpt::graphs::TNodeID > &nodes_set)
void checkIfInvalidDataset(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation)
void getEdgesStats(std::map< std::string, int > *edge_types_to_num) const override
Fill the given map with the type of registered edges as well as the corresponding number of registrat...
void getDescriptiveReport(std::string *report_str) const override
Fill the provided string with a detailed report of the decider/optimizer state.
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation) override
Generic method for fetching the incremental action-observations (or observation-only) measurements...
std::map< std::string, int > m_edge_types_to_nums
void registerNewEdge(const mrpt::graphs::TNodeID &from, const mrpt::graphs::TNodeID &to, const constraint_t &rel_edge) override
Register a new constraint/edge in the current graph.
mrpt::vision::TStereoCalibResults out
void initializeVisuals() override
Initialize visual objects in CDisplayWindow (e.g.
mrpt::obs::CObservation2DRangeScan::Ptr m_fake_laser_scan2D
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
mrpt::graphslam::deciders::CRangeScanOps< GRAPH_T > range_ops_t
Typedef for accessing methods of the RangeScanRegistrationDecider_t parent class. ...
std::map< mrpt::graphs::TNodeID, mrpt::obs::CObservation3DRangeScan::Ptr > m_nodes_to_laser_scans3D
void getNearbyNodesOf(std::set< mrpt::graphs::TNodeID > *nodes_set, const mrpt::graphs::TNodeID &cur_nodeID, double distance)
Get a list of the nodeIDs whose position is within a certain distance to the specified nodeID...
A RGB color - 8bit.
Definition: TColor.h:25
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1807
typename parent_t::nodes_to_scans2D_t nodes_to_scans2D_t
std::map< mrpt::graphs::TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr > nodes_to_scans2D_t
typename GRAPH_T::constraint_t constraint_t
Handy typedefs.



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020