forked from zbw/yiliao2026
275 lines
11 KiB
Markdown
275 lines
11 KiB
Markdown
# Lightweight Kalman Obstacle Tracker Implementation Plan
|
|
|
|
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
|
|
|
**Goal:** Replace the laser-frame EMA tracker with a fixed-size constant-velocity Kalman tracker that remains accurate at 2 m/s, suppresses one-frame false detections, and bridges short sparse-scan dropouts.
|
|
|
|
**Architecture:** `ObstacleTracker` owns one `[x,y,vx,vy]` state and 4x4 covariance per track. Every scan predicts all tracks from the LaserScan timestamp, greedily associates fit observations and then chord observations using Mahalanobis plus Euclidean gates, performs source-specific Kalman updates, and applies separate confirmation, publication, and deletion lifetimes.
|
|
|
|
**Tech Stack:** ROS2 Humble, C++17, Eigen fixed-size matrices, ament_cmake_gtest, colcon with `--symlink-install`.
|
|
|
|
---
|
|
|
|
## File Map
|
|
|
|
- Modify `src/obstacle_scanner/include/obstacle_scanner/tracking.hpp`: Kalman configuration, track state, timestamped API, debug counters.
|
|
- Modify `src/obstacle_scanner/src/tracking.cpp`: prediction, gating, association, correction, lifecycle.
|
|
- Modify `src/obstacle_scanner/test/test_tracking.cpp`: test-first behavioral coverage.
|
|
- Modify `src/obstacle_scanner/src/obstacle_scanner_node.cpp`: timestamp, parameters, debug JSON.
|
|
- Modify `src/obstacle_scanner/config/params.yaml`: parameter migration while preserving the user's `radius_min: 0.02`.
|
|
- Do not change detection behavior, messages, launch files, TF, or navigation packages.
|
|
|
|
### Task 1: Define Timestamped Lifecycle Behavior
|
|
|
|
**Files:**
|
|
- Modify: `src/obstacle_scanner/test/test_tracking.cpp`
|
|
- Modify: `src/obstacle_scanner/include/obstacle_scanner/tracking.hpp`
|
|
- Modify: `src/obstacle_scanner/src/tracking.cpp`
|
|
|
|
- [ ] **Step 1: Write failing tentative and confirmation tests**
|
|
|
|
Use this exact configuration and timestamped calls:
|
|
|
|
```cpp
|
|
TrackerConfig test_config()
|
|
{
|
|
TrackerConfig config;
|
|
config.track_confirm_fit_hits = 2;
|
|
config.track_confirm_total_hits = 3;
|
|
config.track_publish_misses = 3;
|
|
config.track_delete_misses = 5;
|
|
config.process_accel_noise = 3.0;
|
|
config.initial_velocity_stddev = 2.5;
|
|
config.fit_position_stddev = 0.02;
|
|
config.chord_position_stddev = 0.06;
|
|
config.mahalanobis_gate = 9.21;
|
|
config.max_association_distance = 0.35;
|
|
config.max_position_stddev = 0.15;
|
|
config.chord_tolerance = 0.02;
|
|
config.nominal_dt = 1.0 / 12.0;
|
|
config.min_dt = 0.02;
|
|
config.max_dt = 0.20;
|
|
return config;
|
|
}
|
|
```
|
|
|
|
Assert one fit at `t=0` creates one active track but publishes none. Assert a second fit at `t=1/12` confirms one published track. Check `tracks_created` and `tracks_confirmed`.
|
|
|
|
- [ ] **Step 2: Verify RED**
|
|
|
|
Run the Release symlink build. Expected: compilation fails because the new config fields, `update(observations, stamp_seconds)`, and debug counters do not exist.
|
|
|
|
- [ ] **Step 3: Add the minimal API and lifecycle**
|
|
|
|
Replace obsolete EMA fields with the fields above. Change the API to:
|
|
|
|
```cpp
|
|
FrameDebug update(
|
|
const std::vector<Observation> & observations,
|
|
double stamp_seconds);
|
|
```
|
|
|
|
Define each track with `Eigen::Vector4d state`, `Eigen::Matrix4d covariance`, radius, id, fit hits, total hits, misses, confirmation, match flag, and source. Add tracker-level last timestamp initialized to NaN. Implement only creation and two-fit confirmation.
|
|
|
|
- [ ] **Step 4: Verify GREEN**
|
|
|
|
Run `./build/obstacle_scanner/test_tracking`. Expected: new lifecycle tests pass.
|
|
|
|
- [ ] **Step 5: Commit**
|
|
|
|
```bash
|
|
git add src/obstacle_scanner/include/obstacle_scanner/tracking.hpp src/obstacle_scanner/src/tracking.cpp src/obstacle_scanner/test/test_tracking.cpp
|
|
git commit -m "refactor: define Kalman track lifecycle"
|
|
```
|
|
|
|
### Task 2: Implement Prediction And Fit Correction
|
|
|
|
**Files:**
|
|
- Modify: `src/obstacle_scanner/test/test_tracking.cpp`
|
|
- Modify: `src/obstacle_scanner/src/tracking.cpp`
|
|
|
|
- [ ] **Step 1: Write the failing 2 m/s test**
|
|
|
|
Feed fits at x positions `1.0`, `1.1666667`, and `1.3333333` at 12 Hz, followed by one empty frame. Assert one confirmed predicted track and `center_x` within `0.08 m` of `1.5`.
|
|
|
|
- [ ] **Step 2: Verify RED**
|
|
|
|
Run `./build/obstacle_scanner/test_tracking --gtest_filter='*TracksTwoMetersPerSecond*'`. Expected: failure because velocity is not estimated.
|
|
|
|
- [ ] **Step 3: Implement fixed-size prediction**
|
|
|
|
Use:
|
|
|
|
```cpp
|
|
Eigen::Matrix4d transition = Eigen::Matrix4d::Identity();
|
|
transition(0, 2) = dt;
|
|
transition(1, 3) = dt;
|
|
Eigen::Matrix<double, 4, 2> gain;
|
|
gain << 0.5 * dt * dt, 0.0,
|
|
0.0, 0.5 * dt * dt,
|
|
dt, 0.0,
|
|
0.0, dt;
|
|
const Eigen::Matrix4d process_noise =
|
|
std::pow(config_.process_accel_noise, 2) * gain * gain.transpose();
|
|
```
|
|
|
|
Predict `state = F * state` and `P = F * P * F.transpose() + Q`. Use `nominal_dt` for invalid or non-positive deltas and clamp valid deltas to `[min_dt,max_dt]`.
|
|
|
|
- [ ] **Step 4: Implement fit correction**
|
|
|
|
Use a 2x4 position matrix, `fit_position_stddev^2 * I`, an LDLT solve for the 2x2 innovation covariance, and Joseph covariance update:
|
|
|
|
```cpp
|
|
P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose();
|
|
```
|
|
|
|
- [ ] **Step 5: Verify and commit**
|
|
|
|
Run all tracking tests, then commit `tracking.cpp` and its test with message `feat: add constant velocity Kalman tracking`.
|
|
|
|
### Task 3: Add Fit And Chord Association
|
|
|
|
**Files:**
|
|
- Modify: `src/obstacle_scanner/test/test_tracking.cpp`
|
|
- Modify: `src/obstacle_scanner/src/tracking.cpp`
|
|
|
|
- [ ] **Step 1: Write failing association tests**
|
|
|
|
Add independent tests proving:
|
|
|
|
```text
|
|
two fits 0.1667 m apart -> one confirmed track
|
|
a fit 0.50 m away -> new tentative track, no duplicate published track
|
|
one fit plus two valid chords -> one confirmed track
|
|
a chord beyond 0.35 m -> no update and one association rejection
|
|
```
|
|
|
|
Preserve the existing far-side candidate geometry assertion.
|
|
|
|
- [ ] **Step 2: Verify RED**
|
|
|
|
Run the association and chord tests. Expected: Mahalanobis, hard-distance, and fit-plus-two-chords cases fail.
|
|
|
|
- [ ] **Step 3: Implement candidate assignment**
|
|
|
|
For each source pass, construct candidates containing observation index, track index, source-specific center, squared Mahalanobis distance, and Euclidean distance. Accept only:
|
|
|
|
```cpp
|
|
mahalanobis_squared <= config_.mahalanobis_gate &&
|
|
euclidean_distance <= config_.max_association_distance
|
|
```
|
|
|
|
Sort by Mahalanobis score and greedily accept pairs whose observation and track are unused. Process fits before chords. Unmatched fits create tentative tracks; unmatched chords are discarded and counted. Chords use `chord_position_stddev` and never update radius.
|
|
|
|
- [ ] **Step 4: Verify GREEN**
|
|
|
|
Run all tracking tests. Expected: association, geometry, confirmation, and prediction tests pass.
|
|
|
|
- [ ] **Step 5: Commit**
|
|
|
|
Commit tracker and test changes with message `feat: gate fit and chord associations`.
|
|
|
|
### Task 4: Bound Publication And Deletion
|
|
|
|
**Files:**
|
|
- Modify: `src/obstacle_scanner/test/test_tracking.cpp`
|
|
- Modify: `src/obstacle_scanner/src/tracking.cpp`
|
|
|
|
- [ ] **Step 1: Write failing boundary tests**
|
|
|
|
After two fits confirm a track, assert misses 1-3 publish prediction, miss 4 remains internal but is not published, and miss 5 deletes it. Add tests for uncertainty stopping publication early and tentative deletion on its first miss.
|
|
|
|
- [ ] **Step 2: Verify RED**
|
|
|
|
Run `./build/obstacle_scanner/test_tracking --gtest_filter='*Miss*:*Uncertainty*:*Tentative*'`. Expected: lifecycle boundaries fail.
|
|
|
|
- [ ] **Step 3: Implement separate predicates**
|
|
|
|
Delete tentative tracks at one miss and confirmed tracks at `track_delete_misses`. Publish only confirmed tracks satisfying:
|
|
|
|
```cpp
|
|
missed_count <= config_.track_publish_misses &&
|
|
std::sqrt(std::max(covariance(0, 0), covariance(1, 1))) <=
|
|
config_.max_position_stddev
|
|
```
|
|
|
|
Populate created, confirmed, deleted, fit/chord update, rejection, predicted publication, mean innovation, and maximum innovation debug fields.
|
|
|
|
- [ ] **Step 4: Verify GREEN**
|
|
|
|
Run both `test_tracking` and `test_detection`. Expected: zero failed tests.
|
|
|
|
- [ ] **Step 5: Commit**
|
|
|
|
Commit tracker and tests with message `feat: bound predicted obstacle lifetime`.
|
|
|
|
### Task 5: Integrate ROS Parameters And Diagnostics
|
|
|
|
**Files:**
|
|
- Modify: `src/obstacle_scanner/src/obstacle_scanner_node.cpp`
|
|
- Modify: `src/obstacle_scanner/config/params.yaml`
|
|
|
|
- [ ] **Step 1: Verify the node API is RED**
|
|
|
|
Run the Release build after the timestamped tracker API exists. Expected: node compilation fails at the old one-argument update call.
|
|
|
|
- [ ] **Step 2: Wire timestamps and parameters**
|
|
|
|
Declare every new config field. Convert the scan timestamp using:
|
|
|
|
```cpp
|
|
const double stamp_seconds = rclcpp::Time(msg->header.stamp).seconds();
|
|
tracker_debug = tracker_.update(observations, stamp_seconds);
|
|
```
|
|
|
|
Remove obsolete `track_confirm_hits`, `association_gate`, `fit_update_alpha`, and `chord_update_alpha`.
|
|
|
|
- [ ] **Step 3: Expand debug JSON**
|
|
|
|
Add exact keys `tracks_created`, `tracks_confirmed`, `tracks_deleted`, `fit_updates`, `chord_updates`, `association_rejections`, `predicted_published`, `mean_innovation`, and `max_innovation`. Preserve existing keys.
|
|
|
|
- [ ] **Step 4: Migrate YAML carefully**
|
|
|
|
Preserve `radius_min: 0.02`. Add every Kalman and lifecycle parameter with units and behavior comments. Keep `debug` as master and `debug_info` as text-only sub-switch.
|
|
|
|
- [ ] **Step 5: Build and commit**
|
|
|
|
Run the Release symlink build. Expected: one package finishes with exit code zero. Commit only scanner node and YAML with message `feat: configure lightweight Kalman tracker`.
|
|
|
|
### Task 6: Full Verification And Smoke Test
|
|
|
|
**Files:**
|
|
- Verify only; do not alter unrelated workspace files.
|
|
|
|
- [ ] **Step 1: Fresh build**
|
|
|
|
```bash
|
|
source /opt/ros/humble/setup.bash
|
|
cd /home/sunrise/yiliao_ws
|
|
colcon build --symlink-install --packages-select obstacle_scanner --cmake-args -DCMAKE_BUILD_TYPE=Release
|
|
```
|
|
|
|
Expected: exit code zero.
|
|
|
|
- [ ] **Step 2: Fresh tests**
|
|
|
|
```bash
|
|
colcon test --packages-select obstacle_scanner --event-handlers console_direct+
|
|
colcon test-result --test-result-base build/obstacle_scanner/test_results --verbose
|
|
```
|
|
|
|
Expected: zero current failures. Check timestamps and run a checker directly if stale XML appears.
|
|
|
|
- [ ] **Step 3: Isolated ROS smoke node**
|
|
|
|
On domain 22, run a second node named `obstacle_scanner_kalman_smoke`, remap `/obstacles`, `/processed_scan`, and debug info to smoke-test topics, and set image stride to a very large value. Do not stop or rename the user's existing scanner.
|
|
|
|
- [ ] **Step 4: Observe for five seconds**
|
|
|
|
Verify obstacle and debug topics publish near 12 Hz, JSON is valid, processing remains far below the 83 ms scan period, and track counts stay bounded. Stop only the smoke node.
|
|
|
|
- [ ] **Step 5: Final scoped review**
|
|
|
|
Run `git status --short` and inspect only `src/obstacle_scanner` changes. Confirm `radius_min: 0.02` and all unrelated user modifications remain intact. Report build, tests, smoke evidence, and residual tuning risk.
|
|
|