feat(calibration): add data-driven ground alignment with debug and fast iteration flags

2026-02-07 03:20:16 +00:00
parent afc8e9034d
commit 446c02d42a
10 changed files with 1221 additions and 33 deletions
@@ -0,0 +1,32 @@
+- Fixed edge cases in `compute_face_normal` to use stable edge definition for quad faces (corners[1]-corners[0] x corners[3]-corners[0]).
+- Fixed edge cases in compute_face_normal to use stable edge definition for quad faces (corners[1]-corners[0] x corners[3]-corners[0]).
+- Added explicit shape validation and zero-norm guards in rotation_align_vectors.
+- Ensured concrete np.ndarray return types with explicit astype(np.float64) to satisfy type checking.
+
+## Type Checking Warnings
+- `basedpyright` reports numerous warnings, mostly related to `Any` types from `cv2` and `pyzed.sl` bindings which lack full type stubs.
+- Deprecation warnings for `List`, `Dict`, `Tuple` (Python 3.9+) are present but existing style uses them. Kept consistent with existing code.
+- `reportUnknownVariableType` is common due to dynamic nature of OpenCV/ZED returns.
+
+## Parquet Metadata Handling
+- `awkward` library used for parquet reading returns jagged arrays for list columns like `ids`.
+- `ak.to_list()` is necessary to convert these to standard Python lists for dictionary values.
+
+## Backward Compatibility
+- While `FACE_MARKER_MAP` constant remains in `aruco/alignment.py` for potential external consumers, it is no longer used by the CLI tool.
+- Users with old parquet files will now see a warning and no alignment, rather than silent fallback to potentially incorrect hardcoded IDs.
+
+
+
+- None encountered during test implementation. API signatures were consistent with the implementation in `aruco/alignment.py`.
+
+
+## Runtime Errors
+
+## 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.
+
+
+
@@ -0,0 +1,63 @@
+- Implemented core alignment utilities in aruco/alignment.py.
+- Used Rodrigues' rotation formula for vector alignment with explicit handling for parallel and anti-parallel cases.
+- Implemented `FACE_MARKER_MAP` and `get_face_normal_from_geometry` to support multi-marker face normal averaging.
+- Implemented `detect_ground_face` using dot-product scoring against camera up-vector with `loguru` debug logging.
+- Integrated ground-plane alignment into `calibrate_extrinsics.py` with CLI-toggled heuristic and explicit face/marker selection.
+
+## SVO Directory Expansion
+- Implemented directory expansion for `--svo` argument.
+- Iterates through provided paths, checks if directory, and finds `.svo` and `.svo2` files.
+- Maintains backward compatibility for single file paths.
+- Sorts found files to ensure deterministic processing order.
+
+## ArUco Dictionary Selection
+- Added `--aruco-dictionary` CLI option mapping string names to `cv2.aruco` constants.
+- Defaults to `DICT_4X4_50` but supports all standard dictionaries including AprilTags.
+- Passed to `create_detector` to allow flexibility for different marker sets.
+
+## Minimum Markers Configuration
+- Added `--min-markers` CLI option (default 1).
+- Passed to `estimate_pose_from_detections` to filter out poses with insufficient marker support.
+- Useful for improving robustness or allowing single-marker poses when necessary.
+
+## Logging Improvements
+- Added `loguru` debug logs for:
+  - Number of detected markers per frame.
+  - Pose acceptance/rejection with specific reasons (reprojection error, marker count).
+
+## Dynamic Face Mapping
+- Implemented `load_face_mapping` in `aruco/marker_geometry.py` to read face definitions from parquet metadata.
+- Parquet file must contain `name` (string) and `ids` (list of ints) columns.
+- `calibrate_extrinsics.py` now loads this map at runtime and passes it to alignment functions.
+- `aruco/alignment.py` functions (`get_face_normal_from_geometry`, `detect_ground_face`) now accept an optional `face_marker_map` argument.
+
+## Strict Data-Driven Alignment
+- Removed implicit fallback to `FACE_MARKER_MAP` in `aruco/alignment.py`.
+- `calibrate_extrinsics.py` now explicitly checks for loaded face mapping.
+- If mapping is missing (e.g., old parquet without `name`/`ids`), alignment is skipped with a warning instead of using hardcoded defaults.
+- This enforces the requirement that ground alignment configuration must come from the marker definition file.
+
+
+
+- Alignment tests verify that `rotation_align_vectors` correctly handles identity, 90-degree, and anti-parallel cases.
+- `detect_ground_face` and `get_face_normal_from_geometry` are now data-driven, requiring an explicit `face_marker_map` at runtime.
+- Unit tests use mock geometry to verify normal computation and face selection logic without requiring real SVO/parquet data.
+
+- **Parquet Schema**: The marker configuration parquet file (`standard_box_markers_600mm.parquet`) uses a schema with `name` (string), `ids` (list<int64>), and `corners` (list<list<list<float64>>>).
+- **Dual Loading Strategy**: The system loads this single file in two ways:
+  1. `load_marker_geometry`: Flattens `ids` and `corners` to build a global map of Marker ID -> 3D Corners.
+  2. `load_face_mapping`: Uses `name` and `ids` to group markers by face (e.g., "bottom"), which is critical for ground plane alignment.
+
+## Runtime Stability
+- Fixed `AttributeError: 'FrameData' object has no attribute 'confidence_map'` by explicitly adding it to the dataclass and populating it in `SVOReader`.
+- Added `--debug` flag to control log verbosity, defaulting to cleaner INFO level output.
+
+## Consistency Hardening
+- Removed "using default fallback" messaging from `calibrate_extrinsics.py` to align with the strict data-driven requirement.
+
+## 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.
+
+
+
@@ -0,0 +1,393 @@
+# Ground Plane Detection and Auto-Alignment
+
+## TL;DR
+
+> **Quick Summary**: Add ground plane detection and optional world-frame alignment to `calibrate_extrinsics.py` so the output coordinate system always has Y-up, regardless of how the calibration box is placed.
+> 
+> **Deliverables**:
+> - New `aruco/alignment.py` module with ground detection and alignment utilities
+> - CLI options: `--auto-align`, `--ground-face`, `--ground-marker-id`
+> - Face metadata in marker parquet files (or hardcoded mapping)
+> - Debug logs for alignment decisions
+> 
+> **Estimated Effort**: Medium
+> **Parallel Execution**: NO - sequential (dependencies between tasks)
+> **Critical Path**: Task 1 → Task 2 → Task 3 → Task 4 → Task 5
+
+---
+
+## Context
+
+### Original Request
+User wants to detect which side of the calibration box is on the ground and auto-align the world frame so Y is always up, matching the ZED convention seen in `inside_network.json`.
+
+### Interview Summary
+**Key Discussions**:
+- Ground detection: support both heuristic (camera up-vector) AND user-specified (face name or marker ID)
+- Alignment: opt-in via `--auto-align` flag (default OFF)
+- Y-up convention confirmed from reference calibration
+
+**Research Findings**:
+- `inside_network.json` shows Y-up convention (cameras at Y ≈ -1.2m)
+- Camera 41831756 has identity rotation → its axes match world axes
+- Marker parquet contains face names and corner coordinates
+- Face normals can be computed from corners: `cross(c1-c0, c3-c0)`
+- `object_points.parquet`: 3 faces (a, b, c) with 4 markers each
+- `standard_box_markers.parquet`: 6 faces with 1 marker each (21=bottom)
+
+---
+
+## Work Objectives
+
+### Core Objective
+Enable `calibrate_extrinsics.py` to detect the ground-facing box face and apply a corrective rotation so the output world frame has Y pointing up.
+
+### Concrete Deliverables
+- `aruco/alignment.py`: Ground detection and alignment utilities
+- Updated `calibrate_extrinsics.py` with new CLI options
+- Updated marker parquet files with face metadata (optional enhancement)
+
+### Definition of Done
+- [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
+
+### Must Have
+- Heuristic ground detection using camera up-vector
+- User override via `--ground-face` or `--ground-marker-id`
+- Alignment rotation applied to all camera poses
+- Debug logging for alignment decisions
+
+### Must NOT Have (Guardrails)
+- Do NOT modify marker parquet file format (use code-level face mapping for now)
+- Do NOT change behavior when `--auto-align` is not specified
+- Do NOT assume IMU/gravity data is available
+- Do NOT break existing calibration workflow
+
+---
+
+## Verification Strategy
+
+> **UNIVERSAL RULE: ZERO HUMAN INTERVENTION**
+> All tasks verifiable by agent using tools.
+
+### Test Decision
+- **Infrastructure exists**: YES (pytest)
+- **Automated tests**: YES (tests-after)
+- **Framework**: pytest
+
+### Agent-Executed QA Scenarios (MANDATORY)
+
+**Scenario: Auto-align with heuristic detection**
+```
+Tool: Bash
+Steps:
+  1. uv run calibrate_extrinsics.py --svo output --markers aruco/markers/object_points.parquet --aruco-dictionary DICT_APRILTAG_36h11 --auto-align --no-preview --sample-interval 100
+  2. Parse output JSON
+  3. Assert: All camera poses have rotation matrices where Y-axis column ≈ [0, 1, 0] (within tolerance)
+Expected Result: Extrinsics aligned to Y-up
+```
+
+**Scenario: Explicit ground face override**
+```
+Tool: Bash
+Steps:
+  1. uv run calibrate_extrinsics.py --svo output --markers aruco/markers/object_points.parquet --aruco-dictionary DICT_APRILTAG_36h11 --auto-align --ground-face b --no-preview --sample-interval 100
+  2. Check debug logs mention "using specified ground face: b"
+Expected Result: Uses face 'b' as ground regardless of heuristic
+```
+
+**Scenario: No alignment when flag omitted**
+```
+Tool: Bash
+Steps:
+  1. uv run calibrate_extrinsics.py --svo output --markers aruco/markers/object_points.parquet --aruco-dictionary DICT_APRILTAG_36h11 --no-preview --sample-interval 100
+  2. Compare output to previous run without --auto-align
+Expected Result: Output unchanged from current behavior
+```
+
+---
+
+## Execution Strategy
+
+### Dependency Chain
+```
+Task 1: Create alignment module
+    ↓
+Task 2: Add face-to-normal mapping
+    ↓
+Task 3: Implement ground detection heuristic
+    ↓
+Task 4: Add CLI options and integrate
+    ↓
+Task 5: Add tests and verify
+```
+
+---
+
+## TODOs
+
+- [x] 1. Create `aruco/alignment.py` module with core utilities
+
+  **What to do**:
+  - Create new file `aruco/alignment.py`
+  - Implement `compute_face_normal(corners: np.ndarray) -> np.ndarray`: compute unit normal from (4,3) corners
+  - Implement `rotation_align_vectors(from_vec: np.ndarray, to_vec: np.ndarray) -> np.ndarray`: compute 3x3 rotation matrix that aligns `from_vec` to `to_vec` using Rodrigues formula
+  - Implement `apply_alignment_to_pose(T: np.ndarray, R_align: np.ndarray) -> np.ndarray`: apply alignment rotation to 4x4 pose matrix
+  - Add type hints and docstrings
+
+  **Must NOT do**:
+  - Do not add CLI logic here (that's Task 4)
+  - Do not hardcode face mappings here (that's Task 2)
+
+  **Recommended Agent Profile**:
+  - **Category**: `quick`
+  - **Skills**: [`git-master`]
+
+  **Parallelization**:
+  - **Can Run In Parallel**: NO
+  - **Blocks**: Task 2, 3, 4
+
+  **References**:
+  - `aruco/pose_math.py` - Similar matrix utilities (rvec_tvec_to_matrix, invert_transform)
+  - `aruco/marker_geometry.py` - Pattern for utility modules
+  - 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
+
+  **Commit**: YES
+  - Message: `feat(aruco): add alignment utilities for ground plane detection`
+  - Files: `aruco/alignment.py`
+
+---
+
+- [x] 2. Add face-to-marker-id mapping
+
+  **What to do**:
+  - In `aruco/alignment.py`, add `FACE_MARKER_MAP` constant:
+    ```python
+    FACE_MARKER_MAP: dict[str, list[int]] = {
+        # object_points.parquet
+        "a": [16, 17, 18, 19],
+        "b": [20, 21, 22, 23],
+        "c": [24, 25, 26, 27],
+        # standard_box_markers.parquet
+        "bottom": [21],
+        "top": [23],
+        "front": [24],
+        "back": [22],
+        "left": [25],
+        "right": [26],
+    }
+    ```
+  - Implement `get_face_normal_from_geometry(face_name: str, marker_geometry: dict[int, np.ndarray]) -> np.ndarray | None`:
+    - Look up marker IDs for face
+    - Get corners from geometry
+    - Compute and return average normal across markers in that face
+
+  **Must NOT do**:
+  - Do not modify parquet files
+
+  **Recommended Agent Profile**:
+  - **Category**: `quick`
+  - **Skills**: [`git-master`]
+
+  **Parallelization**:
+  - **Can Run In Parallel**: NO
+  - **Blocked By**: Task 1
+  - **Blocks**: Task 3, 4
+
+  **References**:
+  - Bash output from parquet inspection (earlier in conversation):
+    - Face a: IDs [16-19], normal ≈ [0,0,1]
+    - Face b: IDs [20-23], normal ≈ [0,1,0]
+    - 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
+
+  **Commit**: YES (group with Task 1)
+
+---
+
+- [x] 3. Implement ground detection heuristic
+
+  **What to do**:
+  - In `aruco/alignment.py`, implement:
+    ```python
+    def detect_ground_face(
+        visible_marker_ids: set[int],
+        marker_geometry: dict[int, np.ndarray],
+        camera_up_vector: np.ndarray = np.array([0, -1, 0]),  # -Y in camera frame
+    ) -> tuple[str, np.ndarray] | None:
+    ```
+  - Logic:
+    1. For each face in `FACE_MARKER_MAP`:
+       - Check if any of its markers are in `visible_marker_ids`
+       - If yes, compute face normal from geometry
+    2. Find the face whose normal most closely aligns with `camera_up_vector` (highest dot product)
+    3. Return (face_name, face_normal) or None if no faces visible
+  - Add debug logging with loguru
+
+  **Must NOT do**:
+  - Do not transform normals by camera pose here (that's done in caller)
+
+  **Recommended Agent Profile**:
+  - **Category**: `unspecified-low`
+  - **Skills**: [`git-master`]
+
+  **Parallelization**:
+  - **Can Run In Parallel**: NO
+  - **Blocked By**: Task 2
+  - **Blocks**: Task 4
+
+  **References**:
+  - `calibrate_extrinsics.py:385` - Where marker IDs are detected
+  - 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
+
+  **Commit**: YES (group with Task 1, 2)
+
+---
+
+- [x] 4. Integrate into `calibrate_extrinsics.py`
+
+  **What to do**:
+  - Add CLI options:
+    - `--auto-align/--no-auto-align` (default: False)
+    - `--ground-face` (optional string, e.g., "b", "bottom")
+    - `--ground-marker-id` (optional int)
+  - Add imports from `aruco.alignment`
+  - After computing all camera poses (after the main loop, before saving):
+    1. If `--auto-align` is False, skip alignment
+    2. Determine ground face:
+       - If `--ground-face` specified: use it directly
+       - If `--ground-marker-id` specified: find which face contains that ID
+       - Else: use heuristic `detect_ground_face()` with visible markers from first camera
+    3. Get ground face normal from geometry
+    4. Compute `R_align = rotation_align_vectors(ground_normal, [0, 1, 0])`
+    5. Apply to all camera poses: `T_aligned = R_align @ T`
+    6. Log alignment info
+  - Update results dict with aligned poses
+
+  **Must NOT do**:
+  - Do not change behavior when `--auto-align` is not specified
+  - Do not modify per-frame pose computation (only post-process)
+
+  **Recommended Agent Profile**:
+  - **Category**: `unspecified-high`
+  - **Skills**: [`git-master`]
+
+  **Parallelization**:
+  - **Can Run In Parallel**: NO
+  - **Blocked By**: Task 3
+  - **Blocks**: Task 5
+
+  **References**:
+  - `calibrate_extrinsics.py:456-477` - Where final poses are computed and stored
+  - `calibrate_extrinsics.py:266-271` - Existing CLI option pattern
+  - `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
+
+  **Commit**: YES
+  - Message: `feat(calibrate): add --auto-align for ground plane detection and Y-up alignment`
+  - Files: `calibrate_extrinsics.py`
+
+---
+
+- [ ] 5. Add tests and verify end-to-end
+
+  **What to do**:
+  - Create `tests/test_alignment.py`:
+    - Test `compute_face_normal` with known corners
+    - Test `rotation_align_vectors` with various axis pairs
+    - Test `detect_ground_face` with mock marker data
+  - Run full calibration with `--auto-align` and verify output
+  - Compare aligned output to reference `inside_network.json` Y-up convention
+
+  **Must NOT do**:
+  - Do not require actual SVO files for unit tests (mock data)
+
+  **Recommended Agent Profile**:
+  - **Category**: `quick`
+  - **Skills**: [`git-master`]
+
+  **Parallelization**:
+  - **Can Run In Parallel**: NO
+  - **Blocked By**: Task 4
+
+  **References**:
+  - `tests/test_depth_cli_postprocess.py` - Existing test pattern
+  - `/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
+
+  **Commit**: YES
+  - Message: `test(aruco): add alignment module tests`
+  - Files: `tests/test_alignment.py`
+
+---
+
+## Commit Strategy
+
+| After Task | Message | Files | Verification |
+|------------|---------|-------|--------------|
+| 1, 2, 3 | `feat(aruco): add alignment utilities for ground plane detection` | `aruco/alignment.py` | `uv run python -c "from aruco.alignment import *"` |
+| 4 | `feat(calibrate): add --auto-align for ground plane detection and Y-up alignment` | `calibrate_extrinsics.py` | `uv run python -m py_compile calibrate_extrinsics.py` |
+| 5 | `test(aruco): add alignment module tests` | `tests/test_alignment.py` | `uv run pytest tests/test_alignment.py` |
+
+---
+
+## Success Criteria
+
+### Verification Commands
+```bash
+# Compile check
+uv run python -m py_compile calibrate_extrinsics.py
+
+# Type check
+.venv/bin/basedpyright aruco/alignment.py calibrate_extrinsics.py
+
+# Unit tests
+uv run pytest tests/test_alignment.py
+
+# Integration test (requires SVO files)
+uv run calibrate_extrinsics.py --svo output --markers aruco/markers/object_points.parquet --aruco-dictionary DICT_APRILTAG_36h11 --auto-align --no-preview --sample-interval 100 --output aligned_extrinsics.json
+
+# Verify Y-up in output
+uv run python -c "import json, numpy as np; d=json.load(open('aligned_extrinsics.json')); T=np.fromstring(list(d.values())[0]['pose'], sep=' ').reshape(4,4); print('Y-axis:', T[:3,1])"
+# Expected: Y-axis ≈ [0, 1, 0]
+```
+
+### Final Checklist
+- [x] `--auto-align` flag works
+- [x] `--ground-face` override works
+- [x] `--ground-marker-id` override works
+- [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