Main MRPT website > C++ reference for MRPT 1.5.7
CICPCriteriaNRD.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 CICPCRITERIANRD_H
11 #define CICPCRITERIANRD_H
12 
13 #include <mrpt/obs/CRawlog.h>
15 #include <mrpt/obs/CSensoryFrame.h>
18 #include <mrpt/poses/CPosePDF.h>
19 #include <mrpt/poses/CPose3DPDF.h>
22 #include <mrpt/obs/CRawlog.h>
26 #include <mrpt/utils/CConfigFile.h>
28 #include <mrpt/utils/CStream.h>
30 #include <mrpt/slam/CICP.h>
31 #include <mrpt/system/datetime.h>
32 #include <mrpt/system/os.h>
33 #include <mrpt/system/threads.h>
34 
38 
39 #include <string>
40 #include <math.h>
41 
42 namespace mrpt { namespace graphslam { namespace deciders {
43 
44 /**\brief ICP-based Fixed Intervals Node Registration
45  *
46  * ## Description
47  *
48  * Current Decider is meant for adding nodes in 2D datasets recorded using
49  * a laser range finder or RGB-D camera (e.g. Kinect). No odometry data from
50  * encoders is needed. Using ICP to match consecutive RangeScan measurements,
51  * the decider keeps track of the pose transformation since the last registered
52  * node. If the norm or the angle of the latter surpasses certain thresholds
53  * (which are read from an external .ini file) then a new node is added to the
54  * graph)
55  * \sa loadParams, TParams::loadFromConfigFile
56  *
57  * Decider *does not guarantee* thread safety when accessing the GRAPH_T
58  * resource. This is handled by the CGraphSlamEngine class.
59  *
60  * ### Specifications
61  *
62  * - Map type: 2D
63  * - MRPT rawlog format: #1, #2
64  * - Graph Type: CPosePDFGaussianInf
65  * - Observations Used: CObservation2DRangeScan, CObservation3DRangeScan
66  * - Node Registration Strategy: Fixed Intervals
67  *
68  * ### .ini Configuration Parameters
69  *
70  * \htmlinclude config_params_preamble.txt
71  *
72  * - \b class_verbosity
73  * + \a Section : NodeRegistrationDeciderParameters
74  * + \a default value : 1 (LVL_INFO)
75  * + \a Required : FALSE
76  *
77  * - \b registration_max_distance
78  * + \a Section : NodeRegistrationDeciderParameters
79  * + \a Default value : 0.5 // meters
80  * + \a Required : FALSE
81  *
82  * - \b registration_max_angle
83  * + \a Section : NodeRegistrationDeciderParameters
84  * + \a Default value : 10 // degrees
85  * + \a Required : FALSE
86  *
87  * \note Since the decider inherits from the CRangeScanOps
88  * class, it parses the configuration parameters of the latter as well from the
89  * "ICP" section. Refer to the CRangeScanOps documentation for
90  * its list of configuration
91  * parameters
92  *
93  * \note Class contains an instance of the TSlidingWindow class and it parses
94  * the configuration parameters of the latter from the
95  * "NodeRegistrationDeciderParameters" section. Refer to TSlidingWindow
96  * documentation for its list of configuration parameters
97  *
98  * \ingroup mrpt_graphslam_grp
99  */
100 template<class GRAPH_T>
104 {
105  public:
106  // Public functions
107  //////////////////////////////////////////////////////////////
108  /**\brief Handy typedefs */
109  /**\{*/
110  /**\brief type of graph constraints */
111  typedef typename GRAPH_T::constraint_t constraint_t;
112  /**\brief type of underlying poses (2D/3D). */
113  typedef typename GRAPH_T::constraint_t::type_value pose_t;
114  typedef typename GRAPH_T::global_pose_t global_pose_t;
115 
116  typedef mrpt::math::CMatrixFixedNumeric<double,
117  constraint_t::state_length,
118  constraint_t::state_length> InfMat;
119  /**\brief Typedef for accessing methods of the RangeScanRegistrationDecider
120  * parent class.
121  */
124  typedef CICPCriteriaNRD<GRAPH_T> decider_t; /**< self type - Handy typedef */
125  /**\brief Node Registration Decider */
127  /**\}*/
128 
129  /**\brief Class constructor */
130  CICPCriteriaNRD();
131  /**\brief Class destructor */
133 
134  void loadParams(const std::string& source_fname);
135  void printParams() const;
136  void getDescriptiveReport(std::string* report_str) const;
137 
138  /**\brief Update the decider state using the latest dataset measurements.
139  *
140  * \note Depending on the observations at hand, update of the state is
141  * handled either by updateState2D, or by updateState3D methods. This
142  * helps in separating the 2D, 3D RangeScans handling altogether, which in
143  * turn simplifies the overall procedure
144  *
145  * Order of calls:
146  * updateState (calls) ==> updateState2D/3D ==>
147  * checkRegistrationCondition2D/3D ==> CheckRegistrationCondition
148  *
149  * \sa updateState2D, updateState3D
150  */
151  bool updateState(
152  mrpt::obs::CActionCollectionPtr action,
153  mrpt::obs::CSensoryFramePtr observations,
154  mrpt::obs::CObservationPtr observation );
155  /**\brief Specialized updateState method used solely when dealing with
156  * 2DRangeScan information.
157  * \sa updateState3D
158  */
159  bool updateState2D(mrpt::obs::CObservation2DRangeScanPtr observation);
160  /**\brief Specialized updateState method used solely when dealing with
161  * 3DRangeScan information.
162  * \sa updateState2D
163  */
164  bool updateState3D(mrpt::obs::CObservation3DRangeScanPtr observation);
165 
167  public:
168  TParams(decider_t& d);
169  ~TParams();
170 
171  decider_t& decider; /**< Reference to outer decider class */
172 
173  void loadFromConfigFile(
175  const std::string &section);
176  void dumpToTextStream(mrpt::utils::CStream &out) const;
177 
178  double registration_max_distance; /**< Maximum distance for new node registration */
179  double registration_max_angle; /**< Maximum angle difference for new node registration */
180  };
181 
182  // Public members
183  // ////////////////////////////
185 
186  protected:
187  // protected functions
188  //////////////////////////////////////////////////////////////
190  /**\brief Specialized checkRegistrationCondtion method used solely when
191  * dealing with 2DRangeScan information
192  * \sa checkRegistrationCondition3D
193  */
195  /**\brief Specialized checkRegistrationCondition method used solely when
196  * dealing with 3DRangeScan information
197  * \sa checkRegistrationCondition2D
198  */
200 
201  // protected members
202  //////////////////////////////////////////////////////////////
204 
205  /**\brief handy laser scans to use in the class methods
206  */
207  /**\{ */
208  /**\brief 2D LaserScan corresponding to the latest registered node in the graph */
209  mrpt::obs::CObservation2DRangeScanPtr m_last_laser_scan2D;
210  /**\brief Current LaserScan. Set during the new measurements acquisition in
211  * updateState method
212  */
213  mrpt::obs::CObservation2DRangeScanPtr m_curr_laser_scan2D;
214 
215  mrpt::obs::CObservation3DRangeScanPtr m_last_laser_scan3D;
216  mrpt::obs::CObservation3DRangeScanPtr m_curr_laser_scan3D;
217  /**\} */
218 
219  /**\brief Odometry rigid-body transformation since the last accepted LaserScan.
220  *
221  * Decider can use it to smoothen the trajectory in the case of high noise
222  * in the laser measurements
223  */
225  /**\brief pose_t estimation using only odometry information.
226  * \note Utilized only in observation-only rawlogs.
227  *
228  */
230  /**\brief pose_t estimation using only odometry information.
231  * \note Utilized only in observation-only rawlogs.
232  *
233  * Resets next time an ICP edge/Odometry measurement is utilized for
234  * updating the estimated robot position.
235  */
237  /**\brief Keeps track of the last N measurements between the ICP edge and
238  * the corresponding odometry measurements.
239  *
240  * Use the last odometry rigid body transformation instead of the
241  * ICP edge if the mahalanobis distance between them is greater than this
242  * limit.
243  */
245 
246  // criteria for adding new a new node
249 
250  /**How many times we used the ICP Edge instead of Odometry edge*/
252  /**How many times we used the Odometry Edge instead of the ICP edge */
254 };
255 
256 
257 } } } // end of namespaces
258 
259 #include "CICPCriteriaNRD_impl.h"
260 #endif /* end of include guard: CICPCRITERIANRD_H */
bool updateState2D(mrpt::obs::CObservation2DRangeScanPtr observation)
Specialized updateState method used solely when dealing with 2DRangeScan information.
int m_times_used_ICP
How many times we used the ICP Edge instead of Odometry edge.
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
mrpt::graphslam::deciders::CRangeScanOps< GRAPH_T > range_ops_t
Typedef for accessing methods of the RangeScanRegistrationDecider parent class.
decider_t & decider
Reference to outer decider class.
bool updateState(mrpt::obs::CActionCollectionPtr action, mrpt::obs::CSensoryFramePtr observations, mrpt::obs::CObservationPtr observation)
Update the decider state using the latest dataset measurements.
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.
ICP-based Fixed Intervals Node Registration.
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...
CICPCriteriaNRD< GRAPH_T > decider_t
self type - Handy typedef
bool checkRegistrationCondition()
Check whether a new node should be registered in the graph.
void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
GRAPH_T::constraint_t constraint_t
type of graph constraints
A numeric matrix of compile-time fixed size.
mrpt::obs::CObservation3DRangeScanPtr m_curr_laser_scan3D
double registration_max_angle
Maximum angle difference for new node registration.
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...
GLsizei const GLchar ** string
Definition: glext.h:3919
bool updateState3D(mrpt::obs::CObservation3DRangeScanPtr observation)
Specialized updateState method used solely when dealing with 3DRangeScan information.
Class for keeping together all the RangeScanner-related functions.
Definition: CRangeScanOps.h:81
GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
bool checkRegistrationCondition2D()
Specialized checkRegistrationCondtion method used solely when dealing with 2DRangeScan information...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::math::CMatrixFixedNumeric< double, constraint_t::state_length, constraint_t::state_length > InfMat
Class to monitor the evolution of a statistical quantity.
void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
bool checkRegistrationCondition3D()
Specialized checkRegistrationCondition method used solely when dealing with 3DRangeScan information...
mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > parent_t
Node Registration Decider.
mrpt::obs::CObservation3DRangeScanPtr m_last_laser_scan3D
mrpt::obs::CObservation2DRangeScanPtr m_last_laser_scan2D
handy laser scans to use in the class methods
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.
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.
GRAPH_T::constraint_t constraint_t
Handy typedefs.
int m_times_used_odom
How many times we used the Odometry Edge instead of the ICP edge.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
mrpt::obs::CObservation2DRangeScanPtr m_curr_laser_scan2D
Current LaserScan.



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