feat(calibration): robust depth refinement pipeline with diagnostics and benchmarking
This commit is contained in:
@@ -0,0 +1,60 @@
|
||||
## Robust Optimization Patterns
|
||||
- Use `method='trf'` for robust loss + bounds.
|
||||
- `loss='cauchy'` is highly effective for outlier-heavy depth data.
|
||||
- `f_scale` should be tuned to the expected inlier noise (e.g., sensor precision).
|
||||
- Weights must be manually multiplied into the residual vector.
|
||||
# Unit Hardening Learnings
|
||||
|
||||
- **SDK Unit Consistency**: Explicitly setting `init_params.coordinate_units = sl.UNIT.METER` ensures that all SDK-retrieved measures (depth, point clouds, tracking) are in meters, avoiding manual conversion errors.
|
||||
- **Double Scaling Guard**: When moving to SDK-level meter units, existing manual conversions (e.g., `/ 1000.0`) must be guarded or removed. Checking `cam.get_init_parameters().coordinate_units` provides a safe runtime check.
|
||||
- **Depth Sanity Logging**: Adding min/median/max/p95 stats for valid depth values in debug logs helps identify scaling issues (e.g., seeing values in the thousands when expecting meters) or data quality problems early.
|
||||
- **Loguru Integration**: Standardized on `loguru` for debug logging in `SVOReader` to match project patterns.
|
||||
|
||||
## Best-Frame Selection (Task 4)
|
||||
- Implemented `score_frame` function in `calibrate_extrinsics.py` to evaluate frame quality.
|
||||
- Scoring criteria:
|
||||
- Base score: `n_markers * 100.0 - reproj_err`
|
||||
- Depth bonus: Up to +50.0 based on valid depth ratio at marker corners.
|
||||
- Main loop now tracks the frame with the highest score per camera instead of just the latest valid frame.
|
||||
- Deterministic tie-breaking: The first frame with a given score is kept (implicitly by `current_score > best_so_far["score"]`).
|
||||
- This ensures depth verification and refinement use the highest quality data available in the SVO.
|
||||
- **Regression Testing for Units**: Added `tests/test_depth_units.py` which mocks `sl.Camera` and `sl.Mat` to verify that `_retrieve_depth` correctly handles both `sl.UNIT.METER` (no scaling) and `sl.UNIT.MILLIMETER` (divides by 1000) paths. This ensures the unit hardening is robust against future changes.
|
||||
|
||||
## Robust Optimizer Implementation (Task 2)
|
||||
- Replaced `minimize(L-BFGS-B)` with `least_squares(trf, soft_l1)`.
|
||||
- **Key Finding**: `soft_l1` loss with `f_scale=0.1` (10cm) effectively ignores 3m outliers in synthetic tests, whereas MSE is heavily biased by them.
|
||||
- **Regularization**: Split into `reg_rot` (0.1) and `reg_trans` (1.0) to penalize translation more heavily in meters.
|
||||
- **Testing**: Synthetic tests require careful depth map painting to ensure markers project into the correct "measured" regions as the optimizer moves the camera. A 5x5 window lookup means we need to paint at least +/- 30 pixels to cover the optimization trajectory.
|
||||
- **Convergence**: `least_squares` with robust loss may stop slightly earlier than MSE on clean data due to gradient dampening; relaxed tolerance to 5mm for unit tests.
|
||||
|
||||
## Task 5: Diagnostics and Acceptance Gates
|
||||
- Surfaced rich optimizer diagnostics in `refine_extrinsics_with_depth` stats: `termination_status`, `nfev`, `njev`, `optimality`, `n_active_bounds`.
|
||||
- Added data quality counts: `n_points_total`, `n_depth_valid`, `n_confidence_rejected`.
|
||||
- Implemented warning gates in `calibrate_extrinsics.py`:
|
||||
- Negligible improvement: Warns if `improvement_rmse < 1e-4` after more than 5 iterations.
|
||||
- Stalled/Failed: Warns if `success` is false or `nfev <= 1`.
|
||||
- These diagnostics provide better visibility into why refinement might be failing or doing nothing, which is critical for the upcoming benchmark matrix (Task 6).
|
||||
|
||||
## Benchmark Matrix Implementation
|
||||
- Added `--benchmark-matrix` flag to `calibrate_extrinsics.py`.
|
||||
- Implemented `run_benchmark_matrix` to compare 4 configurations:
|
||||
1. baseline (linear loss, no confidence)
|
||||
2. robust (soft_l1, f_scale=0.1, no confidence)
|
||||
3. robust+confidence (soft_l1, f_scale=0.1, confidence weights)
|
||||
4. robust+confidence+best-frame (same as 3 but using the best-scored frame instead of the first valid one)
|
||||
- The benchmark results are printed as a table to stdout and saved in the output JSON under the `benchmark` key for each camera.
|
||||
- Captured `first_frames` in the main loop to provide a consistent baseline for comparison against the `best_frame` (verification_frames).
|
||||
|
||||
## Documentation Updates (2026-02-07)
|
||||
|
||||
### Workflow Documentation
|
||||
- Updated `docs/calibrate-extrinsics-workflow.md` to reflect the new robust refinement pipeline.
|
||||
- Added documentation for new CLI flags: `--use-confidence-weights`, `--benchmark-matrix`.
|
||||
- Explained the switch from `L-BFGS-B` (MSE) to `least_squares` (Soft-L1) for robust optimization.
|
||||
- Documented the "Best Frame Selection" logic (scoring based on marker count, reprojection error, and valid depth).
|
||||
- Marked the "Unit Mismatch" issue as resolved due to explicit meter enforcement in `SVOReader`.
|
||||
|
||||
### Key Learnings
|
||||
- **Documentation as Contract**: Updating the docs *after* implementation revealed that the "Unit Mismatch" section was outdated. Explicitly marking it as "Resolved" preserves the history while clarifying current behavior.
|
||||
- **Benchmark Matrix Value**: Documenting the benchmark matrix makes it a first-class citizen in the workflow, encouraging users to empirically verify refinement improvements rather than trusting defaults.
|
||||
- **Confidence Weights**: Explicitly documenting this feature highlights the importance of sensor uncertainty in the optimization process.
|
||||
@@ -0,0 +1,13 @@
|
||||
# Depth Unit Scaling Patterns
|
||||
|
||||
## Findings
|
||||
- **Native SDK Scaling**: `depth_sensing.py` uses `init_params.coordinate_units = sl.UNIT.METER`.
|
||||
- **Manual Scaling**: `aruco/svo_sync.py` uses `depth_data / 1000.0` because it leaves `coordinate_units` at the default (`MILLIMETER`).
|
||||
|
||||
## Risks
|
||||
- **Double-Scaling**: If `svo_sync.py` is updated to use `sl.UNIT.METER` in `InitParameters`, the manual `/ 1000.0` MUST be removed, otherwise depth values will be 1000x smaller than intended.
|
||||
- **Inconsistency**: Different parts of the codebase handle unit conversion differently (SDK-level vs. Application-level).
|
||||
|
||||
## Recommendations
|
||||
- Standardize on `sl.UNIT.METER` in `InitParameters` across all ZED camera initializations.
|
||||
- Remove manual `/ 1000.0` scaling once SDK-level units are set to meters.
|
||||
Reference in New Issue
Block a user