MRPT  1.9.9
CICPCriteriaNRD.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 
17 #include <mrpt/obs/CSensoryFrame.h>
18 
22 
23 #include <string>
24 
26 {
27 /**\brief ICP-based Fixed Intervals Node Registration
28  *
29  * ## Description
30  *
31  * Current Decider is meant for adding nodes in 2D datasets recorded using
32  * a laser range finder or RGB-D camera (e.g. Kinect). No odometry data from
33  * encoders is needed. Using ICP to match consecutive RangeScan measurements,
34  * the decider keeps track of the pose transformation since the last registered
35  * node. If the norm or the angle of the latter surpasses certain thresholds
36  * (which are read from an external .ini file) then a new node is added to the
37  * graph)
38  * \sa loadParams, TParams::loadFromConfigFile
39  *
40  * Decider *does not guarantee* thread safety when accessing the GRAPH_T
41  * resource. This is handled by the CGraphSlamEngine class.
42  *
43  * ### Specifications
44  *
45  * - Map type: 2D
46  * - MRPT rawlog format: #1, #2
47  * - Graph Type: CPosePDFGaussianInf
48  * - Observations Used: CObservation2DRangeScan, CObservation3DRangeScan
49  * - Node Registration Strategy: Fixed Intervals
50  *
51  * ### .ini Configuration Parameters
52  *
53  * \htmlinclude config_params_preamble.txt
54  *
55  * - \b class_verbosity
56  * + \a Section : NodeRegistrationDeciderParameters
57  * + \a default value : 1 (mrpt::system::LVL_INFO)
58  * + \a Required : FALSE
59  *
60  * - \b registration_max_distance
61  * + \a Section : NodeRegistrationDeciderParameters
62  * + \a Default value : 0.5 // meters
63  * + \a Required : FALSE
64  *
65  * - \b registration_max_angle
66  * + \a Section : NodeRegistrationDeciderParameters
67  * + \a Default value : 10 // degrees
68  * + \a Required : FALSE
69  *
70  * \note Since the decider inherits from the CRangeScanOps
71  * class, it parses the configuration parameters of the latter as well from the
72  * "ICP" section. Refer to the CRangeScanOps documentation for
73  * its list of configuration
74  * parameters
75  *
76  * \note Class contains an instance of the TSlidingWindow class and it parses
77  * the configuration parameters of the latter from the
78  * "NodeRegistrationDeciderParameters" section. Refer to TSlidingWindow
79  * documentation for its list of configuration parameters
80  *
81  * \ingroup mrpt_graphslam_grp
82  */
83 template <class GRAPH_T>
86  GRAPH_T>,
88 {
89  public:
90  /**\brief Handy typedefs */
91  /**\{*/
92  /**\brief type of graph constraints */
93  using constraint_t = typename GRAPH_T::constraint_t;
94  /**\brief type of underlying poses (2D/3D). */
95  using pose_t = typename GRAPH_T::constraint_t::type_value;
96  using global_pose_t = typename GRAPH_T::global_pose_t;
97 
99  double, constraint_t::state_length, constraint_t::state_length>;
100  /**\brief Typedef for accessing methods of the RangeScanRegistrationDecider
101  * parent class.
102  */
104  using decider_t = CICPCriteriaNRD<GRAPH_T>; /**< self type */
105  /**\brief Node Registration Decider */
106  using parent_t =
108  /**\}*/
109 
110  CICPCriteriaNRD();
111  ~CICPCriteriaNRD() override = default;
112 
113  void loadParams(const std::string& source_fname) override;
114  void printParams() const override;
115  void getDescriptiveReport(std::string* report_str) const override;
116 
117  /**\brief Update the decider state using the latest dataset measurements.
118  *
119  * \note Depending on the observations at hand, update of the state is
120  * handled either by updateState2D, or by updateState3D methods. This
121  * helps in separating the 2D, 3D RangeScans handling altogether, which in
122  * turn simplifies the overall procedure
123  *
124  * Order of calls:
125  * updateState (calls) ==> updateState2D/3D ==>
126  * checkRegistrationCondition2D/3D ==> CheckRegistrationCondition
127  *
128  * \sa updateState2D, updateState3D
129  */
130  bool updateState(
132  mrpt::obs::CSensoryFrame::Ptr observations,
133  mrpt::obs::CObservation::Ptr observation) override;
134  /**\brief Specialized updateState method used solely when dealing with
135  * 2DRangeScan information.
136  * \sa updateState3D
137  */
139  /**\brief Specialized updateState method used solely when dealing with
140  * 3DRangeScan information.
141  * \sa updateState2D
142  */
144 
146  {
147  public:
148  TParams(decider_t& d);
149  ~TParams() override = default;
150 
151  decider_t& decider; /**< Reference to outer decider class */
152 
153  void loadFromConfigFile(
154  const mrpt::config::CConfigFileBase& source,
155  const std::string& section) override;
156  void dumpToTextStream(std::ostream& out) const override;
157  /** Maximum distance for new node registration */
159  /** Maximum angle difference for new node registration */
161  };
162 
164 
165  protected:
166  bool checkRegistrationCondition() override;
167  /**\brief Specialized checkRegistrationCondtion method used solely when
168  * dealing with 2DRangeScan information
169  * \sa checkRegistrationCondition3D
170  */
172  /**\brief Specialized checkRegistrationCondition method used solely when
173  * dealing with 3DRangeScan information
174  * \sa checkRegistrationCondition2D
175  */
177 
179 
180  /**\brief handy laser scans to use in the class methods
181  */
182  /**\{ */
183  /**\brief 2D LaserScan corresponding to the latest registered node in the
184  * graph */
186  /**\brief Current LaserScan. Set during the new measurements acquisition in
187  * updateState method
188  */
190 
193  /**\} */
194 
195  /**\brief Odometry rigid-body transformation since the last accepted
196  * LaserScan.
197  *
198  * Decider can use it to smoothen the trajectory in the case of high noise
199  * in the laser measurements
200  */
202  /**\brief pose_t estimation using only odometry information.
203  * \note Utilized only in observation-only rawlogs.
204  *
205  */
207  /**\brief pose_t estimation using only odometry information.
208  * \note Utilized only in observation-only rawlogs.
209  *
210  * Resets next time an ICP edge/Odometry measurement is utilized for
211  * updating the estimated robot position.
212  */
214  /**\brief Keeps track of the last N measurements between the ICP edge and
215  * the corresponding odometry measurements.
216  *
217  * Use the last odometry rigid body transformation instead of the
218  * ICP edge if the mahalanobis distance between them is greater than this
219  * limit.
220  */
222 
223  // criteria for adding new a new node
226 
227  /**How many times we used the ICP Edge instead of Odometry edge*/
229  /**How many times we used the Odometry Edge instead of the ICP edge */
231 };
232 } // namespace mrpt::graphslam::deciders
233 #include "CICPCriteriaNRD_impl.h"
int m_times_used_ICP
How many times we used the ICP Edge instead of Odometry edge.
mrpt::obs::CObservation2DRangeScan::Ptr m_last_laser_scan2D
handy laser scans to use in the class methods
decider_t & decider
Reference to outer decider class.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
bool updateState2D(mrpt::obs::CObservation2DRangeScan::Ptr observation)
Specialized updateState method used solely when dealing with 2DRangeScan information.
Interface for implementing node registration classes.
pose_t m_curr_odometry_only_pose
pose_t estimation using only odometry information.
pose_t m_last_odometry_only_pose
pose_t estimation using only odometry information.
mrpt::obs::CObservation2DRangeScan::Ptr m_curr_laser_scan2D
Current LaserScan.
ICP-based Fixed Intervals Node Registration.
bool checkRegistrationCondition() override
Check whether a new node should be registered in the graph.
double registration_max_distance
Maximum distance for new node registration.
This class allows loading and storing values and vectors of different types from a configuration text...
void getDescriptiveReport(std::string *report_str) const override
Fill the provided string with a detailed report of the decider/optimizer state.
double registration_max_angle
Maximum angle difference for new node registration.
Interface for implementing node/edge registration deciders or optimizer classes.
mrpt::obs::CObservation3DRangeScan::Ptr m_curr_laser_scan3D
constraint_t m_latest_odometry_PDF
Odometry rigid-body transformation since the last accepted LaserScan.
TSlidingWindow m_mahal_distance_ICP_odom
Keeps track of the last N measurements between the ICP edge and the corresponding odometry measuremen...
mrpt::obs::CObservation3DRangeScan::Ptr m_last_laser_scan3D
Class for keeping together all the RangeScanner-related functions.
Definition: CRangeScanOps.h:71
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation) override
Update the decider state using the latest dataset measurements.
bool checkRegistrationCondition2D()
Specialized checkRegistrationCondtion method used solely when dealing with 2DRangeScan information...
typename GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
mrpt::vision::TStereoCalibResults out
Class to monitor the evolution of a statistical quantity.
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.
bool checkRegistrationCondition3D()
Specialized checkRegistrationCondition method used solely when dealing with 3DRangeScan information...
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 printParams() const override
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
typename GRAPH_T::constraint_t constraint_t
type of graph constraints
void loadParams(const std::string &source_fname) override
Load the necessary for the decider/optimizer configuration parameters.
int m_times_used_odom
How many times we used the Odometry Edge instead of the ICP edge.
bool updateState3D(mrpt::obs::CObservation3DRangeScan::Ptr observation)
Specialized updateState method used solely when dealing with 3DRangeScan information.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020