diff --git a/CHANGELOG.md b/CHANGELOG.md index c39aabea..ba4bb965 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,19 @@ +Unreleased + +Performance + +- `closest()` and `near()` no longer make a Python-level pass over the vertices + when there is no k-d tree. The graph keeps the coordinates as a matrix, built + on first use and appended to by `add_vertex`, and applies the built-in metrics + to all rows at once. A query on a 1,000-vertex graph takes 12 microseconds + rather than 730, and stays that fast while vertices are being added, which is + what the Robotics Toolbox's `LatticePlanner` and `RRTPlanner` do (a lattice of + 1,292 vertices is planned in 0.06 s rather than 1.07 s). Results are the + same. The matrix is a copy of the coordinates, so changing one in place, + `vertex.coord[0] = x`, is not seen by `closest()`, as was already so with + `build_kdtree()`; assigning `vertex.coord` is. A metric of your own is still + applied to each vertex in turn. + 1.0.0 First 1.0 release: six years of production use since the first commit in 2020 diff --git a/src/pgraph/PGraph.py b/src/pgraph/PGraph.py index e0fa4de0..a59b01d4 100644 --- a/src/pgraph/PGraph.py +++ b/src/pgraph/PGraph.py @@ -39,6 +39,42 @@ def __init__(self, tree: Any, index: NDArray, p: float, prepare: Any): self.prepare = prepare # point -> point for the tree, or None if out of range +class _PointCache: + """ + The coordinates of a graph's embedded vertices, as rows of a matrix (private) + + Row ``i`` is the coordinate of ``vertices[i]``, in the order of the graph. + The buffer has spare rows so that adding a vertex is an append, which is + what lets :meth:`Graph.closest` stay fast while a planner interleaves + ``add_vertex`` with queries. + """ + + def __init__(self, points: NDArray, vertices: list[Vertex]): + n, dim = points.shape + self.buf = np.empty((n + n // 2 + 16, dim)) + self.buf[:n] = points + self.n = n + self.vertices = vertices + + @property + def dim(self) -> int: + return self.buf.shape[1] + + @property + def points(self) -> NDArray: + """The ``n`` x dim matrix of coordinates (a view, not a copy)""" + return self.buf[: self.n] + + def append(self, vertex: Vertex, coord: NDArray) -> None: + if self.n == self.buf.shape[0]: + grown = np.empty((2 * self.n, self.dim)) + grown[: self.n] = self.buf + self.buf = grown + self.buf[self.n] = coord + self.n += 1 + self.vertices.append(vertex) + + class SE2Metric: r""" Distance between planar poses, a weighted mixed norm @@ -294,6 +330,10 @@ def __init__( # vertices have changed since it was built self._kdtree: _KDTree | None = None self._spatialchange = False + # the coordinates of the embedded vertices as a matrix, built when + # first needed and kept up to date by add_vertex; None if not built or + # invalidated by a removal or an assignment to a ``coord`` + self._pts: _PointCache | None = None if metric is None: self.metric = "L2" else: @@ -980,7 +1020,7 @@ def subgraph(self, vertices: Iterable[Vertex | str] | NDArray) -> Graph: # pickling and copying #: the graph's own containers, which are saved as a flat description - _STRUCTURE = ("_vertexlist", "_vertexdict", "_edges", "_kdtree") + _STRUCTURE = ("_vertexlist", "_vertexdict", "_edges", "_kdtree", "_pts") #: the references a vertex holds, which are rebuilt rather than saved _VERTEX_LINKS = ("_edgelist", "_inlist", "_graph") @@ -1043,6 +1083,7 @@ def __setstate__(self, state: dict[str, Any]) -> None: state[key] = _METRICS[value[1]] self.__dict__.update(state) self._kdtree = None + self._pts = None self._spatialchange = False self._vertexlist = [] @@ -1186,6 +1227,16 @@ def add_vertex( vertex.__dict__["_graph"] = self self._connectivitychange = True self._spatialchange = True + pts = self._pts + if pts is not None and vertex.coord is not None: + # keep the matrix of coordinates up to date; one that does not + # fit it (another length, not finite) is left for _points to + # judge, by dropping the matrix + coord = vertex.coord + if coord.ndim == 1 and coord.shape[0] == pts.dim and np.isfinite(coord).all(): + pts.append(vertex, coord) + else: + self._pts = None return vertex @classmethod @@ -1471,6 +1522,7 @@ def remove_vertex(self, vertex: Vertex) -> None: vertex._label = None # and so not in a component self._connectivitychange = True self._spatialchange = True + self._pts = None def remove(self, x: Edge | Vertex) -> None: """ @@ -1979,12 +2031,14 @@ def build_kdtree(self) -> None: 3 elements, or no vertex has a coordinate :raises ImportError: scipy is not installed - Finding the nearest vertex to a point is a pass over all the vertices, - which is the cost of a query on a large graph that is queried often. A - k-d tree answers in a time that grows more slowly, at the cost of - building it once. :meth:`closest` and :meth:`near` use the index if it - has been built, and fall back to the pass over the vertices if it has - not or if the graph has changed. + Without an index, finding the nearest vertex to a point measures the + distance to every vertex, which is the cost of a query on a large graph + that is queried often. A k-d tree answers in a time that grows more + slowly, at the cost of building it once, which pays for a graph that is + built and then queried. A graph that is queried while it is growing + does better without it, since adding a vertex discards the index. + :meth:`closest` and :meth:`near` use the index if it has been built, + and measure every vertex if it has not or if the graph has changed. The index is for the built-in metrics ``"L1"``, ``"L2"`` and ``"SE2"`` (with any weight, see :class:`SE2Metric`), and results @@ -2107,6 +2161,36 @@ def _metric_columns(self, diff: NDArray) -> NDArray | None: ) return None + def _points(self) -> _PointCache | None: + """ + The coordinates of the embedded vertices as a matrix, or None (private) + + Built on first use and kept by :meth:`add_vertex`. It is None, so + that the caller makes its pass over the vertices, if no vertex has a + coordinate or the coordinates are not all vectors of the same length, + which that pass reports. A vertex whose coordinate is not finite is + left out, as it is never the closest. + """ + if self._pts is not None: + return self._pts + vertices = [v for v in self._vertexlist if v.coord is not None] + if not vertices: + return None + try: + points = np.array([v.coord for v in vertices], dtype=float) + except (ValueError, TypeError): + return None # not all the same length, or not numbers + if points.ndim != 2 or points.shape[1] == 0: + return None + finite = np.isfinite(points).all(axis=1) + if not finite.all(): + points = points[finite] + vertices = [v for v, ok in zip(vertices, finite) if ok] + if not vertices: + return None + self._pts = _PointCache(points, vertices) + return self._pts + def closest(self, coord: ArrayLike) -> tuple[Vertex | None, float]: """ Vertex closest to point @@ -2121,8 +2205,17 @@ def closest(self, coord: ArrayLike) -> tuple[Vertex | None, float]: according to the graph's metric. Vertices without a coordinate (``coord`` is None) are skipped -- they have no position to compare. - This is a pass over the vertices, unless a spatial index has been - built with :meth:`build_kdtree` and is still valid. + For the built-in metrics the distance to every vertex is found in one + operation on a matrix of the coordinates that the graph keeps, which + adding a vertex only appends to, so this stays fast when vertices are + added between queries, as a planner does. A spatial index built with + :meth:`build_kdtree`, if it is still valid, is faster again for a graph + that is not changing. A metric of your own is applied to each vertex + in turn. + + .. note:: The matrix is a copy, so a coordinate changed in place, + ``vertex.coord[0] = x``, is not seen. Assigning + ``vertex.coord = x`` (or ``+=``) is. .. runblock:: pycon @@ -2143,6 +2236,15 @@ def closest(self, coord: ArrayLike) -> tuple[Vertex | None, float]: if np.isfinite(d): return self._vertexlist[int(index.index[i])], float(d) + pts = self._points() + if pts is not None: + q = np.asarray(coord, dtype=float) + if q.shape == (pts.dim,) and np.isfinite(q).all(): + d = self._metric_columns((pts.points - q).T) + if d is not None: # a built-in metric, applied to all at once + i = int(np.argmin(d)) + return pts.vertices[i], float(d[i]) + min_dist = np.inf min_which: Vertex | None = None @@ -2224,9 +2326,9 @@ def near(self, coord: ArrayLike, radius: float) -> list[tuple[Vertex, float]]: vertices at equal distance are in the order of the graph. For the built-in metrics the distance to every vertex is found in one - operation on :meth:`coordinates`, or by the spatial index if - :meth:`build_kdtree` was called; a metric of your own is applied - to each vertex in turn. + operation on the matrix of coordinates that :meth:`closest` uses, or + by the spatial index if :meth:`build_kdtree` was called; a metric of + your own is applied to each vertex in turn. .. runblock:: pycon @@ -2251,14 +2353,14 @@ def near(self, coord: ArrayLike, radius: float) -> list[tuple[Vertex, float]]: candidates = [self._vertexlist[int(index.index[i])] for i in sorted(hits)] if candidates is None: - C = self.coordinates() + pts = self._points() point = np.asarray(coord, dtype=float) - if C.shape[0] != 0 and point.shape == (C.shape[0],): - d = self._metric_columns(C - point[:, None]) + if pts is not None and point.shape == (pts.dim,): + d = self._metric_columns((pts.points - point).T) if d is not None: hit = np.flatnonzero(d <= radius) hit = hit[np.argsort(d[hit], kind="stable")] - return [(self._vertexlist[i], float(d[i])) for i in hit] + return [(pts.vertices[i], float(d[i])) for i in hit] candidates = [v for v in self._vertexlist if v.coord is not None] found = [] @@ -4498,6 +4600,7 @@ def __setattr__(self, name: str, value: Any) -> None: graph = self.__dict__.get("_graph") if graph is not None: graph._spatialchange = True + graph._pts = None object.__setattr__(self, name, value) def __reduce__(self) -> Any: diff --git a/tests/test_point_cache.py b/tests/test_point_cache.py new file mode 100644 index 00000000..4bc0b55c --- /dev/null +++ b/tests/test_point_cache.py @@ -0,0 +1,250 @@ +""" +The matrix of coordinates that ``closest`` and ``near`` use when there is no +spatial index, and that ``add_vertex`` keeps up to date. + +Expectations are computed here, by hand, from the coordinates that were put +in; never by the code under test. The cache is private, so what is checked +is that answers are the same as the pass over the vertices would give, +through every way the graph can change. +""" + +import copy +import pickle # nosec B403 nosemgrep - only used to round-trip graphs made in this file +import unittest + +import numpy as np + +from pgraph import SE2Metric, UGraph, Vertex + + +def roundtrip(obj): + """Pickle and unpickle: these tests only read what they have just written""" + return pickle.loads(pickle.dumps(obj)) # nosec B301 nosemgrep + + +def oracle(points, q, p=2.0): + """Index and distance of the nearest of ``points`` to ``q``, first on ties""" + best, best_d = None, np.inf + for i, x in enumerate(points): + if x is None: + continue + d = float(np.sum(np.abs(np.asarray(x, float) - np.asarray(q, float)) ** p) ** (1 / p)) + if d < best_d: + best, best_d = i, d + return best, best_d + + +class TestClosestMatrix(unittest.TestCase): + def check(self, g, points, q, p=2.0): + v, d = g.closest(q) + i, d_expected = oracle(points, q, p) + if i is None: + self.assertIsNone(v) + self.assertEqual(d, np.inf) + else: + self.assertIs(v, g[i]) + self.assertAlmostEqual(d, d_expected, places=12) + + def test_matches_oracle(self): + rng = np.random.default_rng(1) + pts = rng.uniform(-5, 5, (100, 2)) + g = UGraph() + for x in pts: + g.add_vertex(coord=x) + for q in rng.uniform(-6, 6, (20, 2)): + self.check(g, pts, q) + + def test_interleaved_add_and_query(self): + """The planners' pattern, over enough vertices to grow the buffer""" + rng = np.random.default_rng(2) + g = UGraph() + pts = [] + for _ in range(300): + x = rng.uniform(-5, 5, 2) + g.add_vertex(coord=x) + pts.append(x) + self.check(g, pts, rng.uniform(-5, 5, 2)) + g.validate() + + def test_l1_and_se2_metrics(self): + rng = np.random.default_rng(3) + pts = rng.uniform(-3, 3, (60, 2)) + g = UGraph(metric="L1") + for x in pts: + g.add_vertex(coord=x) + for q in rng.uniform(-3, 3, (10, 2)): + self.check(g, pts, q, p=1.0) + + # SE2: planar distance plus weighted, wrapped, heading difference + w = 2.0 + poses = np.column_stack([rng.uniform(-3, 3, (40, 2)), rng.uniform(-3, 3, 40)]) + h = UGraph(metric=SE2Metric(weight=w)) + for x in poses: + h.add_vertex(coord=x) + for q in poses[:5] + 0.01: + v, d = h.closest(q) + best = np.inf + for i, x in enumerate(poses): + th = (x[2] - q[2] + np.pi) % (2 * np.pi) - np.pi + best = min(best, np.hypot(np.hypot(x[0] - q[0], x[1] - q[1]), w * th)) + self.assertAlmostEqual(d, best, places=12) + + def test_first_wins_a_tie(self): + g = UGraph() + g.add_vertex(coord=[1, 0], name="a") + g.add_vertex(coord=[-1, 0], name="b") + self.assertEqual(g.closest([0, 0])[0].name, "a") + + def test_vertex_without_coordinate_is_skipped(self): + g = UGraph() + g.add_vertex(name="bare") + self.assertEqual(g.closest([0, 0]), (None, np.inf)) # nothing embedded + g.add_vertex(coord=[3, 4], name="p") + g.add_vertex(name="bare2") + g.add_vertex(coord=[1, 1], name="q") + v, d = g.closest([0, 0]) + self.assertEqual(v.name, "q") + self.assertAlmostEqual(d, np.sqrt(2)) + # built before the unembedded vertex was added, and after + self.assertEqual(g.closest([3, 4])[0].name, "p") + + def test_remove_vertex(self): + pts = [[0, 0], [1, 0], [2, 0]] + g = UGraph() + for i, x in enumerate(pts): + g.add_vertex(coord=x, name=f"v{i}") + self.assertEqual(g.closest([0.1, 0])[0].name, "v0") + g.remove_vertex(g["v0"]) + v, d = g.closest([0.1, 0]) + self.assertEqual(v.name, "v1") + self.assertAlmostEqual(d, 0.9) + g.validate() + + def test_assigning_a_coordinate(self): + g = UGraph() + a = g.add_vertex(coord=[0, 0], name="a") + g.add_vertex(coord=[5, 5], name="b") + self.assertEqual(g.closest([0.1, 0])[0].name, "a") + a.coord = np.array([9.0, 9.0]) + self.assertEqual(g.closest([0.1, 0])[0].name, "b") + a.coord += 1 # also seen + self.assertEqual(g.closest([10, 10])[0].name, "a") + + def test_in_place_change_is_not_seen(self): + """The documented limit, shared with the k-d tree""" + g = UGraph() + a = g.add_vertex(coord=[0.0, 0.0], name="a") + g.add_vertex(coord=[5.0, 5.0], name="b") + g.closest([0, 0]) # build + a.coord[0] = 9.0 + self.assertEqual(g.closest([0.1, 0])[0].name, "a") # the stale answer + + def test_nonfinite_coordinates_are_left_out(self): + g = UGraph() + g.add_vertex(coord=[np.nan, 0], name="nan") + g.add_vertex(coord=[1, 1], name="ok") + self.assertEqual(g.closest([0, 0])[0].name, "ok") # built without it + g.add_vertex(coord=[np.inf, 0], name="inf") # an append that is refused + self.assertEqual(g.closest([0, 0])[0].name, "ok") + h = UGraph() + h.add_vertex(coord=[np.nan, np.nan]) + self.assertEqual(h.closest([0, 0]), (None, np.inf)) + + def test_non_finite_query_and_wrong_length_query(self): + g = UGraph() + g.add_vertex(coord=[0, 0], name="a") + g.add_vertex(coord=[1, 1], name="b") + self.assertEqual(g.closest([0, 0])[0].name, "a") + v, d = g.closest([np.nan, 0]) # as the pass over the vertices does + self.assertIsNone(v) + with self.assertRaises(ValueError): + g.closest([0, 0, 0]) + + def test_a_vertex_of_another_length_is_reported(self): + g = UGraph() + g.add_vertex(coord=[0, 0]) + g.closest([0, 0]) # build + g.add_vertex(coord=[1, 2, 3]) # does not fit; the matrix is dropped + with self.assertRaises(ValueError): + g.closest([0, 0]) + h = UGraph() + h.add_vertex(coord=[0, 0]) + h.add_vertex(coord=[1, 2, 3]) + with self.assertRaises(ValueError): # ragged when first built + h.closest([0, 0]) + + def test_non_numeric_coordinate_falls_back(self): + g = UGraph() + g.add_vertex(coord=["a", "b"]) + with self.assertRaises(TypeError): # from the pass over the vertices + g.closest([0, 0]) + + def test_coordinate_that_is_not_a_vector_is_left_to_the_pass(self): + g = UGraph() + g.add_vertex(coord=np.ones((2, 2))) # a matrix + v, d = g.closest([1, 1]) # broadcast against it, as it always was + self.assertIs(v, g[0]) + self.assertEqual(d, 0.0) + self.assertIsNone(g._pts) + h = UGraph() + h.add_vertex(coord=[]) # no elements + self.assertEqual(h.closest([])[1], 0.0) + self.assertIsNone(h._pts) + + def test_user_metric_is_applied_to_each_vertex(self): + calls = [] + + def metric(v): + calls.append(1) + return float(np.abs(v).max()) + + g = UGraph(metric=metric) + for x in ([0, 0], [3, 1], [1, 3]): + g.add_vertex(coord=x) + v, d = g.closest([2, 0]) + self.assertEqual(d, 1.0) + self.assertGreaterEqual(len(calls), 3) + + def test_near(self): + rng = np.random.default_rng(4) + pts = rng.uniform(-5, 5, (80, 2)) + g = UGraph() + for x in pts: + g.add_vertex(coord=x) + g.add_vertex(name="bare") + q, r = np.array([0.5, -0.5]), 2.5 + got = g.near(q, r) + want = sorted( + [(i, float(np.hypot(*(x - q)))) for i, x in enumerate(pts) if np.hypot(*(x - q)) <= r], + key=lambda t: t[1], + ) + self.assertEqual([g._vertexlist.index(v) for v, _ in got], [i for i, _ in want]) + for (_, d), (_, e) in zip(got, want): + self.assertAlmostEqual(d, e, places=12) + # after a vertex is added, with the matrix already built + g.add_vertex(coord=q + 0.01, name="new") + self.assertEqual(g.near(q, r)[0][0].name, "new") + + def test_pickle_and_copy_do_not_carry_it(self): + g = UGraph() + for x in ([0, 0], [4, 4], [1, 1]): + g.add_vertex(coord=x) + g.closest([0, 0]) + for h in (roundtrip(g), copy.deepcopy(g), g.subgraph([True, True, False])): + self.assertIsNone(h._pts) + self.assertIs(h.closest([4, 4])[0], h[1]) + h.add_vertex(coord=[4.1, 4.1], name="x") + self.assertEqual(h.closest([4.2, 4.2])[0].name, "x") + h.validate() + + def test_vertex_instance_added(self): + g = UGraph() + g.add_vertex(coord=[0, 0]) + g.closest([0, 0]) + g.add_vertex(Vertex(coord=[2, 2], name="inst")) + g.add_vertex(Vertex(name="bare")) + self.assertEqual(g.closest([2.1, 2.1])[0].name, "inst") + + +if __name__ == "__main__": + unittest.main()