From 446c02d42ae505437d6c34aa9e148acb96b34379 Mon Sep 17 00:00:00 2001 From: crosstyan Date: Sat, 7 Feb 2026 03:20:16 +0000 Subject: [PATCH] feat(calibration): add data-driven ground alignment with debug and fast iteration flags --- .../notepads/ground-plane-alignment/issues.md | 32 ++ .../ground-plane-alignment/learnings.md | 63 +++ .../.sisyphus/plans/ground-plane-alignment.md | 393 ++++++++++++++++++ py_workspace/aruco/alignment.py | 237 +++++++++++ py_workspace/aruco/marker_geometry.py | 40 ++ .../standard_box_markers_600mm.parquet | Bin 0 -> 6215 bytes py_workspace/aruco/svo_sync.py | 12 + py_workspace/calibrate_extrinsics.py | 265 ++++++++++-- py_workspace/docs/marker-parquet-format.md | 49 +++ py_workspace/tests/test_alignment.py | 163 ++++++++ 10 files changed, 1221 insertions(+), 33 deletions(-) create mode 100644 py_workspace/.sisyphus/notepads/ground-plane-alignment/issues.md create mode 100644 py_workspace/.sisyphus/notepads/ground-plane-alignment/learnings.md create mode 100644 py_workspace/.sisyphus/plans/ground-plane-alignment.md create mode 100644 py_workspace/aruco/alignment.py create mode 100644 py_workspace/aruco/markers/standard_box_markers_600mm.parquet create mode 100644 py_workspace/docs/marker-parquet-format.md create mode 100644 py_workspace/tests/test_alignment.py diff --git a/py_workspace/.sisyphus/notepads/ground-plane-alignment/issues.md b/py_workspace/.sisyphus/notepads/ground-plane-alignment/issues.md new file mode 100644 index 0000000..1f2a681 --- /dev/null +++ b/py_workspace/.sisyphus/notepads/ground-plane-alignment/issues.md @@ -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. + + + diff --git a/py_workspace/.sisyphus/notepads/ground-plane-alignment/learnings.md b/py_workspace/.sisyphus/notepads/ground-plane-alignment/learnings.md new file mode 100644 index 0000000..8a4f566 --- /dev/null +++ b/py_workspace/.sisyphus/notepads/ground-plane-alignment/learnings.md @@ -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), and `corners` (list>>). +- **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. + + + diff --git a/py_workspace/.sisyphus/plans/ground-plane-alignment.md b/py_workspace/.sisyphus/plans/ground-plane-alignment.md new file mode 100644 index 0000000..c0a4dff --- /dev/null +++ b/py_workspace/.sisyphus/plans/ground-plane-alignment.md @@ -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 diff --git a/py_workspace/aruco/alignment.py b/py_workspace/aruco/alignment.py new file mode 100644 index 0000000..91fc028 --- /dev/null +++ b/py_workspace/aruco/alignment.py @@ -0,0 +1,237 @@ +import numpy as np +from loguru import logger + + +def compute_face_normal(corners: np.ndarray) -> np.ndarray: + """ + Compute the normal vector of a face defined by its corners. + Assumes corners are in order (e.g., clockwise or counter-clockwise). + + Args: + corners: (N, 3) array of corner coordinates. + + Returns: + (3,) normalized normal vector. + """ + if corners.ndim != 2 or corners.shape[1] != 3: + raise ValueError(f"Expected (N, 3) array, got {corners.shape}") + if corners.shape[0] < 3: + raise ValueError("At least 3 corners are required to compute a normal.") + + # Use the cross product of two edges + # Stable edge definition for quad faces consistent with plan/repo convention: + # normal from (corners[1]-corners[0]) x (corners[3]-corners[0]) when N>=4, + # fallback to index 2 if N==3. + v1 = corners[1] - corners[0] + if corners.shape[0] >= 4: + v2 = corners[3] - corners[0] + else: + v2 = corners[2] - corners[0] + + normal = np.cross(v1, v2) + norm = np.linalg.norm(normal) + + if norm < 1e-10: + raise ValueError("Corners are collinear or degenerate; cannot compute normal.") + + return (normal / norm).astype(np.float64) + + +def rotation_align_vectors(from_vec: np.ndarray, to_vec: np.ndarray) -> np.ndarray: + """ + Compute the 3x3 rotation matrix that aligns from_vec to to_vec. + + Args: + from_vec: (3,) source vector. + to_vec: (3,) target vector. + + Returns: + (3, 3) rotation matrix. + """ + if from_vec.shape != (3,) or to_vec.shape != (3,): + raise ValueError( + f"Expected (3,) vectors, got {from_vec.shape} and {to_vec.shape}" + ) + + norm_from = np.linalg.norm(from_vec) + norm_to = np.linalg.norm(to_vec) + + if norm_from < 1e-10 or norm_to < 1e-10: + raise ValueError("Cannot align zero-norm vectors.") + + # Normalize inputs + a = from_vec / norm_from + b = to_vec / norm_to + + v = np.cross(a, b) + c = np.dot(a, b) + s = np.linalg.norm(v) + + # Handle parallel case + if s < 1e-10: + if c > 0: + return np.eye(3, dtype=np.float64) + else: + # Anti-parallel case: 180 degree rotation around an orthogonal axis + # Find an orthogonal axis + if abs(a[0]) < 0.9: + ortho = np.array([1.0, 0.0, 0.0]) + else: + ortho = np.array([0.0, 1.0, 0.0]) + + axis = np.cross(a, ortho) + axis /= np.linalg.norm(axis) + + # Rodrigues formula for 180 degrees + K = np.array( + [ + [0.0, -axis[2], axis[1]], + [axis[2], 0.0, -axis[0]], + [-axis[1], axis[0], 0.0], + ] + ) + return (np.eye(3) + 2 * (K @ K)).astype(np.float64) + + # General case using Rodrigues' rotation formula + # R = I + [v]_x + [v]_x^2 * (1-c)/s^2 + vx = np.array([[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]]) + + R = np.eye(3) + vx + (vx @ vx) * ((1 - c) / (s**2)) + return R.astype(np.float64) + + +def apply_alignment_to_pose(T: np.ndarray, R_align: np.ndarray) -> np.ndarray: + """ + Apply an alignment rotation to a 4x4 pose matrix. + The alignment is applied in the global frame (pre-multiplication of rotation). + + Args: + T: (4, 4) homogeneous transformation matrix. + R_align: (3, 3) alignment rotation matrix. + + Returns: + (4, 4) aligned transformation matrix. + """ + if T.shape != (4, 4): + raise ValueError(f"Expected 4x4 matrix, got {T.shape}") + if R_align.shape != (3, 3): + raise ValueError(f"Expected 3x3 matrix, got {R_align.shape}") + + T_align = np.eye(4, dtype=np.float64) + T_align[:3, :3] = R_align + + return (T_align @ T).astype(np.float64) + + +def get_face_normal_from_geometry( + face_name: str, + marker_geometry: dict[int, np.ndarray], + face_marker_map: dict[str, list[int]] | None = None, +) -> np.ndarray | None: + """ + Compute the average normal vector for a face based on available marker geometry. + + Args: + face_name: Name of the face (key in face_marker_map). + marker_geometry: Dictionary mapping marker IDs to their (4, 3) corner coordinates. + face_marker_map: Dictionary mapping face names to marker IDs. + + Returns: + (3,) normalized average normal vector, or None if no markers are available. + """ + if face_marker_map is None: + return None + + if face_name not in face_marker_map: + return None + + marker_ids = face_marker_map[face_name] + normals = [] + + for mid in marker_ids: + if mid in marker_geometry: + try: + n = compute_face_normal(marker_geometry[mid]) + normals.append(n) + except ValueError: + continue + + if not normals: + return None + + avg_normal = np.mean(normals, axis=0) + norm = np.linalg.norm(avg_normal) + + if norm < 1e-10: + return None + + return (avg_normal / norm).astype(np.float64) + + +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]), + face_marker_map: dict[str, list[int]] | None = None, +) -> tuple[str, np.ndarray] | None: + """ + Detect which face of the object is most likely the ground face. + The ground face is the one whose normal is most aligned with the camera's up vector. + + Args: + visible_marker_ids: Set of marker IDs currently visible. + marker_geometry: Dictionary mapping marker IDs to their (4, 3) corner coordinates. + camera_up_vector: (3,) vector representing the 'up' direction in camera frame. + face_marker_map: Dictionary mapping face names to marker IDs. + + Returns: + Tuple of (best_face_name, best_face_normal) or None if no face is detected. + """ + if not visible_marker_ids: + return None + + if face_marker_map is None: + return None + + if camera_up_vector.shape != (3,): + raise ValueError( + f"Expected (3,) camera_up_vector, got {camera_up_vector.shape}" + ) + + up_norm = np.linalg.norm(camera_up_vector) + if up_norm < 1e-10: + raise ValueError("camera_up_vector cannot be zero-norm.") + + up_vec = camera_up_vector / up_norm + best_face = None + best_normal = None + best_score = -np.inf + + # Iterate faces in mapping + for face_name, face_marker_ids in face_marker_map.items(): + # Consider only faces with any visible marker ID + if not any(mid in visible_marker_ids for mid in face_marker_ids): + continue + + normal = get_face_normal_from_geometry( + face_name, marker_geometry, face_marker_map=face_marker_map + ) + if normal is None: + continue + + # Score by dot(normal, normalized camera_up_vector) + score = np.dot(normal, up_vec) + logger.debug(f"Face '{face_name}' considered. Score: {score:.4f}") + + if score > best_score: + best_score = score + best_face = face_name + best_normal = normal + + if best_face is not None and best_normal is not None: + logger.debug( + f"Best ground face detected: '{best_face}' with score {best_score:.4f}" + ) + return best_face, best_normal + + return None diff --git a/py_workspace/aruco/marker_geometry.py b/py_workspace/aruco/marker_geometry.py index 493604f..0c55b88 100644 --- a/py_workspace/aruco/marker_geometry.py +++ b/py_workspace/aruco/marker_geometry.py @@ -37,6 +37,46 @@ def load_marker_geometry(parquet_path: Union[str, Path]) -> dict[int, np.ndarray return marker_geometry +def load_face_mapping(parquet_path: Union[str, Path]) -> dict[str, list[int]]: + """ + Reads face mapping from a parquet file. + + The parquet file is expected to have 'name' and 'ids' columns. + 'name' should be a string (face name), and 'ids' should be a list of integers. + + Args: + parquet_path: Path to the parquet file. + + Returns: + A dictionary mapping face names (lowercase) to lists of marker IDs. + """ + path = Path(parquet_path) + if not path.exists(): + raise FileNotFoundError(f"Parquet file not found: {path}") + + ops = ak.from_parquet(path) + + # Check if required columns exist + if "name" not in ops.fields or "ids" not in ops.fields: + # Fallback or empty if columns missing (e.g. old format) + return {} + + names = cast(np.ndarray, ak.to_numpy(ops["name"])) + # ids might be a jagged array if different faces have different marker counts + # ak.to_list() converts to python lists which is what we want for the dict values + ids_list = ak.to_list(ops["ids"]) + + face_map: dict[str, list[int]] = {} + for name, m_ids in zip(names, ids_list): + if name and m_ids: + # Normalize name to lowercase + key = str(name).lower() + # Ensure IDs are ints + face_map[key] = [int(mid) for mid in m_ids] + + return face_map + + def validate_marker_geometry(geometry: dict[int, np.ndarray]) -> None: """ Validates the marker geometry dictionary. diff --git a/py_workspace/aruco/markers/standard_box_markers_600mm.parquet b/py_workspace/aruco/markers/standard_box_markers_600mm.parquet new file mode 100644 index 0000000000000000000000000000000000000000..262114e3e9ee43524196fe49e8e5657611ca703b GIT binary patch literal 6215 zcmeGhZEPE7`P`&g+Bt=8wP#QH5t%ilS{8rBU0tPkXE*+8;v{ZkpRa1uyR#iTzOy~w z`7;XwqzS|y0%D@{*E9x*7JeYHY6?Z>V=ztik3l0ox-ktTXefyG*C;9zJkNXnN|OQG zfC1|%J-_e!e1E==uCsL5s-tvCU1e0Kg+-sPZ}1>B(xc?^hF+c%GJE$@6gBdk`OP8w z7d2ELZ1|j_dKq?E-O^cIp1Gb5yqqSi~Mf z|F|8WE3WpcjuN+?wk4i9tx4QV9o>6BaKHc3joWDDs;0;CQ99uGS)*>hQs4LO&;9V) z8x7Y7K62?E>q`w6968;QzUy$b=QGoDvC7vPb=$@DedlVh7z^zh+4#)|NB`7FUH|pf zzebnW8m_aR0|&-|*H0e1PNj^uPr@y3$^&ziJ{-Yw*2F@%0bv(xbbA9 z@@jtMg=?j%B0u}I^+yet>06gwfx(`-KPk;Ua6h2-b zc9la9-}9lE}$ z3EcM@dd0M?=yp)LUc^l+2???85psoWqOdI{fC=7qT>c(Rx4SJtuQd)DCx)p(oz}R& zk20!C>a=>T{=7ziNi*C(ac2b?2O$+HP;^3n(Aa0hDnj~RLVpOYB9^F{A)_}m`qwnW zFK8y-R<(4*92nM8#$hNIRnTrCsM2ywN7!~Nuw+%E|L$)6&A#DhdO(A%b@G<^h-7@r@NBdpY`hP*l@7n%;tnyP(=_`~$ONYZV(ZlNWWO&|S#-~aLX_}sc zfA}1u>2cb!`QJS)G;y;}!!;xkWx3k$F#fF(5fgtS^#``^7TB^#rFmCS*BbV)m>Q+zaHVVPiUJ6?j%Ta87nV%!(03RP7;V|Av4Y%rCGIFm>d z$c8L}Bo^6f(22YSDYC@dgBivq));%(D)=g!^~?svn9W&bz0SN~b=G1wuN-3n>6x@! zibX3m_Bh%Cv^GbHmmF5!S4qXBiU@mLx9w&nuPoT+<$xzx68zyjZ*z2P_pvG79}-a? zRuV)I%toAK-Oi0J|L}IVg^N0i9s9cDz~*IFeDGL zW{RykOjLtPj0qLGVnX6aYFBb*AfD|y2xKaG!5*GvCAJ3nq?%|nkOYk#Yry7SFh zj{C$CFLvzf%4>;>Mr7z4YW-sGk+2pVq=rcEu}3=f4t*^a4X5J1Nvv=Fu2272pNbYX zY9aT8aMz#Bp8sxg^xxr6xK9|j<6m>9EB`C^b{^&sC6KkolMyTQO{){;B$%IIMxx!R zxJ|LcJOg)?dXF%+&Yc7Z0h%t;FqgqE8M*{Qim%B-@FM-8HAj%%%GlJqjm$k>C64zq z^pH(GaqImX0bjx_AvR~6)@%U1O6_^pI89qnUW%*bH}zdq1yrQzqo}3_mBIy0&(t0^ zo5?l;Lna)y=1ExN6zV3UD9)5L&Q-hwvs(G+qPb)?`(|NYMRfQ4Ji-Z*I=T|pgrCZU z_!e%CjGZ7t!}9!R*$TU=gKT@?h3o z#Q7O#UVJ?Cu{M3vUW}J2R8 np.ndarray | None: + if not self.enable_depth: + return None + conf_mat = sl.Mat() + cam.retrieve_measure(conf_mat, sl.MEASURE.CONFIDENCE) + return conf_mat.get_data().copy() + def get_depth_at(self, frame: FrameData, x: int, y: int) -> float | None: if frame.depth_map is None: return None diff --git a/py_workspace/calibrate_extrinsics.py b/py_workspace/calibrate_extrinsics.py index ca3942b..3ca27a1 100644 --- a/py_workspace/calibrate_extrinsics.py +++ b/py_workspace/calibrate_extrinsics.py @@ -7,7 +7,11 @@ import pyzed.sl as sl from pathlib import Path from typing import List, Dict, Any, Optional, Tuple -from aruco.marker_geometry import load_marker_geometry, validate_marker_geometry +from aruco.marker_geometry import ( + load_marker_geometry, + validate_marker_geometry, + load_face_mapping, +) from aruco.svo_sync import SVOReader from aruco.detector import ( create_detector, @@ -20,6 +24,37 @@ from aruco.pose_averaging import PoseAccumulator from aruco.preview import draw_detected_markers, draw_pose_axes, show_preview from aruco.depth_verify import verify_extrinsics_with_depth from aruco.depth_refine import refine_extrinsics_with_depth +from aruco.alignment import ( + get_face_normal_from_geometry, + detect_ground_face, + rotation_align_vectors, + apply_alignment_to_pose, +) +from loguru import logger + +ARUCO_DICT_MAP = { + "DICT_4X4_50": cv2.aruco.DICT_4X4_50, + "DICT_4X4_100": cv2.aruco.DICT_4X4_100, + "DICT_4X4_250": cv2.aruco.DICT_4X4_250, + "DICT_4X4_1000": cv2.aruco.DICT_4X4_1000, + "DICT_5X5_50": cv2.aruco.DICT_5X5_50, + "DICT_5X5_100": cv2.aruco.DICT_5X5_100, + "DICT_5X5_250": cv2.aruco.DICT_5X5_250, + "DICT_5X5_1000": cv2.aruco.DICT_5X5_1000, + "DICT_6X6_50": cv2.aruco.DICT_6X6_50, + "DICT_6X6_100": cv2.aruco.DICT_6X6_100, + "DICT_6X6_250": cv2.aruco.DICT_6X6_250, + "DICT_6X6_1000": cv2.aruco.DICT_6X6_1000, + "DICT_7X7_50": cv2.aruco.DICT_7X7_50, + "DICT_7X7_100": cv2.aruco.DICT_7X7_100, + "DICT_7X7_250": cv2.aruco.DICT_7X7_250, + "DICT_7X7_1000": cv2.aruco.DICT_7X7_1000, + "DICT_ARUCO_ORIGINAL": cv2.aruco.DICT_ARUCO_ORIGINAL, + "DICT_APRILTAG_16h5": cv2.aruco.DICT_APRILTAG_16h5, + "DICT_APRILTAG_25h9": cv2.aruco.DICT_APRILTAG_25h9, + "DICT_APRILTAG_36h10": cv2.aruco.DICT_APRILTAG_36h10, + "DICT_APRILTAG_36h11": cv2.aruco.DICT_APRILTAG_36h11, +} def apply_depth_verify_refine_postprocess( @@ -121,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 " @@ -190,24 +225,73 @@ def apply_depth_verify_refine_postprocess( @click.option( "--report-csv", type=click.Path(), help="Optional path for per-frame CSV report." ) +@click.option( + "--auto-align/--no-auto-align", + default=False, + help="Automatically align ground plane.", +) +@click.option( + "--ground-face", type=str, help="Explicit face name for ground alignment." +) +@click.option( + "--ground-marker-id", type=int, help="Explicit marker ID to define ground face." +) +@click.option( + "--aruco-dictionary", + default="DICT_4X4_50", + type=click.Choice(list(ARUCO_DICT_MAP.keys())), + help="ArUco dictionary to use.", +) +@click.option( + "--min-markers", + default=1, + type=int, + help="Minimum markers required for pose estimation.", +) +@click.option( + "--debug/--no-debug", + default=False, + help="Enable verbose debug logging.", +) +@click.option( + "--max-samples", + default=None, + type=int, + help="Maximum number of samples to process before stopping.", +) def main( - svo, - markers, - output, - sample_interval, - max_reproj_error, - preview, - validate_markers, - self_check, - verify_depth, - refine_depth, - depth_mode, - depth_confidence_threshold, - report_csv, + svo: tuple[str, ...], + markers: str, + output: str, + sample_interval: int, + max_reproj_error: float, + preview: bool, + validate_markers: bool, + self_check: bool, + verify_depth: bool, + refine_depth: bool, + depth_mode: str, + depth_confidence_threshold: int, + report_csv: str | None, + auto_align: bool, + ground_face: str | None, + ground_marker_id: int | None, + aruco_dictionary: str, + min_markers: int, + debug: bool, + max_samples: int | None, ): """ Calibrate camera extrinsics relative to a global coordinate system defined by ArUco markers. """ + # Configure logging level + logger.remove() + logger.add( + lambda msg: click.echo(msg, nl=False), + level="DEBUG" if debug else "INFO", + format="{message}", + ) + depth_mode_map = { "NEURAL": sl.DEPTH_MODE.NEURAL, "ULTRA": sl.DEPTH_MODE.ULTRA, @@ -219,17 +303,30 @@ def main( if not (verify_depth or refine_depth): sl_depth_mode = sl.DEPTH_MODE.NONE - # 1. Load Marker Geometry - try: - marker_geometry = load_marker_geometry(markers) - if validate_markers: - validate_marker_geometry(marker_geometry) - click.echo(f"Loaded {len(marker_geometry)} markers from {markers}") - except Exception as e: - click.echo(f"Error loading markers: {e}", err=True) - raise SystemExit(1) + # Expand SVO paths (files or directories) + expanded_svo = [] + for path_str in svo: + path = Path(path_str) + if path.is_dir(): + click.echo(f"Searching for SVO files in {path}...") + found = sorted( + [ + str(p) + for p in path.iterdir() + if p.is_file() and p.suffix.lower() in (".svo", ".svo2") + ] + ) + if found: + click.echo(f"Found {len(found)} SVO files in {path}") + expanded_svo.extend(found) + else: + click.echo(f"Warning: No .svo/.svo2 files found in {path}", err=True) + elif path.is_file(): + expanded_svo.append(str(path)) + else: + click.echo(f"Warning: Path not found: {path}", err=True) - if not svo: + if not expanded_svo: if validate_markers: click.echo("Marker validation successful. No SVOs provided, exiting.") return @@ -239,8 +336,27 @@ def main( ) raise click.UsageError("Missing option '--svo' / '-s'.") + # 1. Load Marker Geometry + try: + marker_geometry = load_marker_geometry(markers) + if validate_markers: + validate_marker_geometry(marker_geometry) + click.echo(f"Loaded {len(marker_geometry)} markers from {markers}") + + # Load face mapping if available + face_marker_map = load_face_mapping(markers) + if face_marker_map: + click.echo(f"Loaded face mapping for {len(face_marker_map)} faces.") + else: + click.echo("No face mapping found in parquet (missing 'name'/'ids').") + face_marker_map = None + + except Exception as e: + click.echo(f"Error loading markers: {e}", err=True) + raise SystemExit(1) + # 2. Initialize SVO Reader - reader = SVOReader(svo, depth_mode=sl_depth_mode) + reader = SVOReader(expanded_svo, depth_mode=sl_depth_mode) if not reader.cameras: click.echo("No SVO files could be opened.", err=True) return @@ -278,7 +394,10 @@ def main( # Store verification frames for post-process check verification_frames = {} - detector = create_detector() + # Track all visible marker IDs for heuristic ground detection + all_visible_ids = set() + + detector = create_detector(dictionary_id=ARUCO_DICT_MAP[aruco_dictionary]) frame_count = 0 sampled_count = 0 @@ -303,6 +422,14 @@ def main( # Detect markers corners, ids = detect_markers(frame.image, detector) + if ids is not None: + all_visible_ids.update(ids.flatten().tolist()) + logger.debug( + f"Cam {serial}: Detected {len(ids)} markers: {ids.flatten()}" + ) + else: + logger.debug(f"Cam {serial}: No markers detected") + if ids is None: if preview: preview_frames[serial] = frame.image @@ -310,7 +437,7 @@ def main( # Estimate pose (T_cam_from_world) pose_res = estimate_pose_from_detections( - corners, ids, marker_geometry, K, min_markers=4 + corners, ids, marker_geometry, K, min_markers=min_markers ) if pose_res: @@ -333,6 +460,14 @@ def main( accumulators[serial].add_pose( T_world_cam, reproj_err, frame_count ) + logger.debug( + f"Cam {serial}: Pose accepted. Reproj={reproj_err:.3f}, Markers={n_markers}" + ) + + else: + logger.debug( + f"Cam {serial}: Pose rejected. Reproj {reproj_err:.3f} > {max_reproj_error}" + ) if preview: img = draw_detected_markers( @@ -340,8 +475,13 @@ def main( ) img = draw_pose_axes(img, rvec, tvec, K, length=0.2) preview_frames[serial] = img - elif preview: - preview_frames[serial] = frame.image + else: + if ids is not None: + logger.debug( + f"Cam {serial}: Pose estimation failed (insufficient markers < {min_markers} or solver failure)" + ) + elif preview: + preview_frames[serial] = frame.image if preview and preview_frames: key = show_preview(preview_frames) @@ -349,12 +489,15 @@ def main( break sampled_count += 1 + if max_samples is not None and sampled_count >= max_samples: + click.echo(f"\nReached max samples ({max_samples}). Stopping.") + break frame_count += 1 if frame_count % 100 == 0: counts = [len(acc.poses) for acc in accumulators.values()] click.echo( - f"Frame {frame_count}, Detections: {dict(zip(serials, counts))}" + f"Frame {frame_count}, Accepted Poses: {dict(zip(serials, counts))}" ) except KeyboardInterrupt: @@ -398,6 +541,62 @@ def main( report_csv, ) + # 5. Optional Ground Plane Alignment + if auto_align: + click.echo("\nPerforming ground plane alignment...") + target_face = ground_face + + # Use loaded map or skip if None + if face_marker_map is None: + click.echo( + "Warning: No face mapping available (missing 'name'/'ids' in parquet). Skipping alignment.", + err=True, + ) + # Skip alignment logic by ensuring loop below doesn't run and heuristic fails gracefully + mapping_to_use = {} + else: + mapping_to_use = face_marker_map + + if not target_face and ground_marker_id is not None: + # Map marker ID to face + for face, ids in mapping_to_use.items(): + if ground_marker_id in ids: + target_face = face + logger.info( + f"Mapped ground-marker-id {ground_marker_id} to face '{face}'" + ) + break + + ground_normal = None + if target_face: + ground_normal = get_face_normal_from_geometry( + 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}'") + else: + # Heuristic detection + heuristic_res = detect_ground_face( + all_visible_ids, marker_geometry, face_marker_map=face_marker_map + ) + if heuristic_res: + target_face, ground_normal = heuristic_res + logger.info(f"Heuristically detected ground face '{target_face}'") + + if ground_normal is not None: + R_align = rotation_align_vectors(ground_normal, np.array([0, 1, 0])) + logger.info(f"Computed alignment rotation for face '{target_face}'") + + for serial, data in results.items(): + T_mean = np.fromstring(data["pose"], sep=" ").reshape(4, 4) + T_aligned = apply_alignment_to_pose(T_mean, R_align) + data["pose"] = " ".join(f"{x:.6f}" for x in T_aligned.flatten()) + logger.debug(f"Applied alignment to camera {serial}") + else: + click.echo( + "Warning: Could not determine ground normal. Skipping alignment." + ) + # 6. Save to JSON with open(output, "w") as f: json.dump(results, f, indent=4, sort_keys=True) diff --git a/py_workspace/docs/marker-parquet-format.md b/py_workspace/docs/marker-parquet-format.md new file mode 100644 index 0000000..19dafb8 --- /dev/null +++ b/py_workspace/docs/marker-parquet-format.md @@ -0,0 +1,49 @@ +# Marker Parquet Format + +This document describes the expected structure for marker configuration files (e.g., `standard_box_markers_600mm.parquet`). These files define both the physical geometry of markers and their logical grouping into faces. + +## Schema + +The parquet file must contain the following columns: + +| Column | Type | Description | +| :--- | :--- | :--- | +| `name` | `string` | Name of the face (e.g., "bottom", "front"). Used for logical grouping. | +| `ids` | `list` | List of ArUco marker IDs belonging to this face. | +| `corners` | `list>>` | 3D coordinates of marker corners. Shape must be `(N, 4, 3)` where N is the number of markers. | + +## Example Data + +Based on `standard_box_markers_600mm.parquet`: + +| name | ids | corners (approximate structure) | +| :--- | :--- | :--- | +| "bottom" | `[21]` | `[[[-0.225, -0.3, 0.226], [0.225, -0.3, 0.226], [0.225, -0.3, -0.224], [-0.225, -0.3, -0.224]]]` | + +## Loader Behavior + +The system uses two different loading strategies based on this file: + +### 1. Geometry Loader (`load_marker_geometry`) +- **Ignores**: `name` column. +- **Uses**: `ids` and `corners`. +- **Process**: + - Flattens all `ids` and `corners` from all rows. + - Reshapes `corners` to `(-1, 4, 3)`. + - Validates that each marker has exactly 4 corners with 3 coordinates (x, y, z). + - Validates coordinates are finite and within reasonable range (< 100m). +- **Output**: `dict[int, np.ndarray]` mapping Marker ID → (4, 3) corner array. + +### 2. Face Mapping Loader (`load_face_mapping`) +- **Uses**: `name` and `ids`. +- **Process**: + - Reads face names and associated marker IDs. + - Normalizes names to lowercase. +- **Output**: `dict[str, list[int]]` mapping Face Name → List of Marker IDs. +- **Usage**: Used for ground plane alignment (e.g., identifying the "bottom" face). + +## Validation Rules +Runtime validation in `marker_geometry.py` ensures: +- `corners` shape is strictly `(4, 3)` per marker. +- No `NaN` or `Inf` values. +- Coordinates are absolute (meters) and must be < 100m. diff --git a/py_workspace/tests/test_alignment.py b/py_workspace/tests/test_alignment.py new file mode 100644 index 0000000..758fda7 --- /dev/null +++ b/py_workspace/tests/test_alignment.py @@ -0,0 +1,163 @@ +import numpy as np +import pytest +from aruco.alignment import ( + compute_face_normal, + rotation_align_vectors, + apply_alignment_to_pose, + get_face_normal_from_geometry, + detect_ground_face, +) + + +def test_compute_face_normal_valid_quad(): + # Define a quad in the XY plane (normal should be Z) + corners = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0]], dtype=np.float64) + + # v1 = corners[1] - corners[0] = [1, 0, 0] + # v2 = corners[3] - corners[0] = [0, 1, 0] + # normal = [1, 0, 0] x [0, 1, 0] = [0, 0, 1] + + normal = compute_face_normal(corners) + np.testing.assert_allclose(normal, [0, 0, 1], atol=1e-10) + + +def test_compute_face_normal_valid_triangle(): + corners = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=np.float64) + + normal = compute_face_normal(corners) + np.testing.assert_allclose(normal, [0, 0, 1], atol=1e-10) + + +def test_compute_face_normal_degenerate(): + # Collinear points + corners = np.array([[0, 0, 0], [1, 0, 0], [2, 0, 0]], dtype=np.float64) + with pytest.raises(ValueError, match="collinear or degenerate"): + compute_face_normal(corners) + + # Too few points + corners_few = np.array([[0, 0, 0], [1, 0, 0]], dtype=np.float64) + with pytest.raises(ValueError, match="At least 3 corners"): + compute_face_normal(corners_few) + + +def test_rotation_align_vectors_identity(): + v1 = np.array([0, 0, 1], dtype=np.float64) + v2 = np.array([0, 0, 1], dtype=np.float64) + R = rotation_align_vectors(v1, v2) + np.testing.assert_allclose(R, np.eye(3), atol=1e-10) + + +def test_rotation_align_vectors_90_deg(): + v1 = np.array([1, 0, 0], dtype=np.float64) + v2 = np.array([0, 1, 0], dtype=np.float64) + R = rotation_align_vectors(v1, v2) + + # Check that R @ v1 == v2 + np.testing.assert_allclose(R @ v1, v2, atol=1e-10) + # Check orthogonality + np.testing.assert_allclose(R @ R.T, np.eye(3), atol=1e-10) + + +def test_rotation_align_vectors_antiparallel(): + v1 = np.array([0, 0, 1], dtype=np.float64) + v2 = np.array([0, 0, -1], dtype=np.float64) + R = rotation_align_vectors(v1, v2) + + np.testing.assert_allclose(R @ v1, v2, atol=1e-10) + np.testing.assert_allclose(R @ R.T, np.eye(3), atol=1e-10) + + +def test_apply_alignment_to_pose(): + # Identity pose + T = np.eye(4, dtype=np.float64) + # 90 deg rotation around Z + R_align = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]], dtype=np.float64) + + T_aligned = apply_alignment_to_pose(T, R_align) + + expected = np.eye(4) + expected[:3, :3] = R_align + np.testing.assert_allclose(T_aligned, expected, atol=1e-10) + + # Non-identity pose + T_pose = np.eye(4) + T_pose[:3, 3] = [1, 2, 3] + T_aligned_pose = apply_alignment_to_pose(T_pose, R_align) + + # Pre-multiplication: R_align @ T_pose + # Rotation part: R_align @ I = R_align + # Translation part: R_align @ [1, 2, 3] = [-2, 1, 3] + expected_pose = np.eye(4) + expected_pose[:3, :3] = R_align + expected_pose[:3, 3] = R_align @ [1, 2, 3] + + np.testing.assert_allclose(T_aligned_pose, expected_pose, atol=1e-10) + + +def test_get_face_normal_from_geometry(): + face_marker_map = {"top": [1, 2]} + # Marker 1: XY plane (normal Z) + # Marker 2: XY plane (normal Z) + marker_geometry = { + 1: np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0]], dtype=np.float64), + 2: np.array([[2, 2, 0], [3, 2, 0], [3, 3, 0], [2, 3, 0]], dtype=np.float64), + } + + normal = get_face_normal_from_geometry("top", marker_geometry, face_marker_map) + np.testing.assert_allclose(normal, [0, 0, 1], atol=1e-10) + + # Missing face map + assert get_face_normal_from_geometry("top", marker_geometry, None) is None + + # Missing face name + assert ( + get_face_normal_from_geometry("bottom", marker_geometry, face_marker_map) + is None + ) + + # No visible markers for face + assert ( + get_face_normal_from_geometry("top", {3: marker_geometry[1]}, face_marker_map) + is None + ) + + +def test_detect_ground_face(): + face_marker_map = { + "bottom": [1], + "top": [2], + } + # Marker 1: normal [0, -1, 0] (aligned with camera up [0, -1, 0]) + # Marker 2: normal [0, 1, 0] (opposite to camera up) + marker_geometry = { + 1: np.array( + [[0, 0, 0], [1, 0, 0], [1, 0, 1], [0, 0, 1]], dtype=np.float64 + ), # Normal is [0, 1, 0] or [0, -1, 0]? + # v1 = [1, 0, 0], v2 = [0, 0, 1] -> cross = [0, -1, 0] + 2: np.array([[0, 1, 0], [1, 1, 0], [1, 1, -1], [0, 1, -1]], dtype=np.float64), + # v1 = [1, 0, 0], v2 = [0, 0, -1] -> cross = [0, 1, 0] + } + + camera_up = np.array([0, -1, 0], dtype=np.float64) + + # Both visible + res = detect_ground_face({1, 2}, marker_geometry, camera_up, face_marker_map) + assert res is not None + face_name, normal = res + assert face_name == "bottom" + np.testing.assert_allclose(normal, [0, -1, 0], atol=1e-10) + + # Only top visible + res = detect_ground_face({2}, marker_geometry, camera_up, face_marker_map) + assert res is not None + face_name, normal = res + assert face_name == "top" + np.testing.assert_allclose(normal, [0, 1, 0], atol=1e-10) + + # None visible + assert ( + detect_ground_face(set(), marker_geometry, camera_up, face_marker_map) is None + ) + + # Missing map + assert detect_ground_face({1, 2}, marker_geometry, camera_up, None) is None