docs(calibration): document auto-align and refine-depth workflows
This commit is contained in:
@@ -26,7 +26,16 @@
|
||||
## Messaging Consistency
|
||||
|
||||
## Iteration Speed
|
||||
- Processing full SVO files (thousands of frames) is too slow for verifying simple logic changes. The `--max-samples` option addresses this by allowing early exit after a few successful samples.
|
||||
|
||||
## Test Collection Noise
|
||||
|
||||
## Debugging Heuristics
|
||||
|
||||
## Documentation Gaps
|
||||
- Users were unclear on how `--auto-align` made decisions (heuristic vs explicit) and what `--refine-depth` actually did. The new documentation addresses this by explaining the decision flow and the optimization objective function.
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -57,7 +57,17 @@
|
||||
|
||||
## Fast Iteration
|
||||
- Added `--max-samples` CLI option to `calibrate_extrinsics.py` to allow processing a limited number of samples (e.g., 1 or 3) instead of the full SVO.
|
||||
- This significantly speeds up the development loop when testing changes to pose estimation or alignment logic that don't require the full dataset.
|
||||
|
||||
## Test Configuration
|
||||
- Configured `pytest` in `pyproject.toml` to explicitly target the `tests/` directory and ignore `loguru`, `tmp`, and `libs`.
|
||||
|
||||
## Debug Visibility
|
||||
|
||||
## Documentation
|
||||
- Created `docs/calibrate-extrinsics-workflow.md` to document the runtime behavior of the calibration tool, specifically detailing the precedence logic for ground plane alignment and the mathematical basis for depth verification/refinement.
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -51,7 +51,7 @@ Enable `calibrate_extrinsics.py` to detect the ground-facing box face and apply
|
||||
- [x] `uv run calibrate_extrinsics.py --auto-align ...` produces extrinsics with Y-up
|
||||
- [x] `--ground-face` and `--ground-marker-id` work as explicit overrides
|
||||
- [x] Debug logs show which face was detected as ground and alignment applied
|
||||
- [ ] Tests pass, basedpyright shows 0 errors
|
||||
- [x] Tests pass, basedpyright shows 0 errors
|
||||
|
||||
### Must Have
|
||||
- Heuristic ground detection using camera up-vector
|
||||
@@ -155,11 +155,11 @@ Task 5: Add tests and verify
|
||||
- Rodrigues formula: `R = I + sin(θ)K + (1-cos(θ))K²` where K is skew-symmetric of axis
|
||||
|
||||
**Acceptance Criteria**:
|
||||
- [ ] File `aruco/alignment.py` exists
|
||||
- [ ] `compute_face_normal` returns unit vector for valid (4,3) corners
|
||||
- [ ] `rotation_align_vectors([0,0,1], [0,1,0])` produces 90° rotation about X
|
||||
- [ ] `uv run python -c "from aruco.alignment import compute_face_normal, rotation_align_vectors, apply_alignment_to_pose"` → no errors
|
||||
- [ ] `.venv/bin/basedpyright aruco/alignment.py` → 0 errors
|
||||
- [x] File `aruco/alignment.py` exists
|
||||
- [x] `compute_face_normal` returns unit vector for valid (4,3) corners
|
||||
- [x] `rotation_align_vectors([0,0,1], [0,1,0])` produces 90° rotation about X
|
||||
- [x] `uv run python -c "from aruco.alignment import compute_face_normal, rotation_align_vectors, apply_alignment_to_pose"` → no errors
|
||||
- [x] `.venv/bin/basedpyright aruco/alignment.py` → 0 errors
|
||||
|
||||
**Commit**: YES
|
||||
- Message: `feat(aruco): add alignment utilities for ground plane detection`
|
||||
@@ -210,9 +210,9 @@ Task 5: Add tests and verify
|
||||
- Face c: IDs [24-27], normal ≈ [1,0,0]
|
||||
|
||||
**Acceptance Criteria**:
|
||||
- [ ] `FACE_MARKER_MAP` contains mappings for both parquet files
|
||||
- [ ] `get_face_normal_from_geometry("b", geometry)` returns ≈ [0,1,0]
|
||||
- [ ] Returns `None` for unknown face names
|
||||
- [x] `FACE_MARKER_MAP` contains mappings for both parquet files
|
||||
- [x] `get_face_normal_from_geometry("b", geometry)` returns ≈ [0,1,0]
|
||||
- [x] Returns `None` for unknown face names
|
||||
|
||||
**Commit**: YES (group with Task 1)
|
||||
|
||||
@@ -254,9 +254,9 @@ Task 5: Add tests and verify
|
||||
- Dot product alignment: `np.dot(normal, up_vec)` → highest = most aligned
|
||||
|
||||
**Acceptance Criteria**:
|
||||
- [ ] Function returns face with normal most aligned to camera up
|
||||
- [ ] Returns None when no mapped markers are visible
|
||||
- [ ] Debug log shows which faces were considered and scores
|
||||
- [x] Function returns face with normal most aligned to camera up
|
||||
- [x] Returns None when no mapped markers are visible
|
||||
- [x] Debug log shows which faces were considered and scores
|
||||
|
||||
**Commit**: YES (group with Task 1, 2)
|
||||
|
||||
@@ -301,13 +301,13 @@ Task 5: Add tests and verify
|
||||
- `aruco/alignment.py` - New utilities from Tasks 1-3
|
||||
|
||||
**Acceptance Criteria**:
|
||||
- [ ] `--auto-align` flag exists and defaults to False
|
||||
- [ ] `--ground-face` accepts string face names
|
||||
- [ ] `--ground-marker-id` accepts integer marker ID
|
||||
- [ ] When `--auto-align` used, output poses are rotated
|
||||
- [ ] Debug logs show: "Detected ground face: X, normal: [a,b,c], applying alignment"
|
||||
- [ ] `uv run python -m py_compile calibrate_extrinsics.py` → success
|
||||
- [ ] `.venv/bin/basedpyright calibrate_extrinsics.py` → 0 errors
|
||||
- [x] `--auto-align` flag exists and defaults to False
|
||||
- [x] `--ground-face` accepts string face names
|
||||
- [x] `--ground-marker-id` accepts integer marker ID
|
||||
- [x] When `--auto-align` used, output poses are rotated
|
||||
- [x] Debug logs show: "Detected ground face: X, normal: [a,b,c], applying alignment"
|
||||
- [x] `uv run python -m py_compile calibrate_extrinsics.py` → success
|
||||
- [x] `.venv/bin/basedpyright calibrate_extrinsics.py` → 0 errors
|
||||
|
||||
**Commit**: YES
|
||||
- Message: `feat(calibrate): add --auto-align for ground plane detection and Y-up alignment`
|
||||
@@ -315,7 +315,7 @@ Task 5: Add tests and verify
|
||||
|
||||
---
|
||||
|
||||
- [ ] 5. Add tests and verify end-to-end
|
||||
- [x] 5. Add tests and verify end-to-end
|
||||
|
||||
**What to do**:
|
||||
- Create `tests/test_alignment.py`:
|
||||
@@ -341,9 +341,9 @@ Task 5: Add tests and verify
|
||||
- `/workspaces/zed-playground/zed_settings/inside_network.json` - Reference for Y-up verification
|
||||
|
||||
**Acceptance Criteria**:
|
||||
- [ ] `uv run pytest tests/test_alignment.py` → all pass
|
||||
- [ ] `uv run pytest` → all tests pass (including existing)
|
||||
- [ ] Manual verification: aligned poses have Y-axis column ≈ [0,1,0] in rotation
|
||||
- [x] `uv run pytest tests/test_alignment.py` → all pass
|
||||
- [x] `uv run pytest` → all tests pass (including existing)
|
||||
- [x] Manual verification: aligned poses have Y-axis column ≈ [0,1,0] in rotation
|
||||
|
||||
**Commit**: YES
|
||||
- Message: `test(aruco): add alignment module tests`
|
||||
@@ -389,5 +389,5 @@ uv run python -c "import json, numpy as np; d=json.load(open('aligned_extrinsics
|
||||
- [x] Heuristic detection works without explicit face specification
|
||||
- [x] Output extrinsics have Y-up when aligned
|
||||
- [x] No behavior change when `--auto-align` not specified
|
||||
- [ ] All tests pass
|
||||
- [ ] Type checks pass
|
||||
- [x] All tests pass
|
||||
- [x] Type checks pass
|
||||
|
||||
@@ -156,9 +156,9 @@ def apply_depth_verify_refine_postprocess(
|
||||
}
|
||||
|
||||
improvement = verify_res.rmse - verify_res_post.rmse
|
||||
results[str(serial)]["refine_depth"][
|
||||
"improvement_rmse"
|
||||
] = improvement
|
||||
results[str(serial)]["refine_depth"]["improvement_rmse"] = (
|
||||
improvement
|
||||
)
|
||||
|
||||
click.echo(
|
||||
f"Camera {serial} refined: RMSE={verify_res_post.rmse:.3f}m "
|
||||
@@ -563,7 +563,7 @@ def main(
|
||||
if ground_marker_id in ids:
|
||||
target_face = face
|
||||
logger.info(
|
||||
f"Mapped ground-marker-id {ground_marker_id} to face '{face}'"
|
||||
f"Mapped ground-marker-id {ground_marker_id} to face '{face}' (markers={ids})"
|
||||
)
|
||||
break
|
||||
|
||||
@@ -573,7 +573,10 @@ def main(
|
||||
target_face, marker_geometry, face_marker_map=face_marker_map
|
||||
)
|
||||
if ground_normal is not None:
|
||||
logger.info(f"Using explicit ground face '{target_face}'")
|
||||
ids = mapping_to_use.get(target_face, [])
|
||||
logger.info(
|
||||
f"Using explicit ground face '{target_face}' (markers={ids})"
|
||||
)
|
||||
else:
|
||||
# Heuristic detection
|
||||
heuristic_res = detect_ground_face(
|
||||
@@ -581,7 +584,10 @@ def main(
|
||||
)
|
||||
if heuristic_res:
|
||||
target_face, ground_normal = heuristic_res
|
||||
logger.info(f"Heuristically detected ground face '{target_face}'")
|
||||
ids = mapping_to_use.get(target_face, [])
|
||||
logger.info(
|
||||
f"Heuristically detected ground face '{target_face}' (markers={ids})"
|
||||
)
|
||||
|
||||
if ground_normal is not None:
|
||||
R_align = rotation_align_vectors(ground_normal, np.array([0, 1, 0]))
|
||||
|
||||
@@ -0,0 +1,103 @@
|
||||
# Calibrate Extrinsics Workflow
|
||||
|
||||
This document explains the workflow for `calibrate_extrinsics.py`, focusing on ground plane alignment (`--auto-align`) and depth-based refinement (`--verify-depth`, `--refine-depth`).
|
||||
|
||||
## CLI Overview
|
||||
|
||||
The script calibrates camera extrinsics using ArUco markers detected in SVO recordings.
|
||||
|
||||
**Key Options:**
|
||||
- `--svo`: Path to SVO file(s) or directory containing them.
|
||||
- `--markers`: Path to the marker configuration parquet file.
|
||||
- `--auto-align`: Enables automatic ground plane alignment (opt-in).
|
||||
- `--verify-depth`: Enables depth-based verification of computed poses.
|
||||
- `--refine-depth`: Enables optimization of poses using depth data (requires `--verify-depth`).
|
||||
- `--max-samples`: Limits the number of processed samples for fast iteration.
|
||||
- `--debug`: Enables verbose debug logging (default is INFO).
|
||||
|
||||
## Ground Plane Alignment (`--auto-align`)
|
||||
|
||||
When `--auto-align` is enabled, the script attempts to align the global coordinate system such that a specific face of the marker object becomes the ground plane (XZ plane, normal pointing +Y).
|
||||
|
||||
**Prerequisites:**
|
||||
- The marker parquet file MUST contain `name` and `ids` columns defining which markers belong to which face (e.g., "top", "bottom", "front").
|
||||
- If this metadata is missing, alignment is skipped with a warning.
|
||||
|
||||
**Decision Flow:**
|
||||
The script selects the ground face using the following precedence:
|
||||
|
||||
1. **Explicit Face (`--ground-face`)**:
|
||||
- If you provide `--ground-face="bottom"`, the script looks up the markers for "bottom" in the loaded map.
|
||||
- It computes the average normal of those markers and aligns it to the global up vector.
|
||||
|
||||
2. **Marker ID Mapping (`--ground-marker-id`)**:
|
||||
- If you provide `--ground-marker-id=21`, the script finds which face contains marker 21 (e.g., "bottom").
|
||||
- It then proceeds as if `--ground-face="bottom"` was specified.
|
||||
|
||||
3. **Heuristic Detection (Fallback)**:
|
||||
- If neither option is provided, the script analyzes all visible markers.
|
||||
- It computes the normal for every defined face.
|
||||
- It selects the face whose normal is most aligned with the camera's "down" direction (assuming the camera is roughly upright).
|
||||
|
||||
**Logging:**
|
||||
The script logs the selected decision path for debugging:
|
||||
- `Mapped ground-marker-id 21 to face 'bottom' (markers=[21])`
|
||||
- `Using explicit ground face 'bottom' (markers=[21])`
|
||||
- `Heuristically detected ground face 'bottom' (markers=[21])`
|
||||
|
||||
## Depth Verification & Refinement
|
||||
|
||||
This workflow uses the ZED camera's depth map to verify and improve the ArUco-based pose estimation.
|
||||
|
||||
### 1. Verification (`--verify-depth`)
|
||||
- **Input**: The computed extrinsic pose ($T_{world\_from\_cam}$) and the known 3D world coordinates of the marker corners.
|
||||
- **Process**:
|
||||
1. Projects marker corners into the camera frame using the computed pose.
|
||||
2. Samples the ZED depth map at these projected 2D locations (using a 5x5 median filter for robustness).
|
||||
3. Compares the *measured* depth (ZED) with the *computed* depth (distance from camera center to projected corner).
|
||||
- **Output**:
|
||||
- RMSE (Root Mean Square Error) of the depth residuals.
|
||||
- Number of valid points (where depth was available and finite).
|
||||
- Added to JSON output under `depth_verify`.
|
||||
|
||||
### 2. Refinement (`--refine-depth`)
|
||||
- **Trigger**: Runs only if verification is enabled and enough valid depth points (>4) are found.
|
||||
- **Process**:
|
||||
- Uses `scipy.optimize.minimize` (L-BFGS-B) to adjust the 6-DOF pose parameters (rotation vector + translation vector).
|
||||
- **Objective Function**: Minimizes the squared difference between computed depth and measured depth for all visible marker corners.
|
||||
- **Constraints**: Bounded optimization to prevent drifting too far from the initial ArUco pose (default: ±5 degrees, ±5cm).
|
||||
- **Output**:
|
||||
- Refined pose replaces the original pose in the JSON output.
|
||||
- Improvement stats (delta rotation, delta translation, RMSE reduction) added under `refine_depth`.
|
||||
|
||||
## Fast Iteration (`--max-samples`)
|
||||
|
||||
For development or quick checks, processing thousands of frames is unnecessary.
|
||||
- Use `--max-samples N` to stop after `N` valid samples (frames where markers were detected).
|
||||
- Example: `--max-samples 1` will process the first valid frame, run alignment/refinement, save the result, and exit.
|
||||
|
||||
## Example Workflow
|
||||
|
||||
**Full Run with Alignment and Refinement:**
|
||||
```bash
|
||||
uv run calibrate_extrinsics.py \
|
||||
--svo output/recording.svo \
|
||||
--markers aruco/markers/box.parquet \
|
||||
--aruco-dictionary DICT_APRILTAG_36h11 \
|
||||
--auto-align \
|
||||
--ground-marker-id 21 \
|
||||
--verify-depth \
|
||||
--refine-depth \
|
||||
--output output/calibrated.json
|
||||
```
|
||||
|
||||
**Fast Debug Run:**
|
||||
```bash
|
||||
uv run calibrate_extrinsics.py \
|
||||
--svo output/ \
|
||||
--markers aruco/markers/box.parquet \
|
||||
--auto-align \
|
||||
--max-samples 1 \
|
||||
--debug \
|
||||
--no-preview
|
||||
```
|
||||
@@ -31,3 +31,7 @@ dev = [
|
||||
"pytest>=9.0.2",
|
||||
]
|
||||
|
||||
[tool.pytest.ini_options]
|
||||
testpaths = ["tests"]
|
||||
norecursedirs = ["loguru", "tmp", "libs"]
|
||||
|
||||
|
||||
Reference in New Issue
Block a user