mrpt.graphslam

mrpt.graphslam: pose-graph optimization.

Provides:

• optimize_graph_spa_levmarq : Levenberg-Marquardt optimization of the node poses of a CNetworkOfPoses2D, CNetworkOfPoses3D, CNetworkOfPoses2DInf or CNetworkOfPoses3DInf graph (mrpt.graphs)

• TResultInfoSpaLevMarq : Its output information

Example:

import mrpt.graphs as graphs
import mrpt.graphslam as graphslam

g = graphs.CNetworkOfPoses2D()
g.loadFromTextFile("graph.g2o")
g.dijkstra_nodes_estimate()
info = graphslam.optimize_graph_spa_levmarq(g, extra_params={"max_iterations": 50})
print(info.final_total_sq_error)

Package Contents

class mrpt.graphslam.TResultInfoSpaLevMarq

C++ API: struct mrpt::graphslam::TResultInfoSpaLevMarq

Output information of optimize_graph_spa_levmarq().

property final_total_sq_error: float

Sum of the squared errors of all the edges (squared Mahalanobis distances for edges with an information matrix).

property num_iters: int

Index of the last Levenberg-Marquardt iteration executed.

mrpt.graphslam.optimize_graph_spa_levmarq(graph: mrpt.graphs.CNetworkOfPoses2D, nodes_to_optimize: set[int] | None = None, extra_params: dict | None = None, feedback: Callable | None = None) → TResultInfoSpaLevMarq

mrpt.graphslam.optimize_graph_spa_levmarq(graph: mrpt.graphs.CNetworkOfPoses3D, nodes_to_optimize: set[int] | None = None, extra_params: dict | None = None, feedback: Callable | None = None) → TResultInfoSpaLevMarq

mrpt.graphslam.optimize_graph_spa_levmarq(graph: mrpt.graphs.CNetworkOfPoses2DInf, nodes_to_optimize: set[int] | None = None, extra_params: dict | None = None, feedback: Callable | None = None) → TResultInfoSpaLevMarq

mrpt.graphslam.optimize_graph_spa_levmarq(graph: mrpt.graphs.CNetworkOfPoses3DInf, nodes_to_optimize: set[int] | None = None, extra_params: dict | None = None, feedback: Callable | None = None) → TResultInfoSpaLevMarq

Optimizes the node poses of a pose graph (in place) with the Sparse Pose Adjustment (SPA) method and Levenberg-Marquardt, from the initial estimate in its nodes (e.g. from dijkstra_nodes_estimate()).

nodes_to_optimize: set of node IDs to optimize (default: all but the root). extra_params: optional dict with any of: verbose (bool, default False), max_iterations (default 100), profiler (bool, default False), initial_lambda (default 0: automatic), tau (default 1e-3), e1 (default 1e-6: stop if |gradient| < e1), e2 (default 1e-6: stop if |delta| < e2*(|x|+e2)). feedback: optional function(graph, iteration, max_iterations, total_sq_error) called on each iteration. Returns a TResultInfoSpaLevMarq. Edges with information matrices (the *Inf graphs) weight the residuals; other edges have unit information.