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)}")