Repository navigation
perf(mobile): reduce RRT collision work and assemble sparse pose graphs - #817
Conversation
Reject overlong RRT connections before collision checks. Assemble pose-graph normal equations from sparse blocks while preserving accumulation order. Add regression tests and a reproducible benchmark for petercorke#755.
Not up to standards ⛔🔴 Issues
|
| Category | Results |
|---|---|
| Security | 6 high |
🟢 Metrics 23 complexity · 0 duplication
Metric Results Complexity 23 Duplication 0
NEW Get contextual insights on your PRs based on Codacy's metrics, along with PR and Jira context, without leaving GitHub. Enable AI reviewer
TIP This summary will be updated as you push new changes.
Codecov Report❌ Patch coverage is
Additional details and impacted files@@ Coverage Diff @@
## main #817 +/- ##
=====================================
Coverage 0.00% 0.00%
=====================================
Files 146 146
Lines 17164 17168 +4
=====================================
- Misses 17164 17168 +4 ☔ View full report in Codecov by Harness. 🚀 New features to boost your workflow:
|
|
Thank you for the performance work on the RRT and the pose graph. I compared the optimised poses for One small request: the book's Appendix F.2.4.1 describes the edge loop with the H and b blocks accumulated and recommends sparse storage, which is what your code does. Could you add a short comment pointing there, so a reader can map the dictionary-of-blocks code to the book's H_ij += … step? Then I'll update the branch and merge it. |
Co-Authored-By: Codex (GPT-6) <noreply@openai.com>
|
Thank you, Peter, for checking the results against the book. I have added a short comment beside the block accumulation pointing to RVC3, Appendix F.2.4.1, and its H_ij and b_i accumulation steps. This only changes the comment; all 41 focused performance/PoseGraph tests (plus 5 subtests) pass. I also sent you an email from kachmarykmi@gmail.com to peter.corke@qut.edu.au. If you have time, I would be very grateful for a reply whenever convenient. Thank you again for your time and guidance! |
2 participants
Summary
RRT planning checked every candidate path for collisions before rejecting connections longer than its existing length limit. Reject these connections first, avoiding unnecessary vehicle polygon transforms and intersection tests.
PoseGraph allocated a dense
(3N, 3N)normal matrix and scanned it to create CSR. Accumulate only the contributing 3x3 blocks, then construct the sparse matrix directly. Preserve the original addition order and remove explicit zeros so the solver sees the same coefficients and sparsity structure.Adds eight regression cases and
examples/benchmark_mobile.py, which records timings and dependency versions and can save the final tree, path and poses for comparison. Tests cover the dense reference equations, repeated/reversed constraints, angle wrapping, a single isolated pose, avoiding dense allocation, and RRT's length boundary.Related issue
Addresses the unnecessary RRT collision work and dense PoseGraph assembly in #755. Per-factor transform batching and optional k-d trees remain outside this PR; this does not close the broader investigation.
Performance
Local medians of three runs, excluding loading/construction, on Windows with Python 3.12.14, NumPy 2.5.4, SciPy 1.18.1, SpatialMath 1.1.18 and pgraph 1.0.0. Baseline:
a4b27adf. RRT uses seed 0 with plotting disabled; PoseGraph uses up to five iterations.The saved RRT trees, query paths and optimised poses match exactly between revisions for both sizes in this environment. Timings are illustrative and machine-dependent.
Run on each revision with the same dependencies:
Validation
mypy: no issues in 163 source files. Ruff passes for the new files, adds no findings in modified modules, and formatting passes for all four files.git diff --checkpasses.Checklist
pytest) — focused suite passes; full-suite baseline failures described aboveImplementation, tests and documentation were developed with OpenAI Codex assistance.