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



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