394 lines
14 KiB
Markdown
394 lines
14 KiB
Markdown
# 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
|