Performance: closest() is a Python loop on the interleaved add/query path used by RTB planners #74

Activity

1. petercorke

   added

   enhancementNew feature or requestNew feature or request

   on Oct 13, 2026
2. petercorke commented on Oct 13, 2026

   

   petercorke

   on Oct 13, 2026

   OwnerAuthor

   More actions

   Step 1 result: profiling the RTB planners

   cProfile of RTB (feat/symbolic-ik, editable) against pgraph main (74402d5). "pgraph share" is tottime of functions defined in pgraph/, as a fraction of the profiled time. Scripts were run once, on one machine, with RTB's own example maps.

   Planner	Size	Wall	pgraph share	Dominant cost
   PRM plan	100 / 400 points	0.15 s / 2.65 s	8% (400)	collision checks (OccGrid.w2g/isoccupied, 637k calls)
   RRT plan	50 / 200 nodes	2.6 s / 32.5 s	1%	Dubins and polygon collision checking
   Lattice plan	372 / 780 / 1292 vertices	0.13 / 0.68 / 1.9 s	84% / 88% / 90%	Graph.closest(): 2.2M metric calls at 1292 vertices
   Queries (all)		<= 0.02 s	n/a	negligible

   What this changes

   • The hypothesis above is wrong for RRT and PRM: the k-d tree is never used there, but pgraph is 1% and 8% of the time, so a faster closest would not move them at these sizes.
   • Lattice is the case that matters: ~90% of plan time is the brute-force closest() loop, growing ~3x per iteration. It interleaves add_vertex with closest, so an explicit build_kdtree() (option 3 in the proposal) cannot help it; the vectorized closest (option 2) can.
   • So the proposal narrows to the vectorized closest, benchmarked on the Lattice workload; the RTB build_kdtree() change is dropped unless larger PRM/RRT sizes show otherwise.

   Caveats

   • Small sizes only; collision checking may also scale with size, so the balance for PRM/RRT at larger sizes is unmeasured.
   • Profiler overhead inflates call-heavy code; shares are approximate.
   • PoseGraph and BundleAdjust not yet profiled.
3. petercorke commented on Oct 13, 2026

   

   petercorke

   on Oct 13, 2026

   OwnerAuthor

   More actions

   Step 1, continued: RRT breakdown, PoseGraph, BundleAdjust

   Correction to the previous comment. RRTPlanner.plan() defaults to showsamples=True, showvalid=True, so the RRT figures there included matplotlib drawing (3.7 s of 9.1 s at 100 nodes). Re-run with plotting off:

   Nodes	Total	Vehicle.polygon() (SE2 + transform)	Polygon2.intersects	Dubins query	pgraph self
   100	1.80 s	0.94 s (52%)	0.35 s (19%)	0.38 s (21%)	0.07 s (4%)
   400	16.7 s	8.14 s (49%)	3.06 s (18%)	2.93 s (18%)	2.09 s (12.5%)

   • Within RRT collision checking, the polygon transform (~30 us/call, building the SE2) costs ~2.7x the intersection test (~11 us/call). That is an RTB/spatialmath cost, not pgraph.
   • pgraph's share is not 1%: it grows with the node count (4% -> 12.5% from 100 to 400 nodes), consistent with closest() being quadratic over an interleaved add/query loop, so RRT also benefits from a faster closest() at larger sizes.

   PoseGraph (killian-small.toro 1,941 vertices / 3,995 edges; killian.g2o 3,873 / 4,987):
   load is 19-33% pgraph but 0.04-0.1 s in total; optimize(iterations=5) takes ~2.1 s / ~2.5 s with pgraph at 1%. The time is in numpy/scipy, including ndarray.nonzero (0.39 s / 1.1 s over 3-5 calls) and ~60k np.pad calls, which look like RTB-side costs.

   BundleAdjust (10 views x 300 landmarks, 3,000 projections): build 0.07 s (pgraph 10%), optimize 2.8 s with pgraph at 0.1%; the time is camera Jacobians (cameraModel, 1.0 s) and scipy sparse indexing.

   Summary

   Algorithm	pgraph share	Dominant cost
   Lattice plan	~90%	closest() brute force
   RRT plan	4% -> 12.5% (growing)	polygon transform, intersects, Dubins
   PRM plan	8%	occupancy-grid collision checks
   PoseGraph, BundleAdjust	<= 1%	numpy/scipy, camera Jacobians

   Same caveats as before: single runs, one machine, RTB's example data, profiler overhead. Scripts were run from the session scratchpad and are not in the repo.

   Plan: implement the vectorized closest() (proposal option 2) and benchmark on the Lattice and RRT workloads.
4. petercorke commented on Oct 17, 2026

   

   petercorke

   on Oct 17, 2026

   OwnerAuthor

   More actions

   Implemented: #81

   The vectorized closest() / near() (proposal option 2) is in #81. Measured, main against the branch:

   • closest, N=1,000: 726 us -> 11.9 us
   • LatticePlanner.plan, 1,292 vertices: 1.07 s -> 0.06 s
   • RRTPlanner.plan, 300 nodes: 5.1 s -> 4.3 s
   • RRT, PRM and Lattice produce identical vertices, edges and paths; add_vertex is unchanged.

   Left open

   • Assign-after pattern. v = g.add_vertex(); v.coord = x drops the matrix, so each following query rebuilds it (O(n); 0.21 ms against 1.34 ms on main at n=2,000). Passing the coordinate to add_vertex(coord=x) or the vertex constructor avoids it. A fix would give every vertex a row (NaN when it has no valid coordinate), aligned with the vertex list, and overwrite the row on assignment; not done because no RTB or MVTB code path assigns after adding.
   • RTB side: Performance: RRTPlanner polygon transforms, PoseGraph.optimize nonzero/pad, and where build_kdtree() would help robotics-toolbox-python#755 (polygon transforms, PoseGraph.optimize, and where build_kdtree() would help).
   • In-place edits (v.coord[0] = x) are not seen by closest; documented.