Python example: mrpt_graphslam_example.py

Optimizes a noisy 2D pose graph with loop closures with mrpt.graphslam.

Modules: mrpt.graphs, mrpt.graphslam, mrpt.poses

 1#!/usr/bin/env python3
 2"""
 3Optimizes a noisy 2D pose graph with loop closures with mrpt.graphslam.
 4
 5Demonstrates:
 6  - CNetworkOfPoses2DInf: pose graph whose edges have information matrices
 7  - dijkstra_nodes_estimate(): initial guess of the node poses from the edges
 8  - optimize_graph_spa_levmarq(): Levenberg-Marquardt optimization, with
 9    parameters and a per-iteration feedback function
10"""
11
12import math
13import random
14
15import numpy as np
16from mrpt.graphs import CNetworkOfPoses2DInf
17from mrpt.graphslam import optimize_graph_spa_levmarq
18from mrpt.poses import CPose2D, CPosePDFGaussianInf
19
20random.seed(1)
21
22# ---------------------------------------------------------------------------
23# Ground truth: a robot driving three laps of a square, 5 poses per side
24# ---------------------------------------------------------------------------
25N_LAPS, N_SIDES, POSES_PER_SIDE, STEP = 3, 4, 5, 1.0
26gt = [CPose2D(0, 0, 0)]
27for lap in range(N_LAPS):
28    for side in range(N_SIDES):
29        for k in range(POSES_PER_SIDE):
30            turn = math.pi / 2 if k == POSES_PER_SIDE - 1 else 0.0
31            gt.append(gt[-1] + CPose2D(STEP, 0, turn))
32n_nodes = len(gt)
33poses_per_lap = N_SIDES * POSES_PER_SIDE
34
35# ---------------------------------------------------------------------------
36# Noisy odometry edges, plus loop closures between the same place in each lap
37# ---------------------------------------------------------------------------
38odo_sigmas = np.array([0.05, 0.05, math.radians(2.0)])
39loop_sigmas = np.array([0.02, 0.02, math.radians(0.5)])
40
41
42def noisy_edge(i, j, sigmas):
43    """The relative pose of node j from node i, with Gaussian noise."""
44    true_rel = gt[j] - gt[i]
45    noise = CPose2D(*[random.gauss(0, s) for s in sigmas])
46    information = np.diag(1.0 / sigmas**2)
47    return CPosePDFGaussianInf(true_rel + noise, information)
48
49
50graph = CNetworkOfPoses2DInf()
51for i in range(n_nodes - 1):
52    graph.insertEdge(i, i + 1, noisy_edge(i, i + 1, odo_sigmas))
53n_loops = 0
54for i in range(n_nodes):
55    for j in range(i + poses_per_lap, n_nodes, poses_per_lap):
56        graph.insertEdge(i, j, noisy_edge(i, j, loop_sigmas))
57        n_loops += 1
58
59# Initial guess of the node poses: composing edges from the root (node 0),
60# which accumulates the odometry drift.
61graph.root = 0
62graph.dijkstra_nodes_estimate()
63print(f"Graph: {graph.nodeCount()} nodes, {graph.edgeCount()} edges ({n_loops} loop closures)")
64
65
66def rmse_position(g):
67    return math.sqrt(sum((g.getNodePose(i).x - p.x) ** 2 + (g.getNodePose(i).y - p.y) ** 2
68                         for i, p in enumerate(gt)) / n_nodes)
69
70
71# The error of the initial guess (max_iterations=0 does not change the graph):
72info = optimize_graph_spa_levmarq(graph, extra_params={"max_iterations": 0})
73print(f"Before: total squared error = {info.final_total_sq_error:.3f}, "
74      f"RMSE of positions = {rmse_position(graph):.3f} m")
75
76
77def on_iteration(g, iteration, max_iterations, total_sq_error):
78    print(f"  iteration {iteration:2d}/{max_iterations}: total squared error = "
79          f"{total_sq_error:.4f}")
80
81
82info = optimize_graph_spa_levmarq(
83    graph, extra_params={"max_iterations": 50}, feedback=on_iteration)
84print(f"After:  total squared error = {info.final_total_sq_error:.3f}, "
85      f"RMSE of positions = {rmse_position(graph):.3f} m")
86print(info)