crosstyan/zed-playground

Fork 0

Files

T

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

2026-02-07 03:20:16 +00:00

14 KiB

Raw Blame History

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

uv run calibrate_extrinsics.py --auto-align ... produces extrinsics with Y-up
--ground-face and --ground-marker-id work as explicit overrides
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

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

2. Add face-to-marker-id mapping

What to do:
- In aruco/alignment.py, add FACE_MARKER_MAP constant:
```
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)

3. Implement ground detection heuristic

What to do:
- In aruco/alignment.py, implement:
```
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)

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

# 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

--auto-align flag works
--ground-face override works
--ground-marker-id override works
Heuristic detection works without explicit face specification
Output extrinsics have Y-up when aligned
No behavior change when --auto-align not specified
All tests pass
Type checks pass

14 KiB Raw Blame History