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"""
10
11import math
12import tempfile, os
13from mrpt.graphs import CNetworkOfPoses2D, CNetworkOfPoses3D
14from mrpt.poses import CPose2D, CPose3D
15
16# ---------------------------------------------------------------------------
17# 2D pose graph
18# ---------------------------------------------------------------------------
19g2 = CNetworkOfPoses2D()
20
21# Add node poses (estimated positions)
22g2.setNodePose(0, CPose2D(0.0, 0.0, 0.0))
23g2.setNodePose(1, CPose2D(1.0, 0.0, 0.0))
24g2.setNodePose(2, CPose2D(1.0, 1.0, 1.5708))  # ~90 deg
25
26# Add odometry edges (relative transforms)
27g2.insertEdge(0, 1, CPose2D(1.0,  0.0, 0.0))
28g2.insertEdge(1, 2, CPose2D(0.0,  1.0, 1.5708))
29g2.insertEdge(2, 0, CPose2D(-1.0, -1.0, -1.5708))  # loop closure
30
31print(f"2D graph: {g2.nodeCount()} nodes, {g2.edgeCount()} edges")
32print(f"  node 1 pose: {g2.getNodePose(1)}")
33print(f"  node IDs: {g2.getNodeIDs()}")
34assert g2.hasNode(0) and g2.hasNode(2)
35assert not g2.hasNode(99)
36
37# ---------------------------------------------------------------------------
38# Save / load round-trip
39# ---------------------------------------------------------------------------
40with tempfile.NamedTemporaryFile(suffix=".graph", delete=False) as f:
41    fname = f.name
42try:
43    g2.saveToTextFile(fname)
44    g2b = CNetworkOfPoses2D()
45    g2b.loadFromTextFile(fname)
46    print(f"  After save/load: {g2b.nodeCount()} nodes, {g2b.edgeCount()} edges  ✓")
47    assert g2b.nodeCount() == 3
48finally:
49    os.unlink(fname)
50
51# ---------------------------------------------------------------------------
52# 3D pose graph
53# ---------------------------------------------------------------------------
54g3 = CNetworkOfPoses3D()
55g3.setNodePose(0, CPose3D(0, 0, 0, 0, 0, 0))
56g3.setNodePose(1, CPose3D(1, 0, 0, 0, 0, 0))
57g3.insertEdge(0, 1, CPose3D(1, 0, 0, 0, 0, 0))
58
59print(f"\n3D graph: {g3.nodeCount()} nodes, {g3.edgeCount()} edges")
60print(f"  {g3}")
61
62# ---------------------------------------------------------------------------
63# Shortest paths and global poses from the graph edges
64# ---------------------------------------------------------------------------
65ring = CNetworkOfPoses2D()
66for i in range(6):
67    ring.insertEdge(i, (i + 1) % 6, CPose2D(1.0, 0.0, math.radians(60)))
68print(f"\nRing graph: path 0 -> 4 = {ring.dijkstra_path(0, 4)}")
69print(f"  hops from node 0: {ring.getNodeDistances(0)}")
70ring.root = 0
71ring.dijkstra_nodes_estimate()
72print(f"  node 3 global pose: {ring.getNodePose(3)}")