diff --git a/examples/benchmark_mobile.py b/examples/benchmark_mobile.py new file mode 100644 index 000000000..d55cc94fc --- /dev/null +++ b/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() diff --git a/src/roboticstoolbox/mobile/PoseGraph.py b/src/roboticstoolbox/mobile/PoseGraph.py index b0b1876db..47c0497c3 100644 --- a/src/roboticstoolbox/mobile/PoseGraph.py +++ b/src/roboticstoolbox/mobile/PoseGraph.py @@ -704,6 +704,9 @@ 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` """ @@ -711,11 +714,10 @@ def linearize_and_solve(self) -> float: 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 diff --git a/src/roboticstoolbox/mobile/RRTPlanner.py b/src/roboticstoolbox/mobile/RRTPlanner.py index 178b89e5c..d59d15c2d 100644 --- a/src/roboticstoolbox/mobile/RRTPlanner.py +++ b/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() diff --git a/tests/test_mobile_performance.py b/tests/test_mobile_performance.py new file mode 100644 index 000000000..23ff9be15 --- /dev/null +++ b/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)