Performance: RRTPlanner polygon transforms, PoseGraph.optimize nonzero/pad, and where build_kdtree() would help #755

Found while profiling the pgraph-using code for petercorke/pgraph-python#74. These are RTB-side costs for investigation, not bugs; nothing here changes behaviour. Measurements are single runs on one machine (Python 3.12) with RTB's own example maps and data, at RTB feat/symbolic-ik and pgraph 1.0.0, so treat shares as approximate (cProfile inflates call-heavy code).

1. RRTPlanner: the vehicle polygon transform costs more than collision detection

plan(showsamples=False, showvalid=False) on the docstring example map, seed 0:

Vehicle.polygon(q) is self._polygon.transformed(SE2(q)), called ~270k times at 400 nodes (~30 us per call against ~11 us for the intersection test). Most of it is building the SE2 (trot2, Pose2.__init__) and np.pad in the transform. Worth looking at: build the 2x2 rotation and translation directly, or transform all of the candidate configurations along a Dubins segment in one numpy call.

Also: plan() defaults to showsamples=True, showvalid=True, so a plain plan(goal=...) spends ~40% of its time in matplotlib Line2D.draw (3.7 s of 9.1 s at 100 nodes). That may be intended, but is worth knowing when timing it.

2. PoseGraph.optimize: ndarray.nonzero and per-factor np.pad

optimize(iterations=5):

A nonzero taking 0.1-0.4 s per call suggests a scan of a large (probably dense) array in or just below linearize_and_solve. I haven't looked at the source to find which one. linear_factors is called per edge each iteration (20k and 15k calls), with trinv2/np.pad inside, which could be vectorized over the edges.

pgraph is 1% of optimize and 19-33% of loading (0.04-0.1 s), so there is nothing to gain on the pgraph side.

3. build_kdtree() where it would help

pgraph's closest() is being changed so that it no longer loops over every vertex (pgraph#74): it will use a matrix of the coordinates, which add_vertex appends to, so interleaved add/query is fast. Because of that, LatticePlanner.plan and RRTPlanner.plan need no change: their closest calls are interleaved with add_vertex, which discards a k-d tree, so a tree would not be used there and building one each iteration would be slower.

For a graph that is finished and then queried, an explicit tree is faster again, and the gap grows with the number of vertices (the matrix is O(n) per query, the tree O(log n)):

Measured with pgraph's benchmark at N=1,000: no tree 726 us per closest (before the pgraph change) or 12 us (after), with tree 6 us. So for a single query the difference is microseconds. It matters for something that queries one graph repeatedly (many start/goal pairs on one PRM roadmap), and I would not add it unless that use case is real.

If it is added:

• build_kdtree() raises ValueError for a custom metric or when no vertex has a coordinate, and ImportError without scipy (a dependency of spatialmath, so present, but pgraph imports it lazily). The planners use the built-in L2 or SE2 metric, so it should be a call with no guard, but it should be checked.
• The tree is dropped by any add_vertex, removal or assignment to vertex.coord, which is why it can only be built at the end of plan.
• It needs a pgraph that has build_kdtree (in 1.0.0).

Not an issue

• PRMPlanner.plan is 92% collision checking (OccGrid.w2g/isoccupied, 637k calls at 400 points), with pgraph 8%, and the all-pairs vertex.distance loop is 0.3 s of 2.65 s.
• BundleAdjust.optimize (MVTB, so not this repo): pgraph 0.1%, camera Jacobians and scipy lil indexing dominate.

Related: #738 (pgraph 1.0 adaptation) and #746 (mobile planner/PoseGraph defects), whose tests would make a good place to check results are unchanged after any of the above.