Hacktoberfest 2026: los issues que los mantenedores marcaron para octubre, abiertos y aptos para principiantes. Explorar issues de Hacktoberfest

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

Abierto
  • ¿Algo no está bien?
#755 1 comentario 0 reacciones 0 asignados Ver en GitHub

Los mantenedores suelen responder en 1 día

Nadie ha tomado este issue todavía.

Evaluación

Dificultad
4/5
Tiempo estimado
3-5 días
Aptitud para principiantes
18/100
Tipo de issue
Refactorización
Claridad
Bastante claro
Estado de actividad
Activo
Stack tecnológico
numpy, python
Área
performance, robotics

Línea de trabajo

Start with Vehicle.polygon in the vehicle module, called from RRTPlanner.plan, and with PoseGraph.optimize, which calls linearize_and_solve and linear_factors. Profile the example map with cProfile to reproduce the timings. Done means a measured speedup with the existing RRT and PoseGraph tests still passing and unchanged results. The issue leaves the approach open, so agree the target with the maintainer before writing code.

Escrito por el modelo de indexación a partir del texto del issue.

Descripción

needs investigation tech-debt

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:

nodes total Vehicle.polygon() Polygon2.intersects DubinsPlanner.query
100 1.80 s 0.94 s (52%) 0.35 s (19%) 0.38 s (21%)
400 16.7 s 8.14 s (49%) 3.06 s (18%) 2.93 s (18%)

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):

file vertices / edges total ndarray.nonzero np.pad pgraph
killian-small.toro 1,941 / 3,995 2.10 s 0.39 s (5 calls) 60k calls, 0.14 s 1%
killian.g2o.zip 3,873 / 4,987 2.50 s 1.12 s (3 calls) 45k calls, 0.10 s 1%

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)):

where what worth it?
PRMPlanner.plan, once the roadmap is built, or lazily on the first query query calls closest twice and then path_Astar only if one plan is queried many times, or for very large npoints
LatticePlanner.plan, after the last iteration query calls closest twice same
RRTPlanner.plan, after the last node query calls closest once marginal

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.

Lenguaje dominante
C++
Estrellas
3.5k
Forks
639
Merge medio
1 d 8 h
PR fusionados (30 d)
113

Preparar el entorno

Primeros pasos

  1. Lee el issue completo y luego la guía de contribución del proyecto.
  2. Comenta en el issue que vas a ocuparte — evita que dos personas hagan lo mismo.
  3. Haz un fork del repositorio y trabaja en una rama.
  4. Abre un pull request que haga referencia al número del issue.

Más de petercorke/robotics-toolbox-python

Todos los issues de petercorke/robotics-toolbox-python

Issues similares

Más issues de C++

Recibe los nuevos issues en tu correo

Un resumen breve de issues de GitHub para principiantes.