MRPT  2.0.4
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:
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:
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"
mrpt::obs::CObservation::Ptr
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:45
mrpt::graphslam::optimizers::CLevMarqGSO::GraphVisualizationParams::text_index_graph
int text_index_graph
Definition: CLevMarqGSO.h:206
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::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: CLevMarqGSO_impl.h:726
mrpt::graphslam::optimizers::CLevMarqGSO::m_has_read_config
bool m_has_read_config
Definition: CLevMarqGSO.h:326
mrpt::graphslam::optimizers::CLevMarqGSO::m_max_ignored_consec_lcs
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
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::optimization_distance_color
mrpt::img::TColor optimization_distance_color
Definition: CLevMarqGSO.h:173
mrpt::graphslam::optimizers::CLevMarqGSO::GraphVisualizationParams::GraphVisualizationParams
GraphVisualizationParams()
Definition: CLevMarqGSO_impl.h:756
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationPolicy
OptimizationPolicy
Enumeration that defines the behaviors towards using or ignoring a newly added loop closure to fully ...
Definition: CLevMarqGSO.h:341
mrpt::graphslam::optimizers::CLevMarqGSO::optimizeGraph
void optimizeGraph() override
Wrapper around _optimizeGraph which first locks the section and then calls the _optimizeGraph method.
Definition: CLevMarqGSO_impl.h:374
CLevMarqGSO_impl.h
mrpt::graphslam::optimizers::CLevMarqGSO::loadParams
void loadParams(const std::string &source_fname) override
Load the necessary for the decider/optimizer configuration parameters.
Definition: CLevMarqGSO_impl.h:639
mrpt::graphslam::optimizers::CLevMarqGSO::m_last_total_num_of_nodes
size_t m_last_total_num_of_nodes
Definition: CLevMarqGSO.h:332
mrpt::graphslam::optimizers::CLevMarqGSO::m_optimization_policy
OptimizationPolicy m_optimization_policy
Definition: CLevMarqGSO.h:347
mrpt::graphslam::optimizers::CLevMarqGSO::m_curr_used_consec_lcs
size_t m_curr_used_consec_lcs
Number of consecutive loop closures that are currently registered.
Definition: CLevMarqGSO.h:368
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationPolicy::Count
@ Count
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
Definition: CSensoryFrame.h:53
mrpt::graphslam::optimizers::CLevMarqGSO::updateState
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.
Definition: CLevMarqGSO_impl.h:23
mrpt::graphslam::optimizers::CGraphSlamOptimizer
Interface for implementing graphSLAM optimizer classes.
Definition: CGraphSlamOptimizer.h:36
mrpt::graphslam::optimizers::CLevMarqGSO::GraphVisualizationParams::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: CLevMarqGSO_impl.h:781
mrpt::graphslam::optimizers::CLevMarqGSO::checkForLoopClosures
bool checkForLoopClosures()
Check if a loop closure edge was added in the graph.
Definition: CLevMarqGSO_impl.h:465
mrpt::graphslam::CRegistrationDeciderOrOptimizer
Interface for implementing node/edge registration deciders or optimizer classes.
Definition: CRegistrationDeciderOrOptimizer.h:42
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::cfg
mrpt::system::TParametersDouble cfg
Definition: CLevMarqGSO.h:163
mrpt::graphslam::optimizers::CLevMarqGSO::getDescriptiveReport
void getDescriptiveReport(std::string *report_str) const override
Fill the provided string with a detailed report of the decider/optimizer state.
Definition: CLevMarqGSO_impl.h:670
mrpt::graphslam::optimizers::CLevMarqGSO::m_max_used_consec_lcs
size_t m_max_used_consec_lcs
Number of maximum cosecutive loop closures that are allowed to be issued.
Definition: CLevMarqGSO.h:363
mrpt::graphslam::optimizers::CLevMarqGSO::m_curr_ignored_consec_lcs
size_t m_curr_ignored_consec_lcs
Consecutive Loop Closures that have currently been ignored.
Definition: CLevMarqGSO.h:379
mrpt::graphslam::optimizers::CLevMarqGSO::notifyOfWindowEvents
void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred) override
Get a list of the window events that happened since the last call.
Definition: CLevMarqGSO_impl.h:89
mrpt::graphs::TNodeID
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
mrpt::graphslam::optimizers::CLevMarqGSO::~CLevMarqGSO
~CLevMarqGSO() override=default
mrpt::graphslam::optimizers::CLevMarqGSO::registered_new_node
bool registered_new_node
Definition: CLevMarqGSO.h:327
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::last_pair_nodes_to_edge
GRAPH_T::edges_map_t last_pair_nodes_to_edge
Definition: CLevMarqGSO.h:184
CGraphSlamOptimizer.h
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
mrpt::graphslam::optimizers::CLevMarqGSO::viz_params
GraphVisualizationParams viz_params
Parameters relevant to the visualization of the graph.
Definition: CLevMarqGSO.h:219
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::~OptimizationParams
~OptimizationParams() override
mrpt::opengl::CRenderizable::Ptr
std::shared_ptr< CRenderizable > Ptr
Definition: CRenderizable.h:50
mrpt::math::distance
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1807
mrpt::graphslam::optimizers::CLevMarqGSO::initializeVisuals
void initializeVisuals() override
Initialize visual objects in CDisplayWindow (e.g.
Definition: CLevMarqGSO_impl.h:60
mrpt::graphslam::optimizers::CLevMarqGSO::fitGraphInView
void fitGraphInView()
Set the camera parameters of the CDisplayWindow3D so that the whole graph is viewed in the window.
Definition: CLevMarqGSO_impl.h:225
CPose2D.h
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationPolicy::UseLoopClosures
@ UseLoopClosures
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::text_index_optimization_distance
int text_index_optimization_distance
Definition: CLevMarqGSO.h:172
mrpt::graphslam::optimizers::CLevMarqGSO::updateGraphVisualization
void updateGraphVisualization()
Called internally for updating the visualization scene for the graph building procedure.
Definition: CLevMarqGSO_impl.h:157
mrpt::graphslam::optimizers::CLevMarqGSO::m_autozoom_active
bool m_autozoom_active
Definition: CLevMarqGSO.h:328
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::LC_min_nodeid_diff
int LC_min_nodeid_diff
Definition: CLevMarqGSO.h:180
mrpt::graphslam::optimizers::CLevMarqGSO::GraphVisualizationParams::keystroke_graph_autofit
std::string keystroke_graph_autofit
Definition: CLevMarqGSO.h:205
mrpt::graphslam::optimizers::CLevMarqGSO::initOptDistanceVisualizationInternal
mrpt::opengl::CRenderizable::Ptr initOptDistanceVisualizationInternal(const mrpt::poses::CPose2D &p_unused)
Setup the corresponding Disk/Sphere instance.
Definition: CLevMarqGSO_impl.h:306
mrpt::graphslam::optimizers::CLevMarqGSO::GraphVisualizationParams::~GraphVisualizationParams
~GraphVisualizationParams() override
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::keystroke_optimize_graph
std::string keystroke_optimize_graph
Keystroke to manually trigger a full graph optimization.
Definition: CLevMarqGSO.h:177
CConfigFileBase.h
mrpt::graphslam::optimizers::CLevMarqGSO::m_first_time_call
bool m_first_time_call
Definition: CLevMarqGSO.h:325
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::optimization_distance
double optimization_distance
optimize only for the nodes found in a certain distance from the current position.
Definition: CLevMarqGSO.h:170
mrpt::math::CMatrixFixed< double, constraint_t::state_length, constraint_t::state_length >
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams
Struct for holding the optimization-related variables in a compact form.
Definition: CLevMarqGSO.h:153
mrpt::graphslam::optimizers::CLevMarqGSO::GraphVisualizationParams::visualize_optimized_graph
bool visualize_optimized_graph
Definition: CLevMarqGSO.h:202
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:45
mrpt::system::TParameters< double >
mrpt::graphslam::optimizers::CLevMarqGSO::GraphVisualizationParams
struct for holding the graph visualization-related variables in a compact form
Definition: CLevMarqGSO.h:191
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:41
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:87
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::keystroke_optimization_distance
std::string keystroke_optimization_distance
Keystroke to toggle the optimization distance on/off.
Definition: CLevMarqGSO.h:175
mrpt::graphslam::optimizers
Definition: CEmptyGSO.h:18
mrpt::graphslam::optimizers::CLevMarqGSO::justFullyOptimizedGraph
bool justFullyOptimizedGraph() const override
Definition: CLevMarqGSO_impl.h:589
mrpt::graphslam::optimizers::CLevMarqGSO::GraphVisualizationParams::keystroke_graph_toggle
std::string keystroke_graph_toggle
Definition: CLevMarqGSO.h:204
mrpt::img::TColor
A RGB color - 8bit.
Definition: TColor.h:26
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:27
mrpt::graphslam::optimizers::CLevMarqGSO::_optimizeGraph
void _optimizeGraph(bool is_full_update=false)
Optimize the given graph.
Definition: CLevMarqGSO_impl.h:395
CRenderizable.h
levmarq.h
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::OptimizationParams
OptimizationParams()
Definition: CLevMarqGSO_impl.h:704
mrpt::graphslam::optimizers::CLevMarqGSO::checkForFullOptimization
bool checkForFullOptimization()
Decide whether to issue a full graph optimization.
Definition: CLevMarqGSO_impl.h:509
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::optimization_on_second_thread
bool optimization_on_second_thread
Definition: CLevMarqGSO.h:165
mrpt::graphslam::optimizers::CLevMarqGSO::getNearbyNodesOf
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.
Definition: CLevMarqGSO_impl.h:602
mrpt::graphslam::optimizers::CLevMarqGSO::GraphVisualizationParams::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CLevMarqGSO_impl.h:764
mrpt::graphslam::optimizers::CLevMarqGSO::toggleOptDistanceVisualization
void toggleOptDistanceVisualization()
toggle the optimization distance object on and off
Definition: CLevMarqGSO_impl.h:357
CPose3D.h
CSensoryFrame.h
mrpt::graphslam::optimizers::CLevMarqGSO::initGraphVisualization
void initGraphVisualization()
Initialize objects relateed to the Graph Visualization.
Definition: CLevMarqGSO_impl.h:137
mrpt::graphslam::optimizers::CLevMarqGSO::updateVisuals
void updateVisuals() override
Update the relevant visual features in CDisplayWindow.
Definition: CLevMarqGSO_impl.h:73
mrpt::graphslam::optimizers::CLevMarqGSO::opt_params
OptimizationParams opt_params
Parameters relevant to the optimizatio nfo the graph.
Definition: CLevMarqGSO.h:217
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationPolicy::IgnoreLoopClosures
@ IgnoreLoopClosures
CLoadableOptions.h
mrpt::graphslam::optimizers::CLevMarqGSO::levMarqFeedback
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.
Definition: CLevMarqGSO_impl.h:595
mrpt::graphslam::optimizers::CLevMarqGSO::pose_t
typename GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D)
Definition: CLevMarqGSO.h:127
mrpt::graphslam::optimizers::CLevMarqGSO::CLevMarqGSO
CLevMarqGSO()
Definition: CLevMarqGSO_impl.h:15
mrpt::graphslam::optimizers::CLevMarqGSO::initOptDistanceVisualization
void initOptDistanceVisualization()
Initialize the Disk/Sphere used for visualizing the optimization distance.
Definition: CLevMarqGSO_impl.h:264
mrpt::graphslam::optimizers::CLevMarqGSO::toggleGraphVisualization
void toggleGraphVisualization()
Toggle the graph visualization on and off.
Definition: CLevMarqGSO_impl.h:208
mrpt::graphslam::optimizers::CLevMarqGSO::m_just_fully_optimized_graph
bool m_just_fully_optimized_graph
Indicates whether a full graph optimization was just issued.
Definition: CLevMarqGSO.h:385
mrpt::graphslam::optimizers::CLevMarqGSO::constraint_t
typename GRAPH_T::constraint_t constraint_t
Handy typedefs.
Definition: CLevMarqGSO.h:125
mrpt::graphslam::optimizers::CLevMarqGSO::GraphVisualizationParams::offset_y_graph
double offset_y_graph
Definition: CLevMarqGSO.h:207
mrpt::graphslam::optimizers::CLevMarqGSO
Levenberg-Marquardt non-linear graph slam optimization scheme.
Definition: CLevMarqGSO.h:119
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::offset_y_optimization_distance
double offset_y_optimization_distance
Definition: CLevMarqGSO.h:171
mrpt::graphslam::optimizers::CLevMarqGSO::m_min_nodes_for_optimization
size_t m_min_nodes_for_optimization
Minimum number of nodes before we try optimizing the graph.
Definition: CLevMarqGSO.h:388
mrpt::graphslam::optimizers::CLevMarqGSO::GraphVisualizationParams::cfg
mrpt::system::TParametersDouble cfg
Definition: CLevMarqGSO.h:201
mrpt::graphslam::optimizers::CLevMarqGSO::printParams
void printParams() const override
Definition: CLevMarqGSO_impl.h:631
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CLevMarqGSO_impl.h:713
mrpt::graphslam::optimizers::CLevMarqGSO::m_thread_optimize
std::thread m_thread_optimize
Definition: CLevMarqGSO.h:335
TColor.h
mrpt::graphslam::optimizers::CLevMarqGSO::updateOptDistanceVisualization
void updateOptDistanceVisualization()
Update the position of the disk indicating the distance in which Levenberg-Marquardt graph optimizati...
Definition: CLevMarqGSO_impl.h:338



Page generated by Doxygen 1.8.18 for MRPT 2.0.4 at Thu Sep 24 07:14:18 UTC 2020