Main MRPT website > C++ reference
MRPT logo
Classes | Namespaces | Functions

Detailed Description

Back to list of all libraries | See all modules

Library mrpt-graphslam

Graph-SLAM: See the namespace mrpt::graphslam. For an introduction to graph-slam maps refer to the summary in


struct  mrpt::graphslam::graphslam_traits< GRAPH_T >
 Auxiliary traits template for use among graph-slam problems to make life easier with these complicated, long data type names. More...
struct  mrpt::graphslam::TResultInfoSpaLevMarq
 Output information for mrpt::graphslam::optimize_graph_spa_levmarq() More...


 SLAM methods related to graphs of pose constraints.


template<class GRAPH_T >
void mrpt::graphslam::optimize_graph_spa_levmarq (GRAPH_T &graph, TResultInfoSpaLevMarq &out_info, const std::set< mrpt::utils::TNodeID > *in_nodes_to_optimize=NULL, const mrpt::utils::TParametersDouble &extra_params=mrpt::utils::TParametersDouble(), typename graphslam_traits< GRAPH_T >::TFunctorFeedback functor_feedback=NULL)
 Optimize a graph of pose constraints using the Sparse Pose Adjustment (SPA) sparse representation and a Levenberg-Marquartd optimizer. More...

Function Documentation

◆ optimize_graph_spa_levmarq()

template<class GRAPH_T >
void mrpt::graphslam::optimize_graph_spa_levmarq ( GRAPH_T &  graph,
TResultInfoSpaLevMarq out_info,
const std::set< mrpt::utils::TNodeID > *  in_nodes_to_optimize = NULL,
const mrpt::utils::TParametersDouble extra_params = mrpt::utils::TParametersDouble(),
typename graphslam_traits< GRAPH_T >::TFunctorFeedback  functor_feedback = NULL 

Optimize a graph of pose constraints using the Sparse Pose Adjustment (SPA) sparse representation and a Levenberg-Marquartd optimizer.

This method works for all types of graphs derived from CNetworkOfPoses (see its reference mrpt::graphs::CNetworkOfPoses for the list). The input data are all the pose constraints in graph (graph.edges), and the gross first estimations of the "global" pose coordinates (in graph.nodes).

Note that these first coordinates can be obtained with mrpt::graphs::CNetworkOfPoses::dijkstra_nodes_estimate().

The method implemented in this file is based on this work:

  • "Efficient Sparse Pose Adjustment for 2D Mapping", Kurt Konolige et al., 2010. , but generalized for not only 2D but 2D and 3D poses, and using on-manifold optimization.
[in,out]graphThe input edges and output poses.
[out]out_infoSome basic output information on the process.
[in]nodes_to_optimizeThe list of nodes whose global poses are to be optimized. If NULL (default), all the node IDs are optimized (but that marked as root in the graph).
[in]extra_paramsOptional parameters, see below.
[in]functor_feedbackOptional: a pointer to a user function can be set here to be called on each LM loop iteration (eg to refresh the current state and error, refresh a GUI, etc.)

List of optional parameters by name in "extra_params":

  • "verbose": (default=0) If !=0, produce verbose ouput.
  • "max_iterations": (default=100) Maximum number of Lev-Marq. iterations.
  • "scale_hessian": (default=0.1) Multiplies the Hessian matrix by this scalar (may improve convergence speed).
  • "initial_lambda": (default=0) <=0 means auto guess, otherwise, initial lambda value for the lev-marq algorithm.
  • "tau": (default=1e-3) Initial tau value for the lev-marq algorithm.
  • "e1": (default=1e-6) Lev-marq algorithm iteration stopping criterion #1: |gradient| < e1
  • "e2": (default=1e-6) Lev-marq algorithm iteration stopping criterion #2: |delta_incr| < e2*(x_norm+e2)
The following graph types are supported: mrpt::graphs::CNetworkOfPoses2D, mrpt::graphs::CNetworkOfPoses3D, mrpt::graphs::CNetworkOfPoses2DInf, mrpt::graphs::CNetworkOfPoses3DInf
Template Parameters
GRAPH_TNormally a mrpt::graphs::CNetworkOfPoses<EDGE_TYPE,MAPS_IMPLEMENTATION>. Users won't have to write this template argument by hand, since the compiler will auto-fit it depending on the type of the graph object.
See also
The example "graph_slam_demo"
Implementation can be found in file levmarq_impl.h

Definition at line 80 of file levmarq.h.

References __CURRENT_FUNCTION_NAME__, ASSERT_ABOVE_, ASSERT_EQUAL_, ASSERTDEB_, ASSERTDEBMSG_, begin(), mrpt::math::CSparseMatrix::compressFromTriplet(), mrpt::utils::CTimeLogger::enter(), mrpt::graphslam::TResultInfoSpaLevMarq::final_total_sq_error, mrpt::utils::find_in_vector(), mrpt::format(), mrpt::mrpt::format(), mrpt::math::CSparseMatrix::insert_entry_fast(), mrpt::math::CSparseMatrix::insert_submatrix(), mrpt::utils::keep_max(), mrpt::mrpt::utils::keep_max(), mrpt::utils::CTimeLogger::leave(), mrpt::system::os::memcpy(), MRPT_END, MRPT_START, mrpt::math::norm(), mrpt::math::norm_inf(), mrpt::graphslam::TResultInfoSpaLevMarq::num_iters, mrpt::utils::square(), mrpt::math::UNINITIALIZED_MATRIX, and mrpt::poses::UNINITIALIZED_POSE.

Page generated by Doxygen 1.8.14 for MRPT 1.0.2 SVN: at lun oct 28 00:52:41 CET 2019 Hosted on: Logo