MRPT  1.9.9
CLevMarqGSO.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-2018, 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 CLEVMARQGSO_H
11 #define CLEVMARQGSO_H
12 
13 #include <mrpt/obs/CSensoryFrame.h>
16 #include <mrpt/img/TColor.h>
18 #include <mrpt/poses/CPose2D.h>
19 #include <mrpt/poses/CPose3D.h>
20 
21 #include <mrpt/graphslam/levmarq.h>
23 
24 #include <iostream>
25 #include <string>
26 #include <map>
27 #include <cmath> // fabs function
28 
30 {
31 /**\brief Levenberg-Marquardt non-linear graph slam optimization scheme.
32  *
33  * ## Description
34  *
35  * Current decider optimizes the graph according to the
36  * graphslam::optimize_spa_levmarq method. Refer to the latter for more
37  * details on the implementation.
38  *
39  * ### .ini Configuration Parameters
40  *
41  * \htmlinclude graphslam-engine_config_params_preamble.txt
42  *
43  * - \b class_verbosity
44  * + \a Section : OptimizerParameters
45  * + \a Default value : 1 (mrpt::system::LVL_INFO)
46  * + \a Required : FALSE
47  *
48  * - \b optimization_on_second_thread
49  * + \a Section : OptimizerParameters
50  * + \a Default value : FALSE
51  * + \a Required : FALSE
52  * + \a Description : Specify whether to use a second thread to optimize
53  * the graph.
54  *
55  * - \b LC_min_nodeid_diff
56  * + \a Section : GeneralConfiguration
57  * + \a Default value : 30
58  * + \a Required : FALSE
59  * + \a Description : Minimum NodeID difference for an edge to be considered
60  * a loop closure.
61  *
62  * - \b optimization_distance
63  * + \a Section : OptimizerParameters
64  * + \a Default value : 5
65  * + \a Required : FALSE
66  * + \a Description : Positions of the nodes within the specified distance
67  from the current
68  * graph node are optimized according to the corresponding constraints between
69  * them
70  *
71  * - \b verbose
72  * + \a Section : OptimizerParameters
73  * + \a Default value : FALSE
74  * + \a Required : FALSE
75  * + \a Description : Refers to the Levenberg-Marquardt optimization.
76  * procedure
77  *
78  * - \b profiler
79  * + \a Section : OptimizerParameters
80  * + \a Default value : FALSE
81  * + \a Required : FALSE
82  * + \a Description : Refers to the Levenberg-Marquardt optimization.
83  * procedure
84  *
85  * - \b max_iterations
86  * + \a Section : OptimizerParameters
87  * + \a Default value : 100
88  * + \a Required : FALSE
89  * + \a Description : Refers to the Levenberg-Marquardt optimization. Sets
90  * the maximum number of iterations of the optimization scheme.
91  *
92  * - \b scale_hessian
93  * + \a Section : OptimizerParameters
94  * + \a Default value : 0.2
95  * + \a Required : FALSE
96  * + \a Description : Refers to the Levenberg-Marquardt optimization.
97  *
98  * - \b tau
99  * + \a Section : OptimizerParameters
100  * + \a Default value : 1e-3
101  * + \a Required : FALSE
102  * + \a Description : Refers to the Levenberg-Marquardt optimization.
103  *
104  * \note For a detailed description of the optimization parameters of the
105  * Levenberg-Marquardt scheme, refer to
106  *
107  http://reference.mrpt.org/devel/group__mrpt__graphslam__grp.html#ga022f4a70be5ec7c432f46374e4bb9d66
108  *
109  * \note For a detailed description of the graph visualization parameters
110  * refer to
111  *
112  http://reference.mrpt.org/devel/group__mrpt__opengl__grp.html#ga30efc9f6fcb49801e989d174e0f65a61
113 
114  *
115  * \ingroup mrpt_graphslam_grp
116  */
117 template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
120 {
121  public:
122  // Public methods
123  //////////////////////////////////////////////////////////////
124  /**\brief Handy typedefs */
125  /**\{*/
126  using constraint_t = typename GRAPH_T::constraint_t;
127  /** type of underlying poses (2D/3D)*/
128  using pose_t = typename GRAPH_T::constraint_t::type_value;
130  double, constraint_t::state_length, constraint_t::state_length>;
133  /**\}*/
134 
135  CLevMarqGSO();
136  ~CLevMarqGSO();
137 
138  bool updateState(
140  mrpt::obs::CSensoryFrame::Ptr observations,
141  mrpt::obs::CObservation::Ptr observation);
142 
143  void initializeVisuals();
144  void updateVisuals();
145  /**\brief Get a list of the window events that happened since the last
146  * call.
147  */
149  const std::map<std::string, bool>& events_occurred);
150  /**\brief Struct for holding the optimization-related variables in a
151  * compact form
152  */
154  {
155  public:
158 
159  void loadFromConfigFile(
161  const std::string& section);
162  void dumpToTextStream(std::ostream& out) const;
163 
165  // True if optimization procedure is to run in a multithreading fashion
167 
168  /**\brief optimize only for the nodes found in a certain distance from
169  * the current position. Optimize for the entire graph if set to1
170  */
175  /**\brief Keystroke to toggle the optimization distance on/off */
177  /**\brief Keystroke to manually trigger a full graph optimization */
179 
180  // nodeID difference for an edge to be considered loop closure
182 
183  // Map of TPairNodesID to their corresponding edge as recorded in the
184  // last update of the optimizer state
185  typename GRAPH_T::edges_map_t last_pair_nodes_to_edge;
186  };
187 
188  /**\brief struct for holding the graph visualization-related variables in a
189  * compact form
190  */
192  {
193  public:
196 
197  void loadFromConfigFile(
199  const std::string& section);
200  void dumpToTextStream(std::ostream& out) const;
201 
204  // textMessage parameters
205  std::string keystroke_graph_toggle; // see Ctor for initialization
206  std::string keystroke_graph_autofit; // see Ctor for initialization
209  };
210 
211  void loadParams(const std::string& source_fname);
212  void printParams() const;
213  void getDescriptiveReport(std::string* report_str) const;
214 
215  bool justFullyOptimizedGraph() const;
216 
217  // Public members
218  // ////////////////////////////
219  /** Parameters relevant to the optimizatio nfo the graph. */
221  /** Parameters relevant to the visualization of the graph. */
223 
224  protected:
225  // protected methods
226  // ////////////////////////////
227 
228  /**\brief Feedback of the Levenberg-Marquardt graph optimization procedure.
229  *
230  */
231  static void levMarqFeedback(
232  const GRAPH_T& graph, const size_t iter, const size_t max_iter,
233  const double cur_sq_error);
234 
235  /**\brief Optimize the given graph.
236  *
237  * Wrapper around the graphslam::optimize_spa_levmarq method
238  * \sa optimize_spa_levmarq, optimizeGraph
239  *
240  * \param[in] full_update Impose that method optimizes the whole graph
241  *
242  */
243  void _optimizeGraph(bool is_full_update = false);
244  /** \brief Wrapper around _optimizeGraph which first locks the section and
245  * then calls the _optimizeGraph method.
246  *
247  * Used in multithreaded optimization
248  * \sa _optimizeGraph()
249  */
250  void optimizeGraph();
251  /**\brief Check if a loop closure edge was added in the graph.
252  *
253  * Match the previously registered edges in the graph with the current. If
254  * there is a node difference *in any new edge* greater than
255  * \b LC_min_nodeid_diff (see .ini parameter) then new constraint is
256  * considered a Loop Closure
257  *
258  * \return True if \b any of the newly added edges is considered a loop
259  * closure
260  */
261  bool checkForLoopClosures();
262  /**\brief Decide whether to issue a full graph optimization
263  *
264  * In case N consecutive full optimizations have been issued, skip some of
265  * the next as they slow down the overall execution and they don't reduce
266  * the overall error
267  *
268  * \return True for issuing a full graph optimization, False otherwise
269  */
271  /**\brief Initialize objects relateed to the Graph Visualization
272  */
273  void initGraphVisualization();
274  /**\brief Called internally for updating the visualization scene for the
275  * graph
276  * building procedure
277  */
278  inline void updateGraphVisualization();
279  /**\brief Toggle the graph visualization on and off.
280  */
282  /**\brief Set the camera parameters of the CDisplayWindow3D so that the
283  * whole
284  * graph is viewed in the window.
285  *
286  * \warning Method assumes that the COpenGLinstance *is not locked* prior to
287  * this
288  * call, so make sure you have issued
289  * CDisplayWindow3D::unlockAccess3DScene() before calling this method.
290  */
291  inline void fitGraphInView();
292 
293  /**\brief Initialize the Disk/Sphere used for visualizing the optimization
294  * distance.
295  */
296  /**\{*/
298  /**\brief Setup the corresponding Disk/Sphere instance.
299  *
300  * Method overloads are used to overcome the C++ specialization
301  * restrictions
302  *
303  * \return Disk/Sphere instance for 2D/3D SLAM respectively
304  */
305  /**\{*/
307  const mrpt::poses::CPose2D& p_unused);
309  const mrpt::poses::CPose3D& p_unused);
310  /**\}*/
311 
312  /**\}*/
313 
314  /**\brief Update the position of the disk indicating the distance in which
315  * Levenberg-Marquardt graph optimization is executed
316  */
317  inline void updateOptDistanceVisualization();
318  /**\brief toggle the optimization distance object on and off
319  */
321  /**\brief Get a list of the nodeIDs whose position is within a certain
322  * distance to the specified nodeID
323  */
324  void getNearbyNodesOf(
325  std::set<mrpt::graphs::TNodeID>* nodes_set,
326  const mrpt::graphs::TNodeID& cur_nodeID, double distance);
327 
328  // protected members
329  //////////////////////////////////////////////////////////////
330 
335 
336  // start optimizing the graph after a certain number of nodes has been
337  // added (when m_graph->nodeCount() > m_last_total_num_of_nodes)
339 
340  // Use second thread for graph optimization
341  std::thread m_thread_optimize;
342 
343  /**\brief Enumeration that defines the behaviors towards using or ignoring a
344  * newly added loop closure to fully optimize the graph
345  */
347  {
351  };
352  /**\brief Should I fully optimize the graph on loop closure?
353  */
355  /**\name Smart Full-Optimization Command
356  *
357  * Instead of issuing a full optimization every time a loop closure is
358  * detected, ignore current loop closure when enough consecutive loop
359  * closures have already been utilised.
360  * This avoids the added computational cost that is needed for optimizing
361  * the graph without reducing the accuracy of the overall operation
362  */
363  /**\{*/
364 
365  /**\brief Number of maximum cosecutive loop closures that are allowed to be
366  * issued.
367  *
368  * \sa m_curr_used_consec_lcs, m_max_ignored_consec_lcs
369  */
371  /**\brief Number of consecutive loop closures that are currently registered
372  *
373  * \sa m_max_used_consec_lcs
374  */
376  /**\brief Number of consecutive loop closures to ignore after \b
377  * m_max_used_consec_lcs have already been issued.
378  *
379  * \sa m_curr_ignored_consec_lcs, m_max_used_consec_lcs
380  */
382  /**\brief Consecutive Loop Closures that have currently been ignored
383  *
384  * \sa m_max_ignored_consec_lcs
385  */
387 
388  /**\}*/
389 
390  /**\brief Indicates whether a full graph optimization was just issued.
391  */
393 
394  /**\brief Minimum number of nodes before we try optimizing the graph */
396 };
397 }
398 #include "CLevMarqGSO_impl.h"
399 
400 #endif /* end of include guard: CLEVMARQGSO_H */
401 
402 
void initGraphVisualization()
Initialize objects relateed to the Graph Visualization.
void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
static void levMarqFeedback(const GRAPH_T &graph, const size_t iter, const size_t max_iter, const double cur_sq_error)
Feedback of the Levenberg-Marquardt graph optimization procedure.
bool checkForFullOptimization()
Decide whether to issue a full graph optimization.
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...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
mrpt::opengl::CRenderizable::Ptr initOptDistanceVisualizationInternal(const mrpt::poses::CPose2D &p_unused)
Setup the corresponding Disk/Sphere instance.
void updateGraphVisualization()
Called internally for updating the visualization scene for the graph building procedure.
void toggleGraphVisualization()
Toggle the graph visualization on and off.
Struct for holding the optimization-related variables in a compact form.
Definition: CLevMarqGSO.h:153
FullOptimizationPolicy
Enumeration that defines the behaviors towards using or ignoring a newly added loop closure to fully ...
Definition: CLevMarqGSO.h:346
void optimizeGraph()
Wrapper around _optimizeGraph which first locks the section and then calls the _optimizeGraph method...
typename GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D)
Definition: CLevMarqGSO.h:128
bool m_just_fully_optimized_graph
Indicates whether a full graph optimization was just issued.
Definition: CLevMarqGSO.h:392
bool justFullyOptimizedGraph() const
Used by the caller to query for possible full graph optimization on the latest optimizer run...
std::string keystroke_optimize_graph
Keystroke to manually trigger a full graph optimization.
Definition: CLevMarqGSO.h:178
double optimization_distance
optimize only for the nodes found in a certain distance from the current position.
Definition: CLevMarqGSO.h:171
void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
void toggleOptDistanceVisualization()
toggle the optimization distance object on and off
OptimizationParams opt_params
Parameters relevant to the optimizatio nfo the graph.
Definition: CLevMarqGSO.h:220
void initOptDistanceVisualization()
Initialize the Disk/Sphere used for visualizing the optimization distance.
typename GRAPH_T::constraint_t constraint_t
Handy typedefs.
Definition: CLevMarqGSO.h:126
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
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...
void initializeVisuals()
Initialize visual objects in CDisplayWindow (e.g.
void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
Interface for implementing node/edge registration deciders or optimizer classes.
FullOptimizationPolicy m_optimization_policy
Should I fully optimize the graph on loop closure?
Definition: CLevMarqGSO.h:354
size_t m_min_nodes_for_optimization
Minimum number of nodes before we try optimizing the graph.
Definition: CLevMarqGSO.h:395
void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred)
Get a list of the window events that happened since the last call.
size_t m_max_ignored_consec_lcs
Number of consecutive loop closures to ignore after m_max_used_consec_lcs have already been issued...
Definition: CLevMarqGSO.h:381
GLsizei const GLchar ** string
Definition: glext.h:4101
bool checkForLoopClosures()
Check if a loop closure edge was added in the graph.
size_t m_curr_ignored_consec_lcs
Consecutive Loop Closures that have currently been ignored.
Definition: CLevMarqGSO.h:386
std::string keystroke_optimization_distance
Keystroke to toggle the optimization distance on/off.
Definition: CLevMarqGSO.h:176
Interface for implementing graphSLAM optimizer classes.
void updateOptDistanceVisualization()
Update the position of the disk indicating the distance in which Levenberg-Marquardt graph optimizati...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
size_t m_max_used_consec_lcs
Number of maximum cosecutive loop closures that are allowed to be issued.
Definition: CLevMarqGSO.h:370
void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
GraphVisualizationParams viz_params
Parameters relevant to the visualization of the graph.
Definition: CLevMarqGSO.h:222
size_t m_curr_used_consec_lcs
Number of consecutive loop closures that are currently registered.
Definition: CLevMarqGSO.h:375
A RGB color - 8bit.
Definition: TColor.h:20
Levenberg-Marquardt non-linear graph slam optimization scheme.
Definition: CLevMarqGSO.h:118
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation)
Generic method for fetching the incremental action/observation readings from the calling function...
void fitGraphInView()
Set the camera parameters of the CDisplayWindow3D so that the whole graph is viewed in the window...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
struct for holding the graph visualization-related variables in a compact form
Definition: CLevMarqGSO.h:191
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1891
void updateVisuals()
Update the relevant visual features in CDisplayWindow.
void _optimizeGraph(bool is_full_update=false)
Optimize the given graph.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020