perf(mobile): reduce RRT collision work and assemble sparse pose graphs

examples/benchmark_mobile.py

@@ -0,0 +1,122 @@
+#!/usr/bin/env python
+"""
+Benchmark RRT planning and pose-graph optimisation without plotting.
+
+Run this script with the same dependencies on each revision being compared.
+Construction and file loading are excluded from the timings. Each repetition
+starts with a fresh planner or graph, with a fixed RRT random seed.
+
+Use ``--output before.npz`` and ``--output after.npz`` to save the final tree,
+path and optimised poses for numerical comparison alongside the timings.
+The graph defaults to the bundled ``killian-small.toro`` dataset.
+
+For the larger examples in issue #755, use ``--npoints 400`` and
+``--graph data/killian.g2o.zip``. Timings depend on the machine and dependency
+versions; compare results from the same environment.
+"""
+
+import argparse
+import json
+import platform
+from importlib.metadata import version
+from pathlib import Path
+from statistics import median
+from time import perf_counter
+
+import numpy as np
+from spatialmath import Polygon2
+
+from roboticstoolbox import Bicycle, PolygonMap, PoseGraph, RRTPlanner
+
+
+def make_rrt(npoints: int, seed: int) -> RRTPlanner:
+    """
+    Build the RRT documentation example
+
+    :param npoints: number of vertices to add to the tree
+    :param seed: random seed for repeatable sampling
+    :returns: unplanned RRT instance with polygonal obstacles
+    """
+    workspace = PolygonMap(workspace=[0, 10])
+    workspace.add([(5, 50), (5, 6), (6, 6), (6, 50)])
+    workspace.add([(5, 4), (5, -50), (6, -50), (6, 4)])
+    outline = Polygon2([(-1.5, 0.75), (-1.5, -0.75), (1.5, -0.75), (1.5, 0.75)])
+    return RRTPlanner(
+        map=workspace,
+        vehicle=Bicycle(steer_max=0.4, L=2, polygon=outline),
+        npoints=npoints,
+        seed=seed,
+        progress=False,
+    )
+
+
+def main() -> None:
+    """Measure repeated runs and optionally save their final numerical results."""
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--repeats", type=int, default=3)
+    parser.add_argument("--npoints", type=int, default=100)
+    parser.add_argument("--seed", type=int, default=0)
+    parser.add_argument("--iterations", type=int, default=5)
+    parser.add_argument("--graph", default="data/killian-small.toro")
+    parser.add_argument("--output", type=Path)
+    args = parser.parse_args()
+    if min(args.repeats, args.npoints, args.iterations) < 1:
+        parser.error("repeats, npoints and iterations must be positive")
+
+    rrt_times: list[float] = []
+    graph_times: list[float] = []
+    for _ in range(args.repeats):
+        rrt = make_rrt(args.npoints, args.seed)
+        start = perf_counter()
+        rrt.plan(goal=(8, 2, -np.pi / 2), showsamples=False, showvalid=False)
+        rrt_times.append(perf_counter() - start)
+
+        pg = PoseGraph(args.graph)
+        start = perf_counter()
+        pg.optimize(iterations=args.iterations)
+        graph_times.append(perf_counter() - start)
+
+    print(
+        json.dumps(
+            {
+                "python": platform.python_version(),
+                "platform": platform.platform(),
+                "dependencies": {
+                    name: version(name)
+                    for name in (
+                        "numpy",
+                        "scipy",
+                        "spatialmath-python",
+                        "pgraph-python",
+                    )
+                },
+                "rrt": {
+                    "npoints": args.npoints,
+                    "seed": args.seed,
+                    "seconds": rrt_times,
+                    "median": median(rrt_times),
+                },
+                "posegraph": {
+                    "file": args.graph,
+                    "vertices": pg.graph.n,
+                    "edges": pg.graph.ne,
+                    "iterations": args.iterations,
+                    "seconds": graph_times,
+                    "median": median(graph_times),
+                },
+            },
+            indent=2,
+        )
+    )
+    if args.output is not None:
+        path, _ = rrt.query((2, 8, -np.pi / 2))
+        np.savez(
+            args.output,
+            tree=np.array([v.coord for v in rrt.g]),
+            path=path,
+            poses=np.array([v.coord for v in pg.graph]),
+        )
+
+
+if __name__ == "__main__":
+    main()

src/roboticstoolbox/mobile/PoseGraph.py

@@ -704,18 +704,20 @@ def linearize_and_solve(self) -> float:
         If the graph is ``verbose`` the cost and the time taken are printed.
 
         The first vertex is anchored, since the measurements are all relative.
+        The system matrix is assembled from sparse 3-by-3 blocks, so assembly
+        storage grows with the graph's vertices and edges rather than the
+        square of its vertex count.
 
         .. seealso:: :meth:`optimize`
         """
         t0 = time.time()
 
         g = self.graph
 
-        # H and b are respectively the system matrix and the system vector
-        H = np.zeros((g.n * 3, g.n * 3))
+        # Each edge contributes four 3x3 blocks. Assemble only those blocks,
+        # rather than allocating and scanning a dense (3N, 3N) matrix.
+        blocks: dict[tuple[int, int], NDArray] = {(0, 0): np.zeros((3, 3))}
         b = np.zeros((g.n * 3, 1))
-        # this loop constructs the global system by accumulating in H and b the contributions
-        # of all edges (see lecture)
 
         etotal = 0
         for edge in g.edges():
@@ -736,28 +738,41 @@ def linearize_and_solve(self) -> float:
             islice = slice(i * 3, (i + 1) * 3)
             jslice = slice(j * 3, (j + 1) * 3)
 
-            # accumulate the blocks in H and b
-            H[islice, islice] += Hii
-            H[jslice, jslice] += Hjj
-            H[islice, jslice] += Hij
-            H[jslice, islice] += Hij.T
+            # RVC3, Appendix F.2.4.1: accumulate H_ij += ... and b_i += ...
+            # over the edges, storing only the nonzero blocks of H.
+            # Keep the original accumulation order: sorting duplicate COO
+            # entries can change rounding in poorly conditioned pose graphs.
+            for key, block in (
+                ((i, i), Hii),
+                ((j, j), Hjj),
+                ((i, j), Hij),
+                ((j, i), Hij.T),
+            ):
+                if key in blocks:
+                    blocks[key] += block
+                else:
+                    blocks[key] = block.copy()
 
             b[islice, 0] += bi
             b[jslice, 0] += bj
 
             etotal += np.inner(e, e)
 
-        # note that the system (H b) is obtained only from
-        # relative constraints. H is not full rank.
-        # we solve the problem by anchoring the position of
-        # the the first vertex.
-        # this can be expressed by adding the equation
-        #  deltax(1:3,1) = 0
-        # which is equivalent to the following
-
-        H[0:3, 0:3] += np.eye(3)
-
-        SH = scipy.sparse.csr_matrix(H)
+        # Relative constraints leave a global translation and rotation free.
+        # Anchor the first pose by adding an identity block to its diagonal.
+        blocks[0, 0] += np.eye(3)
+
+        # Expand each block's vertex indices into scalar matrix coordinates
+        # in the same row-major order as its nine flattened coefficients.
+        indices = np.array(list(blocks))
+        rows = (3 * indices[:, 0, None] + np.repeat(np.arange(3), 3)).ravel()
+        cols = (3 * indices[:, 1, None] + np.tile(np.arange(3), 3)).ravel()
+        SH = scipy.sparse.coo_matrix(
+            (np.array(list(blocks.values())).ravel(), (rows, cols)),
+            shape=(g.n * 3, g.n * 3),
+        ).tocsr()
+        # Match the nonzero structure of the original dense-to-CSR conversion.
+        SH.eliminate_zeros()
         deltax = scipy.sparse.linalg.spsolve(SH, b)  # SH \ b
 
         # split the increments in nice 3x1 vectors and sum them up to the original matrix

src/roboticstoolbox/mobile/RRTPlanner.py

@@ -206,6 +206,11 @@ def plan(
             if path is None:
                 continue
 
+            # Reject overlong connections before transforming the vehicle at
+            # every path sample and testing its polygon against the map.
+            if pstatus.length > 6:
+                continue
+
             collision = False
             for config in path:
                 if self.map.iscollision(vehicle.polygon(config)):
@@ -215,10 +220,6 @@ def plan(
                 # print('collision')
                 continue
 
-            if pstatus.length > 6:
-                # print('too long')
-                continue
-
             # we have a valid configuration to add to the graph
             count += 1
             self.progress_next()

tests/test_mobile_performance.py

@@ -0,0 +1,134 @@
+"""
+Regression tests for mobile planning optimisations.
+
+Compare sparse pose-graph assembly with the original dense equations, and
+check that RRT rejects overlong connections before doing collision work.
+Wall-clock timings belong in benchmarks, not assertions on a shared CI runner.
+"""
+
+from types import SimpleNamespace
+from typing import Any
+from unittest.mock import patch
+
+import numpy as np
+import numpy.testing as nt
+import pgraph
+import pytest
+import scipy.sparse
+import scipy.sparse.linalg
+import spatialmath.base as smb
+from spatialmath import Polygon2
+
+from roboticstoolbox import Bicycle, PolygonMap, PoseGraph, RRTPlanner
+from roboticstoolbox.mobile.PoseGraph import PGEdge, PGVertex
+from roboticstoolbox.tools.types import NDArray
+
+
+def dense_system(pg: PoseGraph) -> tuple[NDArray, NDArray, float]:
+    """
+    Assemble the original dense normal equations as a reference
+
+    :param pg: pose graph at the current iteration
+    :returns: system matrix, right-hand side and total edge error
+    """
+    H = np.zeros((3 * pg.graph.n, 3 * pg.graph.n))
+    b = np.zeros((3 * pg.graph.n, 1))
+    energy = 0.0
+    for edge in pg.graph.edges():
+        e, A, B = edge.linear_factors()
+        omega = edge.info
+        i, j = [vertex.index for vertex in edge.endpoints]
+        si, sj = slice(3 * i, 3 * i + 3), slice(3 * j, 3 * j + 3)
+        H[si, si] += A.T @ omega @ A
+        H[sj, sj] += B.T @ omega @ B
+        Hij = A.T @ omega @ B
+        H[si, sj] += Hij
+        H[sj, si] += Hij.T
+        b[si, 0] += -A.T @ omega @ e
+        b[sj, 0] += -B.T @ omega @ e
+        energy += np.inner(e, e)
+    H[:3, :3] += np.eye(3)
+    return H, b, energy
+
+
+@pytest.mark.parametrize("repeated_edge", [False, True])
+@pytest.mark.parametrize("offset", [0.0, 3.0])
+def test_sparse_posegraph_matches_dense_iterations(
+    repeated_edge: bool, offset: float
+) -> None:
+    pg = PoseGraph("data/pg1.g2o")
+    for vertex in pg.graph:
+        vertex.coord[2] += offset
+    if repeated_edge:
+        # Repeated constraints must accumulate into the same blocks, including
+        # when the endpoints occur in the opposite order.
+        first = pg.graph.edges()[0]
+        v1, v2 = first.endpoints
+        v1.connect(v2, edge=PGEdge(v1, v2, first.mean.copy(), first.info.copy()))
+        mean = smb.tr2xyt(smb.trinv2(smb.xyt2tr(first.mean)))
+        v2.connect(v1, edge=PGEdge(v2, v1, mean, first.info.copy()))
+
+    solve = scipy.sparse.linalg.spsolve
+    for _ in range(3):
+        H, b, energy = dense_system(pg)
+        expected = np.array([v.coord.copy() for v in pg.graph])
+        expected += solve(scipy.sparse.csr_matrix(H), b).reshape((-1, 3))
+        expected[:, 2] = smb.angdiff(expected[:, 2])
+        with patch.object(scipy.sparse.linalg, "spsolve", wraps=solve) as solver:
+            assert pg.linearize_and_solve() == energy
+        matrix, vector = solver.call_args.args
+        nt.assert_array_equal(matrix.toarray(), H)
+        nt.assert_array_equal(vector, b)
+        nt.assert_array_equal(np.array([v.coord for v in pg.graph]), expected)
+
+
+def test_posegraph_does_not_allocate_a_dense_system() -> None:
+    pg = PoseGraph("data/killian-small.toro")
+    zeros = np.zeros
+
+    def bounded_zeros(
+        shape: int | tuple[int, ...], *args: Any, **kwargs: Any
+    ) -> NDArray:
+        if isinstance(shape, tuple) and shape == (3 * pg.graph.n, 3 * pg.graph.n):
+            pytest.fail("pose-graph assembly allocated a quadratic dense matrix")
+        return zeros(shape, *args, **kwargs)
+
+    with patch.object(np, "zeros", side_effect=bounded_zeros):
+        assert np.isfinite(pg.linearize_and_solve())
+
+
+def test_single_pose_without_edges_is_anchored() -> None:
+    pg = PoseGraph("data/pg1.g2o")
+    pg.graph = pgraph.UGraph()
+    vertex = PGVertex("vertex", 0, coord=np.array([2.0, -1.0, 0.4]))
+    pg.graph.add_vertex(vertex)
+    before = vertex.coord.copy()
+    assert pg.linearize_and_solve() == 0
+    nt.assert_allclose(vertex.coord, before, atol=1e-15)
+
+
+@pytest.mark.parametrize("length", [6.01, 20.0])
+def test_rrt_rejects_overlong_paths_before_collision_checks(length: float) -> None:
+    outline = Polygon2([(-0.1, -0.1), (0.1, -0.1), (0.1, 0.1), (-0.1, 0.1)])
+    vehicle = Bicycle(polygon=outline)
+    planner = RRTPlanner(
+        map=PolygonMap(workspace=[0, 10]), vehicle=vehicle, npoints=1, progress=False
+    )
+    path = np.array([[1.0, 0, 0], [0.0, 0, 0]])
+    # The second candidate is exactly at the length limit and must still have
+    # every configuration checked before it can enter the tree.
+    candidates = [
+        (path, SimpleNamespace(length=length)),
+        (path, SimpleNamespace(length=6)),
+    ]
+    with (
+        patch.object(planner, "qrandom_free", return_value=path[0]),
+        patch.object(planner.dubins, "query", side_effect=candidates),
+        patch.object(vehicle, "polygon", wraps=vehicle.polygon) as polygon,
+        patch.object(planner.map, "iscollision", return_value=False) as collision,
+    ):
+        planner.plan(goal=path[-1], showsamples=False, showvalid=False)
+    assert polygon.call_count == len(path)
+    assert collision.call_count == len(path)
+    assert planner.g.n == 2
+    nt.assert_array_equal(planner.g[1].path, path)