Python example: mrpt_graphs_example.py
Builds, saves and searches 2D and 3D pose graphs with mrpt.graphs.
Modules: mrpt.graphs, mrpt.poses
1#!/usr/bin/env python3
2"""
3Builds, saves and searches 2D and 3D pose graphs with mrpt.graphs.
4
5Demonstrates:
6 - CNetworkOfPoses2D: build, insert nodes/edges, query
7 - CNetworkOfPoses3D: same in 3D
8 - save/load to text file
9 - Dijkstra shortest paths and spanning tree, with custom edge costs
10"""
11
12import math
13import tempfile, os
14from mrpt.graphs import CNetworkOfPoses2D, CNetworkOfPoses3D
15from mrpt.poses import CPose2D, CPose3D
16
17# ---------------------------------------------------------------------------
18# 2D pose graph
19# ---------------------------------------------------------------------------
20g2 = CNetworkOfPoses2D()
21
22# Add node poses (estimated positions)
23g2.setNodePose(0, CPose2D(0.0, 0.0, 0.0))
24g2.setNodePose(1, CPose2D(1.0, 0.0, 0.0))
25g2.setNodePose(2, CPose2D(1.0, 1.0, 1.5708)) # ~90 deg
26
27# Add odometry edges (relative transforms)
28g2.insertEdge(0, 1, CPose2D(1.0, 0.0, 0.0))
29g2.insertEdge(1, 2, CPose2D(0.0, 1.0, 1.5708))
30g2.insertEdge(2, 0, CPose2D(-1.0, -1.0, -1.5708)) # loop closure
31
32print(f"2D graph: {g2.nodeCount()} nodes, {g2.edgeCount()} edges")
33print(f" node 1 pose: {g2.getNodePose(1)}")
34print(f" node IDs: {g2.getNodeIDs()}")
35assert g2.hasNode(0) and g2.hasNode(2)
36assert not g2.hasNode(99)
37
38# ---------------------------------------------------------------------------
39# Save / load round-trip
40# ---------------------------------------------------------------------------
41with tempfile.NamedTemporaryFile(suffix=".graph", delete=False) as f:
42 fname = f.name
43try:
44 g2.saveToTextFile(fname)
45 g2b = CNetworkOfPoses2D()
46 g2b.loadFromTextFile(fname)
47 print(f" After save/load: {g2b.nodeCount()} nodes, {g2b.edgeCount()} edges ✓")
48 assert g2b.nodeCount() == 3
49finally:
50 os.unlink(fname)
51
52# ---------------------------------------------------------------------------
53# 3D pose graph
54# ---------------------------------------------------------------------------
55g3 = CNetworkOfPoses3D()
56g3.setNodePose(0, CPose3D(0, 0, 0, 0, 0, 0))
57g3.setNodePose(1, CPose3D(1, 0, 0, 0, 0, 0))
58g3.insertEdge(0, 1, CPose3D(1, 0, 0, 0, 0, 0))
59
60print(f"\n3D graph: {g3.nodeCount()} nodes, {g3.edgeCount()} edges")
61print(f" {g3}")
62
63# ---------------------------------------------------------------------------
64# Shortest paths and global poses from the graph edges
65# ---------------------------------------------------------------------------
66ring = CNetworkOfPoses2D()
67for i in range(6):
68 ring.insertEdge(i, (i + 1) % 6, CPose2D(1.0, 0.0, math.radians(60)))
69print(f"\nRing graph: path 0 -> 4 = {ring.dijkstra_path(0, 4)}")
70print(f" hops from node 0: {ring.getNodeDistances(0)}")
71print(f" shortest-path tree, as {{node: parent}}: {ring.dijkstra_tree(0)}")
72# Custom edge costs, here metric lengths, instead of one per edge:
73metric = lambda from_id, to_id, edge: edge.norm()
74print(f" meters from node 0: {ring.getNodeDistances(0, edge_weight=metric)}")
75ring.root = 0
76ring.dijkstra_nodes_estimate()
77print(f" node 3 global pose: {ring.getNodePose(3)}")