MRPT  2.0.4
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"
mrpt::obs::CObservation::Ptr
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:45
mrpt::graphslam::deciders::CNodeRegistrationDecider
Interface for implementing node registration classes.
Definition: CNodeRegistrationDecider.h:32
mrpt::graphslam::deciders::CICPCriteriaNRD::TParams::registration_max_distance
double registration_max_distance
Maximum distance for new node registration.
Definition: CICPCriteriaNRD.h:158
mrpt::graphslam::deciders::CICPCriteriaNRD::TParams
Definition: CICPCriteriaNRD.h:145
mrpt::graphslam::deciders::CICPCriteriaNRD::checkRegistrationCondition2D
bool checkRegistrationCondition2D()
Specialized checkRegistrationCondtion method used solely when dealing with 2DRangeScan information.
Definition: CICPCriteriaNRD_impl.h:140
mrpt::graphslam::deciders::CICPCriteriaNRD::m_mahal_distance_ICP_odom
TSlidingWindow m_mahal_distance_ICP_odom
Keeps track of the last N measurements between the ICP edge and the corresponding odometry measuremen...
Definition: CICPCriteriaNRD.h:221
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
Definition: CSensoryFrame.h:53
mrpt::graphslam::deciders::CICPCriteriaNRD::m_curr_odometry_only_pose
pose_t m_curr_odometry_only_pose
pose_t estimation using only odometry information.
Definition: CICPCriteriaNRD.h:206
mrpt::graphslam::deciders::CICPCriteriaNRD::m_last_laser_scan3D
mrpt::obs::CObservation3DRangeScan::Ptr m_last_laser_scan3D
Definition: CICPCriteriaNRD.h:191
mrpt::graphslam::deciders::CICPCriteriaNRD::m_is_using_3DScan
bool m_is_using_3DScan
Definition: CICPCriteriaNRD.h:178
mrpt::graphslam::deciders::CICPCriteriaNRD::m_use_angle_difference_node_reg
bool m_use_angle_difference_node_reg
Definition: CICPCriteriaNRD.h:224
mrpt::graphslam::CRegistrationDeciderOrOptimizer
Interface for implementing node/edge registration deciders or optimizer classes.
Definition: CRegistrationDeciderOrOptimizer.h:41
mrpt::graphslam::deciders::CNodeRegistrationDecider::constraint_t
typename GRAPH_T::constraint_t constraint_t
type of graph constraints
Definition: CNodeRegistrationDecider.h:41
mrpt::graphslam::deciders::CICPCriteriaNRD::checkRegistrationCondition
bool checkRegistrationCondition() override
Check whether a new node should be registered in the graph.
Definition: CICPCriteriaNRD_impl.h:236
mrpt::obs::CActionCollection::Ptr
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
Definition: CActionCollection.h:28
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
CObservation3DRangeScan.h
mrpt::graphslam::deciders::CICPCriteriaNRD::TParams::~TParams
~TParams() override=default
mrpt::graphslam::deciders::CICPCriteriaNRD::TParams::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CICPCriteriaNRD_impl.h:342
CActionCollection.h
mrpt::graphslam::deciders::CICPCriteriaNRD::m_curr_laser_scan3D
mrpt::obs::CObservation3DRangeScan::Ptr m_curr_laser_scan3D
Definition: CICPCriteriaNRD.h:192
CRangeScanOps.h
CICPCriteriaNRD_impl.h
mrpt::obs::CObservation2DRangeScan::Ptr
std::shared_ptr< mrpt::obs ::CObservation2DRangeScan > Ptr
Definition: CObservation2DRangeScan.h:56
mrpt::graphslam::deciders::CICPCriteriaNRD::m_times_used_odom
int m_times_used_odom
How many times we used the Odometry Edge instead of the ICP edge.
Definition: CICPCriteriaNRD.h:230
CConfigFileBase.h
mrpt::math::CMatrixFixed< double, constraint_t::state_length, constraint_t::state_length >
mrpt::graphslam::deciders::CICPCriteriaNRD::TParams::decider
decider_t & decider
Reference to outer decider class.
Definition: CICPCriteriaNRD.h:151
TSlidingWindow.h
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::graphslam::deciders::CICPCriteriaNRD::m_last_laser_scan2D
mrpt::obs::CObservation2DRangeScan::Ptr m_last_laser_scan2D
handy laser scans to use in the class methods
Definition: CICPCriteriaNRD.h:185
mrpt::graphslam::deciders::CICPCriteriaNRD::checkRegistrationCondition3D
bool checkRegistrationCondition3D()
Specialized checkRegistrationCondition method used solely when dealing with 3DRangeScan information.
Definition: CICPCriteriaNRD_impl.h:229
mrpt::config::CLoadableOptions
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
Definition: config/CLoadableOptions.h:26
CNodeRegistrationDecider.h
mrpt::graphslam::deciders::CICPCriteriaNRD::loadParams
void loadParams(const std::string &source_fname) override
Load the necessary for the decider/optimizer configuration parameters.
Definition: CICPCriteriaNRD_impl.h:272
mrpt::graphslam::deciders::CICPCriteriaNRD::TParams::loadFromConfigFile
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.
Definition: CICPCriteriaNRD_impl.h:362
mrpt::graphslam::deciders::CICPCriteriaNRD::updateState
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.
Definition: CICPCriteriaNRD_impl.h:38
mrpt::graphslam::deciders::CICPCriteriaNRD::updateState3D
bool updateState3D(mrpt::obs::CObservation3DRangeScan::Ptr observation)
Specialized updateState method used solely when dealing with 3DRangeScan information.
Definition: CICPCriteriaNRD_impl.h:221
mrpt::graphslam::deciders::CICPCriteriaNRD::CICPCriteriaNRD
CICPCriteriaNRD()
Definition: CICPCriteriaNRD_impl.h:15
CSensoryFrame.h
mrpt::graphslam::deciders::CICPCriteriaNRD::updateState2D
bool updateState2D(mrpt::obs::CObservation2DRangeScan::Ptr observation)
Specialized updateState method used solely when dealing with 2DRangeScan information.
Definition: CICPCriteriaNRD_impl.h:117
mrpt::graphslam::deciders::CICPCriteriaNRD::getDescriptiveReport
void getDescriptiveReport(std::string *report_str) const override
Fill the provided string with a detailed report of the decider/optimizer state.
Definition: CICPCriteriaNRD_impl.h:300
CLoadableOptions.h
mrpt::graphslam::deciders::CNodeRegistrationDecider::pose_t
typename GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
Definition: CNodeRegistrationDecider.h:43
mrpt::graphslam::deciders::CICPCriteriaNRD::TParams::TParams
TParams(decider_t &d)
Definition: CICPCriteriaNRD_impl.h:337
mrpt::graphslam::deciders::CICPCriteriaNRD::m_latest_odometry_PDF
constraint_t m_latest_odometry_PDF
Odometry rigid-body transformation since the last accepted LaserScan.
Definition: CICPCriteriaNRD.h:201
mrpt::graphslam::deciders::CICPCriteriaNRD::~CICPCriteriaNRD
~CICPCriteriaNRD() override=default
mrpt::graphslam::TSlidingWindow
Class to monitor the evolution of a statistical quantity.
Definition: TSlidingWindow.h:42
mrpt::graphslam::deciders::CNodeRegistrationDecider::global_pose_t
typename GRAPH_T::global_pose_t global_pose_t
Definition: CNodeRegistrationDecider.h:44
mrpt::graphslam::deciders::CICPCriteriaNRD::m_curr_laser_scan2D
mrpt::obs::CObservation2DRangeScan::Ptr m_curr_laser_scan2D
Current LaserScan.
Definition: CICPCriteriaNRD.h:189
mrpt::graphslam::deciders::CICPCriteriaNRD::TParams::registration_max_angle
double registration_max_angle
Maximum angle difference for new node registration.
Definition: CICPCriteriaNRD.h:160
mrpt::graphslam::deciders::CICPCriteriaNRD::m_use_distance_node_reg
bool m_use_distance_node_reg
Definition: CICPCriteriaNRD.h:225
mrpt::graphslam::deciders::CICPCriteriaNRD::params
TParams params
Definition: CICPCriteriaNRD.h:163
mrpt::graphslam::deciders::CICPCriteriaNRD
ICP-based Fixed Intervals Node Registration.
Definition: CICPCriteriaNRD.h:84
mrpt::graphslam::deciders::CICPCriteriaNRD::m_last_odometry_only_pose
pose_t m_last_odometry_only_pose
pose_t estimation using only odometry information.
Definition: CICPCriteriaNRD.h:213
mrpt::graphslam::deciders
Definition: CEmptyERD.h:16
mrpt::graphslam::deciders::CICPCriteriaNRD::m_times_used_ICP
int m_times_used_ICP
How many times we used the ICP Edge instead of Odometry edge.
Definition: CICPCriteriaNRD.h:228
mrpt::obs::CObservation3DRangeScan::Ptr
std::shared_ptr< mrpt::obs ::CObservation3DRangeScan > Ptr
Definition: CObservation3DRangeScan.h:170
mrpt::graphslam::deciders::CRangeScanOps
Class for keeping together all the RangeScanner-related functions.
Definition: CRangeScanOps.h:71
CObservation2DRangeScan.h
mrpt::graphslam::deciders::CICPCriteriaNRD::printParams
void printParams() const override
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
Definition: CICPCriteriaNRD_impl.h:293



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sat Jun 27 14:00:59 UTC 2020