# 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
- [x] 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**:
  - [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`
  - 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**:
  - [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)

---

- [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**:
  - [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)

---

- [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**:
  - [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`
  - Files: `calibrate_extrinsics.py`

---

- [x] 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**:
  - [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`
  - 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
- [x] All tests pass
- [x] Type checks pass