[mrpt-graphslam]

GraphSLAM algorithms

Library mrpt-graphslam

This C++ library is part of MRPT and can be installed in Debian-based systems with:

sudo apt install libmrpt-graphslam-dev

Basic graph-SLAM algorithms: See the namespace mrpt::graphslam.

For an introduction to graph-slam maps refer to the summary in https://www.mrpt.org/Graph-SLAM_maps

Library contents

// structs

struct mrpt::graphslam::apps::TOptimizerProps;
struct mrpt::graphslam::apps::TRegistrationDeciderProps;
struct mrpt::graphslam::TResultInfoSpaLevMarq;
struct mrpt::graphslam::TSlidingWindow;

template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
struct mrpt::graphslam::TUncertaintyPath;

template <class GRAPH_t>
struct mrpt::graphslam::apps::TUserOptionsChecker;

template <class GRAPH_T>
struct mrpt::graphslam::graphslam_traits;

// classes

class mrpt::graphslam::detail::CEdgeCounter;

template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class mrpt::graphslam::deciders::CEdgeRegistrationDecider;

template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class mrpt::graphslam::deciders::CEmptyNRD;

template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class mrpt::graphslam::deciders::CFixedIntervalsNRD;

template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class mrpt::graphslam::CGraphSlamEngine;

template <class GRAPH_t = typename mrpt::graphs::CNetworkOfPoses2DInf>
class mrpt::graphslam::optimizers::CGraphSlamOptimizer;

template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class mrpt::graphslam::deciders::CICPCriteriaERD;

template <class GRAPH_T>
class mrpt::graphslam::deciders::CICPCriteriaNRD;

template <class GRAPH_T>
class mrpt::graphslam::deciders::CIncrementalNodeRegistrationDecider;

template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class mrpt::graphslam::optimizers::CLevMarqGSO;

template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class mrpt::graphslam::deciders::CLoopCloserERD;

template <class GRAPH_T>
class mrpt::graphslam::deciders::CNodeRegistrationDecider;

template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class mrpt::graphslam::deciders::CRangeScanOps;

template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class mrpt::graphslam::CRegistrationDeciderOrOptimizer;

class mrpt::graphslam::CWindowManager;
class mrpt::graphslam::CWindowObserver;

// global functions

template <
    class GRAPH_T,
    class FEEDBACK_CALLABLE = typename graphslam_traits<GRAPH_T>::TFunctorFeedback
    >
void mrpt::graphslam::optimize_graph_spa_levmarq(
    GRAPH_T& graph,
    TResultInfoSpaLevMarq& out_info,
    const std::set<mrpt::graphs::TNodeID>* in_nodes_to_optimize = nullptr,
    const mrpt::containers::yaml& extra_params = {},
    FEEDBACK_CALLABLE functor_feedback = FEEDBACK_CALLABLE()
    );

void mrpt::graphslam::registerAllClasses_mrpt_graphslam();

Global Functions

template <
    class GRAPH_T,
    class FEEDBACK_CALLABLE = typename graphslam_traits<GRAPH_T>::TFunctorFeedback
    >
void mrpt::graphslam::optimize_graph_spa_levmarq(
    GRAPH_T& graph,
    TResultInfoSpaLevMarq& out_info,
    const std::set<mrpt::graphs::TNodeID>* in_nodes_to_optimize = nullptr,
    const mrpt::containers::yaml& extra_params = {},
    FEEDBACK_CALLABLE functor_feedback = FEEDBACK_CALLABLE()
    )

Optimize a graph of pose constraints using the Sparse Pose Adjustment (SPA) sparse representation and a Levenberg-Marquardt 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.

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.
“initial_lambda”: (default=0) <=0 means auto guess, otherwise, initial lambda value for the lev-marq algorithm.
“tau”: (default=1e-6) 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

Implementation can be found in file levmarq_impl.h

Parameters:

graph	The input edges and output poses.
out_info	Some basic output information on the process.
nodes_to_optimize	The list of nodes whose global poses are to be optimized. If nullptr (default), all the node IDs are optimized (but that marked as root in the graph).
extra_params	Optional parameters, see below.
functor_feedback	Optional: 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.)
GRAPH_T	Normally 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.