Hacktoberfest 2026: những issue maintainer đã đánh dấu cho tháng Mười, đang mở và phù hợp người mới. Xem issue Hacktoberfest

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

Đang mở
  • Có gì không ổn?
#755 1 bình luận 0 reaction 0 người được giao Xem trên GitHub

Maintainer thường phản hồi trong vòng 1 ngày

Chưa có ai nhận issue này.

Đánh giá

Độ khó
4/5
Thời gian dự kiến
3-5 ngày
Mức phù hợp với người mới
18/100
Loại issue
Tái cấu trúc
Độ rõ ràng
Khá rõ ràng
Mức độ hoạt động
Sôi nổi
Công nghệ
numpy, python
Lĩnh vực
performance, robotics

Hướng nghiên cứu

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.

Do mô hình lập chỉ mục viết ra từ nội dung của issue.

Mô tả

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.

Ngôn ngữ chính
C++
Star
3.5k
Fork
639
Merge trung bình
1 ngày 14 giờ
Pull request đã merge (30 ngày)
96

Chuẩn bị môi trường

Bắt đầu từ đâu

  1. Đọc hết issue, rồi đọc hướng dẫn đóng góp của dự án.
  2. Bình luận trên issue rằng bạn sẽ nhận — tránh hai người làm cùng một việc.
  3. Fork repository và làm thay đổi trên một nhánh.
  4. Mở pull request có tham chiếu số hiệu của issue.

Issue khác của petercorke/robotics-toolbox-python

Tất cả issue của petercorke/robotics-toolbox-python

Issue tương tự

Thêm issue về C++

Nhận issue mới trong hộp thư của bạn

Bản tóm tắt ngắn những issue GitHub phù hợp với người mới.