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