Hacktoberfest 2026：维护者为十月标记出来的 issue，仍然开放、适合新手。 浏览 Hacktoberfest issue

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

未关闭
  • 哪里不对？
#755 1 条评论 0 个 reaction 已指派 0 人 在 GitHub 查看

维护者通常 1 天内回复

还没有人认领这个 Issue。

评估

难度
4/5
预计耗时
3-5 天
新手友好度
18/100
Issue 类型
重构
描述清晰度
基本清楚
活跃度
活跃
技术栈
numpy, python
领域
performance, robotics

调研方向

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.

由索引模型根据 Issue 内容生成。

描述

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.

主要语言
C++
星标
3.5k
派生
639
平均合并
1 天 8 小时
30 天内合并 PR
113

环境准备

从这里开始

  1. 先读完整个 Issue，再读项目的贡献指南。
  2. 在 Issue 下留言说明你要接手 —— 这能避免两个人做同样的事。
  3. Fork 仓库，在一个分支上完成修改。
  4. 提交 Pull Request，并在描述里引用这个 Issue 编号。

petercorke/robotics-toolbox-python 的其他 Issue

查看 petercorke/robotics-toolbox-python 的全部 Issue

相似的 Issue

更多 C++ Issue

把新 issue 发到你的邮箱

精选适合新手参与的 GitHub issue 摘要。