From fca04c47c1b19d82c83f60e0d9e5291a44964a7f Mon Sep 17 00:00:00 2001 From: crosstyan Date: Sat, 7 Feb 2026 03:34:28 +0000 Subject: [PATCH] docs(calibration): document auto-align and refine-depth workflows --- .../notepads/ground-plane-alignment/issues.md | 11 +- .../ground-plane-alignment/learnings.md | 12 +- .../.sisyphus/plans/ground-plane-alignment.md | 50 ++++----- py_workspace/calibrate_extrinsics.py | 18 ++- .../docs/calibrate-extrinsics-workflow.md | 103 ++++++++++++++++++ py_workspace/pyproject.toml | 4 + 6 files changed, 165 insertions(+), 33 deletions(-) create mode 100644 py_workspace/docs/calibrate-extrinsics-workflow.md diff --git a/py_workspace/.sisyphus/notepads/ground-plane-alignment/issues.md b/py_workspace/.sisyphus/notepads/ground-plane-alignment/issues.md index 1f2a681..090a819 100644 --- a/py_workspace/.sisyphus/notepads/ground-plane-alignment/issues.md +++ b/py_workspace/.sisyphus/notepads/ground-plane-alignment/issues.md @@ -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. + + + diff --git a/py_workspace/.sisyphus/notepads/ground-plane-alignment/learnings.md b/py_workspace/.sisyphus/notepads/ground-plane-alignment/learnings.md index 8a4f566..17ed0ed 100644 --- a/py_workspace/.sisyphus/notepads/ground-plane-alignment/learnings.md +++ b/py_workspace/.sisyphus/notepads/ground-plane-alignment/learnings.md @@ -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. + + + diff --git a/py_workspace/.sisyphus/plans/ground-plane-alignment.md b/py_workspace/.sisyphus/plans/ground-plane-alignment.md index c0a4dff..3340f43 100644 --- a/py_workspace/.sisyphus/plans/ground-plane-alignment.md +++ b/py_workspace/.sisyphus/plans/ground-plane-alignment.md @@ -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 diff --git a/py_workspace/calibrate_extrinsics.py b/py_workspace/calibrate_extrinsics.py index 3ca27a1..a43dcea 100644 --- a/py_workspace/calibrate_extrinsics.py +++ b/py_workspace/calibrate_extrinsics.py @@ -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])) diff --git a/py_workspace/docs/calibrate-extrinsics-workflow.md b/py_workspace/docs/calibrate-extrinsics-workflow.md new file mode 100644 index 0000000..e3ce6ef --- /dev/null +++ b/py_workspace/docs/calibrate-extrinsics-workflow.md @@ -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 +``` diff --git a/py_workspace/pyproject.toml b/py_workspace/pyproject.toml index 5007164..230ae73 100644 --- a/py_workspace/pyproject.toml +++ b/py_workspace/pyproject.toml @@ -31,3 +31,7 @@ dev = [ "pytest>=9.0.2", ] +[tool.pytest.ini_options] +testpaths = ["tests"] +norecursedirs = ["loguru", "tmp", "libs"] +