docs(plan): start depth-extrinsic-verify and mark tasks 1-4 complete
This commit is contained in:
@@ -0,0 +1,707 @@
|
||||
# Depth-Based Extrinsic Verification and Refinement
|
||||
|
||||
## TL;DR
|
||||
|
||||
> **Quick Summary**: Add depth-based verification and refinement capabilities to the existing ArUco calibration CLI. Compare predicted depth (from computed extrinsics) against measured depth (from ZED sensors) to validate calibration quality, and optionally optimize extrinsics to minimize depth residuals.
|
||||
>
|
||||
> **Deliverables**:
|
||||
> - `aruco/depth_verify.py` - Depth residual computation and verification metrics
|
||||
> - `aruco/depth_refine.py` - Direct optimization to refine extrinsics using depth
|
||||
> - Extended `aruco/svo_sync.py` - Depth-enabled SVO reader
|
||||
> - Updated `calibrate_extrinsics.py` - New CLI flags for depth verification/refinement
|
||||
> - `tests/test_depth_verify.py` - Unit tests for depth modules
|
||||
> - Verification reports in JSON + optional CSV
|
||||
>
|
||||
> **Estimated Effort**: Medium (2-3 days)
|
||||
> **Parallel Execution**: YES - 2 waves
|
||||
> **Critical Path**: Task 1 → Task 2 → Task 4 → Task 5 → Task 6
|
||||
|
||||
---
|
||||
|
||||
## Context
|
||||
|
||||
### Original Request
|
||||
User wants to add a utility to examine/fuse the extrinsic parameters via depth info with the ArUco box. The goal is to verify that ArUco-computed extrinsics are correct by comparing predicted vs measured depth, and optionally refine them using direct optimization.
|
||||
|
||||
### Interview Summary
|
||||
**Key Discussions**:
|
||||
- Primary goal: Both verify AND refine extrinsics using depth data
|
||||
- Integration: Add to existing `calibrate_extrinsics.py` CLI (new flags)
|
||||
- Depth mode: CLI argument with default to NEURAL
|
||||
- Target geometry: Any markers from parquet file (not just ArUco box)
|
||||
|
||||
**User Decisions**:
|
||||
- Refinement method: Direct optimization (minimize depth residuals)
|
||||
- Output: Full reporting (console + JSON + optional CSV)
|
||||
- Depth filtering: Confidence-based with ZED thresholds
|
||||
- Testing: Tests after implementation
|
||||
- CLI flags: Separate `--verify-depth` and `--refine-depth` flags
|
||||
|
||||
### Research Findings
|
||||
- **ZED SDK depth**: `retrieve_measure(mat, MEASURE.DEPTH)` returns depth in meters
|
||||
- **Pixel access**: `mat.get_value(x, y)` returns depth at specific coordinates
|
||||
- **Depth residual**: `r = z_measured - z_predicted` where `z_predicted = (R @ P_world + t)[2]`
|
||||
- **Confidence filtering**: Use `MEASURE.CONFIDENCE` with threshold (lower = more reliable)
|
||||
- **Current SVOReader**: Uses `DEPTH_MODE.NONE` - needs extension for depth
|
||||
|
||||
### Metis Review
|
||||
**Identified Gaps** (addressed):
|
||||
- Transform chain clarity → Use existing `T_world_cam` convention from calibrate_extrinsics.py
|
||||
- Depth sampling at corners → Use 5x5 median window around projected pixel
|
||||
- Confidence threshold direction → Verify ZED semantics (0-100, lower = more confident)
|
||||
- Optimization bounds → Add regularization to stay within ±5cm / ±5° of initial
|
||||
- Unit consistency → Verify parquet uses meters (same as ZED depth)
|
||||
- Non-regression → Depth features strictly opt-in, no behavior change without flags
|
||||
|
||||
---
|
||||
|
||||
## Work Objectives
|
||||
|
||||
### Core Objective
|
||||
Add depth-based verification and optional refinement to the calibration pipeline, allowing users to validate and improve ArUco-computed extrinsics using ZED depth measurements.
|
||||
|
||||
### Concrete Deliverables
|
||||
- `py_workspace/aruco/depth_verify.py` - Depth residual computation
|
||||
- `py_workspace/aruco/depth_refine.py` - Extrinsic optimization
|
||||
- `py_workspace/aruco/svo_sync.py` - Extended with depth support
|
||||
- `py_workspace/calibrate_extrinsics.py` - Updated with new CLI flags
|
||||
- `py_workspace/tests/test_depth_verify.py` - Unit tests
|
||||
- Output: Verification stats in JSON, optional per-frame CSV
|
||||
|
||||
### Definition of Done
|
||||
- [ ] `uv run calibrate_extrinsics.py --help` → shows --verify-depth, --refine-depth, --depth-mode flags
|
||||
- [ ] Running without depth flags produces identical output to current behavior
|
||||
- [ ] `--verify-depth` produces verification metrics in output JSON
|
||||
- [ ] `--refine-depth` optimizes extrinsics and reports pre/post metrics
|
||||
- [ ] `--report-csv` outputs per-frame residuals to CSV file
|
||||
- [ ] `uv run pytest tests/test_depth_verify.py` → all tests pass
|
||||
|
||||
### Must Have
|
||||
- Extend SVOReader to optionally enable depth mode and retrieve depth maps
|
||||
- Compute depth residuals at detected marker corner positions
|
||||
- Use 5x5 median window for robust depth sampling
|
||||
- Confidence-based filtering (reject low-confidence depth)
|
||||
- Verification metrics: RMSE, mean absolute, median, depth-normalized error
|
||||
- Direct optimization using scipy.optimize.minimize with bounds
|
||||
- Regularization to prevent large jumps from initial extrinsics (±5cm, ±5°)
|
||||
- Report both depth metrics AND existing reprojection metrics pre/post refinement
|
||||
- JSON schema versioning field
|
||||
- Opt-in CLI flags (no behavior change when not specified)
|
||||
|
||||
### Must NOT Have (Guardrails)
|
||||
- NO bundle adjustment or intrinsics optimization
|
||||
- NO ICP or point cloud registration (use pixel-depth residuals only)
|
||||
- NO per-frame time-varying extrinsics
|
||||
- NO new detection pipelines (reuse existing ArUco detection)
|
||||
- NO GUI viewers or interactive tuning
|
||||
- NO modification of existing output format when depth flags not used
|
||||
- NO alternate ArUco detection code paths
|
||||
|
||||
---
|
||||
|
||||
## Verification Strategy
|
||||
|
||||
> **UNIVERSAL RULE: ZERO HUMAN INTERVENTION**
|
||||
>
|
||||
> ALL tasks must be verifiable by agent-executed commands. No "user visually confirms" criteria.
|
||||
|
||||
### Test Decision
|
||||
- **Infrastructure exists**: YES (pytest already in use)
|
||||
- **Automated tests**: YES (tests-after)
|
||||
- **Framework**: pytest
|
||||
|
||||
### Agent-Executed QA Scenarios (MANDATORY)
|
||||
|
||||
| Type | Tool | How Agent Verifies |
|
||||
|------|------|-------------------|
|
||||
| CLI | Bash | Run command, check exit code, parse output |
|
||||
| JSON output | Bash (jq/python) | Parse JSON, validate structure and values |
|
||||
| Unit tests | Bash (pytest) | Run tests, assert all pass |
|
||||
| Non-regression | Bash | Compare outputs with/without depth flags |
|
||||
|
||||
---
|
||||
|
||||
## Execution Strategy
|
||||
|
||||
### Parallel Execution Waves
|
||||
|
||||
```
|
||||
Wave 1 (Start Immediately):
|
||||
├── Task 1: Extend SVOReader for depth support
|
||||
└── Task 2: Create depth residual computation module
|
||||
|
||||
Wave 2 (After Wave 1):
|
||||
├── Task 3: Create depth refinement module (depends: 2)
|
||||
├── Task 4: Add CLI flags to calibrate_extrinsics.py (depends: 1, 2)
|
||||
└── Task 5: Integrate verification into CLI workflow (depends: 1, 2, 4)
|
||||
|
||||
Wave 3 (After Wave 2):
|
||||
├── Task 6: Integrate refinement into CLI workflow (depends: 3, 5)
|
||||
└── Task 7: Add unit tests (depends: 2, 3)
|
||||
|
||||
Critical Path: Task 1 → Task 2 → Task 4 → Task 5 → Task 6
|
||||
```
|
||||
|
||||
### Dependency Matrix
|
||||
|
||||
| Task | Depends On | Blocks | Can Parallelize With |
|
||||
|------|------------|--------|---------------------|
|
||||
| 1 | None | 4, 5 | 2 |
|
||||
| 2 | None | 3, 4, 5 | 1 |
|
||||
| 3 | 2 | 6 | 4 |
|
||||
| 4 | 1, 2 | 5, 6 | 3 |
|
||||
| 5 | 1, 2, 4 | 6, 7 | None |
|
||||
| 6 | 3, 5 | 7 | None |
|
||||
| 7 | 2, 3 | None | 6 |
|
||||
|
||||
---
|
||||
|
||||
## TODOs
|
||||
|
||||
- [x] 1. Extend SVOReader for depth support
|
||||
|
||||
**What to do**:
|
||||
- Modify `py_workspace/aruco/svo_sync.py`
|
||||
- Add `depth_mode` parameter to `SVOReader.__init__()` (default: `DEPTH_MODE.NONE`)
|
||||
- Add `enable_depth` property that returns True if depth_mode != NONE
|
||||
- Add `depth_map: Optional[np.ndarray]` field to `FrameData` dataclass
|
||||
- In `grab_all()` and `grab_synced()`, if depth enabled:
|
||||
- Call `cam.retrieve_measure(depth_mat, sl.MEASURE.DEPTH)`
|
||||
- Store `depth_mat.get_data().copy()` in FrameData
|
||||
- Add `get_depth_at(frame: FrameData, x: int, y: int) -> Optional[float]` helper
|
||||
- Add `get_depth_window_median(frame: FrameData, x: int, y: int, size: int = 5) -> Optional[float]`
|
||||
|
||||
**Must NOT do**:
|
||||
- Do NOT change default behavior (depth_mode defaults to NONE)
|
||||
- Do NOT retrieve depth when not needed (performance)
|
||||
|
||||
**Recommended Agent Profile**:
|
||||
- **Category**: `unspecified-low`
|
||||
- Reason: Extending existing class with new optional feature
|
||||
- **Skills**: []
|
||||
|
||||
**Parallelization**:
|
||||
- **Can Run In Parallel**: YES
|
||||
- **Parallel Group**: Wave 1 (with Task 2)
|
||||
- **Blocks**: Tasks 4, 5
|
||||
- **Blocked By**: None
|
||||
|
||||
**References**:
|
||||
- `py_workspace/aruco/svo_sync.py:35` - Current depth_mode = NONE setting
|
||||
- `py_workspace/depth_sensing.py:95` - retrieve_measure pattern
|
||||
- `py_workspace/libs/pyzed_pkg/pyzed/sl.pyi:9879-9941` - retrieve_measure API
|
||||
|
||||
**Acceptance Criteria**:
|
||||
|
||||
**Agent-Executed QA Scenarios:**
|
||||
|
||||
```
|
||||
Scenario: SVOReader with depth disabled (default)
|
||||
Tool: Bash (python)
|
||||
Steps:
|
||||
1. cd /workspaces/zed-playground/py_workspace
|
||||
2. python -c "from aruco.svo_sync import SVOReader; r = SVOReader([]); assert not r.enable_depth; print('PASS')"
|
||||
Expected Result: Prints "PASS"
|
||||
|
||||
Scenario: SVOReader accepts depth_mode parameter
|
||||
Tool: Bash (python)
|
||||
Steps:
|
||||
1. python -c "from aruco.svo_sync import SVOReader; import pyzed.sl as sl; r = SVOReader([], depth_mode=sl.DEPTH_MODE.NEURAL); assert r.enable_depth; print('PASS')"
|
||||
Expected Result: Prints "PASS"
|
||||
|
||||
Scenario: FrameData has depth_map field
|
||||
Tool: Bash (python)
|
||||
Steps:
|
||||
1. python -c "from aruco.svo_sync import FrameData; import numpy as np; f = FrameData(image=np.zeros((10,10,3), dtype=np.uint8), timestamp_ns=0, frame_index=0, serial_number=0, depth_map=None); print('PASS')"
|
||||
Expected Result: Prints "PASS"
|
||||
```
|
||||
|
||||
**Commit**: YES
|
||||
- Message: `feat(aruco): extend SVOReader with depth map support`
|
||||
- Files: `py_workspace/aruco/svo_sync.py`
|
||||
|
||||
---
|
||||
|
||||
- [x] 2. Create depth residual computation module
|
||||
|
||||
**What to do**:
|
||||
- Create `py_workspace/aruco/depth_verify.py`
|
||||
- Implement `project_point_to_pixel(P_cam: np.ndarray, K: np.ndarray) -> tuple[int, int]`
|
||||
- Project 3D camera-frame point to pixel coordinates
|
||||
- Implement `compute_depth_residual(P_world, T_world_cam, depth_map, K, window_size=5) -> Optional[float]`
|
||||
- Transform point to camera frame: `P_cam = invert_transform(T_world_cam) @ [P_world, 1]`
|
||||
- Project to pixel, sample depth with median window
|
||||
- Return `z_measured - z_predicted` or None if invalid
|
||||
- Implement `DepthVerificationResult` dataclass:
|
||||
- Fields: `residuals: list[float]`, `rmse: float`, `mean_abs: float`, `median: float`, `depth_normalized_rmse: float`, `n_valid: int`, `n_total: int`
|
||||
- Implement `verify_extrinsics_with_depth(T_world_cam, marker_corners_world, depth_map, K, confidence_map=None, confidence_thresh=50) -> DepthVerificationResult`
|
||||
- For each marker corner, compute residual
|
||||
- Filter by confidence if provided
|
||||
- Compute aggregate metrics
|
||||
|
||||
**Must NOT do**:
|
||||
- Do NOT use ICP or point cloud alignment
|
||||
- Do NOT modify extrinsics (that's Task 3)
|
||||
|
||||
**Recommended Agent Profile**:
|
||||
- **Category**: `unspecified-low`
|
||||
- Reason: Math-focused module, moderate complexity
|
||||
- **Skills**: []
|
||||
|
||||
**Parallelization**:
|
||||
- **Can Run In Parallel**: YES
|
||||
- **Parallel Group**: Wave 1 (with Task 1)
|
||||
- **Blocks**: Tasks 3, 4, 5
|
||||
- **Blocked By**: None
|
||||
|
||||
**References**:
|
||||
- `py_workspace/aruco/pose_math.py` - Transform utilities (invert_transform, etc.)
|
||||
- `py_workspace/aruco/detector.py:62-85` - Camera matrix building pattern
|
||||
- Librarian findings on depth residual computation
|
||||
|
||||
**Acceptance Criteria**:
|
||||
|
||||
**Agent-Executed QA Scenarios:**
|
||||
|
||||
```
|
||||
Scenario: Project point to pixel correctly
|
||||
Tool: Bash (python)
|
||||
Steps:
|
||||
1. python -c "
|
||||
from aruco.depth_verify import project_point_to_pixel
|
||||
import numpy as np
|
||||
K = np.array([[1000, 0, 640], [0, 1000, 360], [0, 0, 1]])
|
||||
P_cam = np.array([0, 0, 1]) # Point at origin, 1m away
|
||||
u, v = project_point_to_pixel(P_cam, K)
|
||||
assert u == 640 and v == 360, f'Got {u}, {v}'
|
||||
print('PASS')
|
||||
"
|
||||
Expected Result: Prints "PASS"
|
||||
|
||||
Scenario: Compute depth residual with perfect match
|
||||
Tool: Bash (python)
|
||||
Steps:
|
||||
1. python -c "
|
||||
from aruco.depth_verify import compute_depth_residual
|
||||
import numpy as np
|
||||
# Identity transform, point at (0, 0, 2m)
|
||||
T = np.eye(4)
|
||||
K = np.array([[1000, 0, 320], [0, 1000, 240], [0, 0, 1]])
|
||||
depth_map = np.full((480, 640), 2.0, dtype=np.float32)
|
||||
P_world = np.array([0, 0, 2])
|
||||
r = compute_depth_residual(P_world, T, depth_map, K, window_size=1)
|
||||
assert abs(r) < 0.001, f'Residual should be ~0, got {r}'
|
||||
print('PASS')
|
||||
"
|
||||
Expected Result: Prints "PASS"
|
||||
|
||||
Scenario: DepthVerificationResult has required fields
|
||||
Tool: Bash (python)
|
||||
Steps:
|
||||
1. python -c "from aruco.depth_verify import DepthVerificationResult; r = DepthVerificationResult(residuals=[], rmse=0, mean_abs=0, median=0, depth_normalized_rmse=0, n_valid=0, n_total=0); print('PASS')"
|
||||
Expected Result: Prints "PASS"
|
||||
```
|
||||
|
||||
**Commit**: YES
|
||||
- Message: `feat(aruco): add depth verification module with residual computation`
|
||||
- Files: `py_workspace/aruco/depth_verify.py`
|
||||
|
||||
---
|
||||
|
||||
- [x] 3. Create depth refinement module
|
||||
|
||||
**What to do**:
|
||||
- Create `py_workspace/aruco/depth_refine.py`
|
||||
- Implement `extrinsics_to_params(T: np.ndarray) -> np.ndarray`
|
||||
- Convert 4x4 matrix to 6-DOF params (rvec + tvec)
|
||||
- Implement `params_to_extrinsics(params: np.ndarray) -> np.ndarray`
|
||||
- Convert 6-DOF params back to 4x4 matrix
|
||||
- Implement `depth_residual_objective(params, marker_corners_world, depth_map, K, initial_params, regularization_weight=0.1) -> float`
|
||||
- Compute sum of squared depth residuals + regularization term
|
||||
- Regularization: penalize deviation from initial_params
|
||||
- Implement `refine_extrinsics_with_depth(T_initial, marker_corners_world, depth_map, K, max_translation_m=0.05, max_rotation_deg=5.0) -> tuple[np.ndarray, dict]`
|
||||
- Use `scipy.optimize.minimize` with method='L-BFGS-B'
|
||||
- Add bounds based on max_translation and max_rotation
|
||||
- Return refined T and stats dict (iterations, final_cost, delta_translation, delta_rotation)
|
||||
|
||||
**Must NOT do**:
|
||||
- Do NOT optimize intrinsics or distortion
|
||||
- Do NOT allow unbounded optimization (must use regularization/bounds)
|
||||
|
||||
**Recommended Agent Profile**:
|
||||
- **Category**: `unspecified-low`
|
||||
- Reason: Optimization with scipy, moderate complexity
|
||||
- **Skills**: []
|
||||
|
||||
**Parallelization**:
|
||||
- **Can Run In Parallel**: YES
|
||||
- **Parallel Group**: Wave 2 (with Task 4)
|
||||
- **Blocks**: Task 6
|
||||
- **Blocked By**: Task 2
|
||||
|
||||
**References**:
|
||||
- `py_workspace/aruco/pose_math.py` - rvec_tvec_to_matrix, matrix_to_rvec_tvec
|
||||
- scipy.optimize.minimize documentation
|
||||
- Librarian findings on direct optimization
|
||||
|
||||
**Acceptance Criteria**:
|
||||
|
||||
**Agent-Executed QA Scenarios:**
|
||||
|
||||
```
|
||||
Scenario: Params round-trip conversion
|
||||
Tool: Bash (python)
|
||||
Steps:
|
||||
1. python -c "
|
||||
from aruco.depth_refine import extrinsics_to_params, params_to_extrinsics
|
||||
from aruco.pose_math import rvec_tvec_to_matrix
|
||||
import numpy as np
|
||||
T = rvec_tvec_to_matrix(np.array([0.1, 0.2, 0.3]), np.array([1, 2, 3]))
|
||||
params = extrinsics_to_params(T)
|
||||
T2 = params_to_extrinsics(params)
|
||||
assert np.allclose(T, T2, atol=1e-9), 'Round-trip failed'
|
||||
print('PASS')
|
||||
"
|
||||
Expected Result: Prints "PASS"
|
||||
|
||||
Scenario: Refinement respects bounds
|
||||
Tool: Bash (python)
|
||||
Steps:
|
||||
1. python -c "
|
||||
from aruco.depth_refine import refine_extrinsics_with_depth
|
||||
import numpy as np
|
||||
# Synthetic test with small perturbation
|
||||
T = np.eye(4)
|
||||
T[0, 3] = 0.01 # 1cm offset
|
||||
corners = np.array([[0, 0, 2], [0.1, 0, 2], [0.1, 0.1, 2], [0, 0.1, 2]])
|
||||
K = np.array([[1000, 0, 320], [0, 1000, 240], [0, 0, 1]])
|
||||
depth = np.full((480, 640), 2.0, dtype=np.float32)
|
||||
T_refined, stats = refine_extrinsics_with_depth(T, corners, depth, K, max_translation_m=0.05)
|
||||
delta = stats['delta_translation_norm_m']
|
||||
assert delta < 0.05, f'Translation moved too far: {delta}'
|
||||
print('PASS')
|
||||
"
|
||||
Expected Result: Prints "PASS"
|
||||
```
|
||||
|
||||
**Commit**: YES
|
||||
- Message: `feat(aruco): add depth refinement module with bounded optimization`
|
||||
- Files: `py_workspace/aruco/depth_refine.py`
|
||||
|
||||
---
|
||||
|
||||
- [x] 4. Add CLI flags to calibrate_extrinsics.py
|
||||
|
||||
**What to do**:
|
||||
- Modify `py_workspace/calibrate_extrinsics.py`
|
||||
- Add new click options:
|
||||
- `--verify-depth / --no-verify-depth` (default: False) - Enable depth verification
|
||||
- `--refine-depth / --no-refine-depth` (default: False) - Enable depth refinement
|
||||
- `--depth-mode` (default: "NEURAL") - Depth computation mode (NEURAL, ULTRA, PERFORMANCE)
|
||||
- `--depth-confidence-threshold` (default: 50) - Confidence threshold for depth filtering
|
||||
- `--report-csv PATH` - Optional path for per-frame CSV report
|
||||
- Update InitParameters when depth flags are set
|
||||
- Pass depth_mode to SVOReader
|
||||
|
||||
**Must NOT do**:
|
||||
- Do NOT change any existing behavior when new flags are not specified
|
||||
- Do NOT remove or modify existing CLI options
|
||||
|
||||
**Recommended Agent Profile**:
|
||||
- **Category**: `quick`
|
||||
- Reason: Adding CLI options, straightforward
|
||||
- **Skills**: []
|
||||
|
||||
**Parallelization**:
|
||||
- **Can Run In Parallel**: YES
|
||||
- **Parallel Group**: Wave 2 (with Task 3)
|
||||
- **Blocks**: Tasks 5, 6
|
||||
- **Blocked By**: Tasks 1, 2
|
||||
|
||||
**References**:
|
||||
- `py_workspace/calibrate_extrinsics.py:22-42` - Existing click options
|
||||
- Click documentation for option syntax
|
||||
|
||||
**Acceptance Criteria**:
|
||||
|
||||
**Agent-Executed QA Scenarios:**
|
||||
|
||||
```
|
||||
Scenario: CLI help shows new flags
|
||||
Tool: Bash
|
||||
Steps:
|
||||
1. cd /workspaces/zed-playground/py_workspace
|
||||
2. uv run calibrate_extrinsics.py --help | grep -E "(verify-depth|refine-depth|depth-mode)"
|
||||
Expected Result: All three flags appear in help output
|
||||
|
||||
Scenario: Default behavior unchanged
|
||||
Tool: Bash (python)
|
||||
Steps:
|
||||
1. python -c "
|
||||
# Parse default values
|
||||
import click
|
||||
from calibrate_extrinsics import main
|
||||
ctx = click.Context(main)
|
||||
params = {p.name: p.default for p in main.params}
|
||||
assert params.get('verify_depth') == False, 'verify_depth should default False'
|
||||
assert params.get('refine_depth') == False, 'refine_depth should default False'
|
||||
print('PASS')
|
||||
"
|
||||
Expected Result: Prints "PASS"
|
||||
```
|
||||
|
||||
**Commit**: YES
|
||||
- Message: `feat(cli): add depth verification and refinement flags`
|
||||
- Files: `py_workspace/calibrate_extrinsics.py`
|
||||
|
||||
---
|
||||
|
||||
- [x] 5. Integrate verification into CLI workflow
|
||||
|
||||
**What to do**:
|
||||
- Modify `py_workspace/calibrate_extrinsics.py`
|
||||
- When `--verify-depth` is set:
|
||||
- After computing extrinsics, run depth verification for each camera
|
||||
- Use detected marker corners (already in image coordinates) + known 3D positions
|
||||
- Sample depth at corner pixel positions using median window
|
||||
- Compute DepthVerificationResult per camera
|
||||
- Add `depth_verify` section to output JSON:
|
||||
```json
|
||||
{
|
||||
"serial": {
|
||||
"pose": "...",
|
||||
"stats": {...},
|
||||
"depth_verify": {
|
||||
"rmse": 0.015,
|
||||
"mean_abs": 0.012,
|
||||
"median": 0.010,
|
||||
"depth_normalized_rmse": 0.008,
|
||||
"n_valid": 45,
|
||||
"n_total": 48
|
||||
}
|
||||
}
|
||||
}
|
||||
```
|
||||
- Print verification summary to console
|
||||
- If `--report-csv` specified, write per-frame residuals
|
||||
|
||||
**Must NOT do**:
|
||||
- Do NOT modify extrinsics (that's Task 6)
|
||||
- Do NOT break existing JSON format for cameras without depth_verify
|
||||
|
||||
**Recommended Agent Profile**:
|
||||
- **Category**: `unspecified-high`
|
||||
- Reason: Integration task, requires careful coordination
|
||||
- **Skills**: []
|
||||
|
||||
**Parallelization**:
|
||||
- **Can Run In Parallel**: NO
|
||||
- **Parallel Group**: Wave 2 (sequential)
|
||||
- **Blocks**: Tasks 6, 7
|
||||
- **Blocked By**: Tasks 1, 2, 4
|
||||
|
||||
**References**:
|
||||
- `py_workspace/calibrate_extrinsics.py:186-212` - Current output generation
|
||||
- `py_workspace/aruco/depth_verify.py` - Verification module (Task 2)
|
||||
|
||||
**Acceptance Criteria**:
|
||||
|
||||
**Agent-Executed QA Scenarios:**
|
||||
|
||||
```
|
||||
Scenario: Verify-depth adds depth_verify to JSON
|
||||
Tool: Bash
|
||||
Preconditions: SVO files and markers exist
|
||||
Steps:
|
||||
1. uv run calibrate_extrinsics.py --svo *.svo2 --markers aruco/output/standard_box_markers.parquet --output /tmp/test_verify.json --verify-depth --no-preview --sample-interval 100
|
||||
2. python -c "import json; d=json.load(open('/tmp/test_verify.json')); k=list(d.keys())[0]; assert 'depth_verify' in d[k], 'Missing depth_verify'; print('PASS')"
|
||||
Expected Result: Prints "PASS"
|
||||
|
||||
Scenario: CSV report generated when flag set
|
||||
Tool: Bash
|
||||
Steps:
|
||||
1. uv run calibrate_extrinsics.py ... --verify-depth --report-csv /tmp/residuals.csv
|
||||
2. python -c "import csv; rows=list(csv.reader(open('/tmp/residuals.csv'))); assert len(rows) > 1; print('PASS')"
|
||||
Expected Result: Prints "PASS"
|
||||
```
|
||||
|
||||
**Commit**: YES
|
||||
- Message: `feat(cli): integrate depth verification into calibration workflow`
|
||||
- Files: `py_workspace/calibrate_extrinsics.py`
|
||||
|
||||
---
|
||||
|
||||
- [ ] 6. Integrate refinement into CLI workflow
|
||||
|
||||
**What to do**:
|
||||
- Modify `py_workspace/calibrate_extrinsics.py`
|
||||
- When `--refine-depth` is set (requires `--verify-depth` implicitly):
|
||||
- After initial extrinsics computation, run depth refinement
|
||||
- Report both pre-refinement and post-refinement metrics
|
||||
- Update the pose in output JSON with refined values
|
||||
- Add `refine_depth` section to output JSON:
|
||||
```json
|
||||
{
|
||||
"serial": {
|
||||
"pose": "...", // Now refined
|
||||
"stats": {...},
|
||||
"depth_verify": {...}, // Pre-refinement
|
||||
"depth_verify_post": {...}, // Post-refinement
|
||||
"refine_depth": {
|
||||
"iterations": 15,
|
||||
"delta_translation_norm_m": 0.008,
|
||||
"delta_rotation_deg": 0.5,
|
||||
"improvement_rmse": 0.003
|
||||
}
|
||||
}
|
||||
}
|
||||
```
|
||||
- Print refinement summary to console
|
||||
|
||||
**Must NOT do**:
|
||||
- Do NOT allow refinement without verification (refine implies verify)
|
||||
- Do NOT remove regularization bounds
|
||||
|
||||
**Recommended Agent Profile**:
|
||||
- **Category**: `unspecified-high`
|
||||
- Reason: Final integration, careful coordination
|
||||
- **Skills**: []
|
||||
|
||||
**Parallelization**:
|
||||
- **Can Run In Parallel**: NO
|
||||
- **Parallel Group**: Wave 3 (final)
|
||||
- **Blocks**: Task 7
|
||||
- **Blocked By**: Tasks 3, 5
|
||||
|
||||
**References**:
|
||||
- `py_workspace/aruco/depth_refine.py` - Refinement module (Task 3)
|
||||
- Task 5 output format
|
||||
|
||||
**Acceptance Criteria**:
|
||||
|
||||
**Agent-Executed QA Scenarios:**
|
||||
|
||||
```
|
||||
Scenario: Refine-depth produces refined extrinsics
|
||||
Tool: Bash
|
||||
Steps:
|
||||
1. uv run calibrate_extrinsics.py --svo *.svo2 --markers aruco/output/standard_box_markers.parquet --output /tmp/test_refine.json --refine-depth --no-preview --sample-interval 100
|
||||
2. python -c "import json; d=json.load(open('/tmp/test_refine.json')); k=list(d.keys())[0]; assert 'refine_depth' in d[k]; assert 'depth_verify_post' in d[k]; print('PASS')"
|
||||
Expected Result: Prints "PASS"
|
||||
|
||||
Scenario: Refine reports improvement metrics
|
||||
Tool: Bash
|
||||
Steps:
|
||||
1. python -c "import json; d=json.load(open('/tmp/test_refine.json')); k=list(d.keys())[0]; r=d[k]['refine_depth']; assert 'delta_translation_norm_m' in r; print('PASS')"
|
||||
Expected Result: Prints "PASS"
|
||||
```
|
||||
|
||||
**Commit**: YES
|
||||
- Message: `feat(cli): integrate depth refinement into calibration workflow`
|
||||
- Files: `py_workspace/calibrate_extrinsics.py`
|
||||
|
||||
---
|
||||
|
||||
- [ ] 7. Add unit tests for depth modules
|
||||
|
||||
**What to do**:
|
||||
- Create `py_workspace/tests/test_depth_verify.py`
|
||||
- Test cases:
|
||||
- `test_project_point_to_pixel` - Verify projection math
|
||||
- `test_compute_depth_residual_perfect` - Zero residual for matching depth
|
||||
- `test_compute_depth_residual_offset` - Correct residual for offset depth
|
||||
- `test_verify_extrinsics_metrics` - Verify RMSE, mean_abs, median computation
|
||||
- `test_invalid_depth_handling` - NaN/Inf depth returns None
|
||||
- Create `py_workspace/tests/test_depth_refine.py`
|
||||
- Test cases:
|
||||
- `test_params_roundtrip` - extrinsics_to_params ↔ params_to_extrinsics
|
||||
- `test_refinement_reduces_error` - Synthetic case where refinement improves fit
|
||||
- `test_refinement_respects_bounds` - Verify max_translation/rotation honored
|
||||
|
||||
**Must NOT do**:
|
||||
- Do NOT require real SVO files for unit tests (use synthetic data)
|
||||
- Do NOT test CLI directly (that's integration testing)
|
||||
|
||||
**Recommended Agent Profile**:
|
||||
- **Category**: `quick`
|
||||
- Reason: Straightforward test implementation
|
||||
- **Skills**: []
|
||||
|
||||
**Parallelization**:
|
||||
- **Can Run In Parallel**: YES
|
||||
- **Parallel Group**: Wave 3 (with Task 6)
|
||||
- **Blocks**: None
|
||||
- **Blocked By**: Tasks 2, 3
|
||||
|
||||
**References**:
|
||||
- `py_workspace/tests/test_pose_math.py` - Existing test patterns
|
||||
- `py_workspace/tests/test_pose_averaging.py` - More test patterns
|
||||
|
||||
**Acceptance Criteria**:
|
||||
|
||||
**Agent-Executed QA Scenarios:**
|
||||
|
||||
```
|
||||
Scenario: All depth unit tests pass
|
||||
Tool: Bash
|
||||
Steps:
|
||||
1. cd /workspaces/zed-playground/py_workspace
|
||||
2. uv run pytest tests/test_depth_verify.py tests/test_depth_refine.py -v
|
||||
Expected Result: Exit code 0, all tests pass
|
||||
|
||||
Scenario: Test count is reasonable
|
||||
Tool: Bash
|
||||
Steps:
|
||||
1. uv run pytest tests/test_depth_*.py --collect-only | grep "test_"
|
||||
Expected Result: At least 8 tests collected
|
||||
```
|
||||
|
||||
**Commit**: YES
|
||||
- Message: `test(aruco): add unit tests for depth verification and refinement`
|
||||
- Files: `py_workspace/tests/test_depth_verify.py`, `py_workspace/tests/test_depth_refine.py`
|
||||
|
||||
---
|
||||
|
||||
## Commit Strategy
|
||||
|
||||
| After Task | Message | Files | Verification |
|
||||
|------------|---------|-------|--------------|
|
||||
| 1 | `feat(aruco): extend SVOReader with depth support` | svo_sync.py | python import test |
|
||||
| 2 | `feat(aruco): add depth verification module` | depth_verify.py | python import test |
|
||||
| 3 | `feat(aruco): add depth refinement module` | depth_refine.py | python import test |
|
||||
| 4 | `feat(cli): add depth flags` | calibrate_extrinsics.py | --help works |
|
||||
| 5 | `feat(cli): integrate depth verification` | calibrate_extrinsics.py | --verify-depth works |
|
||||
| 6 | `feat(cli): integrate depth refinement` | calibrate_extrinsics.py | --refine-depth works |
|
||||
| 7 | `test(aruco): add depth tests` | tests/test_depth_*.py | pytest passes |
|
||||
|
||||
---
|
||||
|
||||
## Success Criteria
|
||||
|
||||
### Verification Commands
|
||||
```bash
|
||||
# CLI shows new flags
|
||||
uv run calibrate_extrinsics.py --help # Expected: shows --verify-depth, --refine-depth
|
||||
|
||||
# Non-regression: without depth flags, behavior unchanged
|
||||
uv run calibrate_extrinsics.py --markers aruco/output/standard_box_markers.parquet --validate-markers # Expected: exit 0
|
||||
|
||||
# Depth verification works
|
||||
uv run calibrate_extrinsics.py --svo *.svo2 --markers aruco/output/standard_box_markers.parquet --output test.json --verify-depth --no-preview
|
||||
|
||||
# Depth refinement works
|
||||
uv run calibrate_extrinsics.py --svo *.svo2 --markers aruco/output/standard_box_markers.parquet --output test.json --refine-depth --no-preview
|
||||
|
||||
# Tests pass
|
||||
uv run pytest tests/test_depth_*.py -v # Expected: all pass
|
||||
```
|
||||
|
||||
### Final Checklist
|
||||
- [ ] All "Must Have" present
|
||||
- [ ] All "Must NOT Have" absent
|
||||
- [ ] All tests pass
|
||||
- [ ] CLI --help shows all new options
|
||||
- [ ] Output JSON includes depth_verify section when flag used
|
||||
- [ ] Output JSON includes refine_depth section when flag used
|
||||
- [ ] Refinement respects bounds (±5cm, ±5°)
|
||||
- [ ] Both pre/post refinement metrics reported
|
||||
Reference in New Issue
Block a user