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

2026-02-07 03:20:16 +00:00
parent afc8e9034d
commit 446c02d42a
10 changed files with 1221 additions and 33 deletions
@@ -0,0 +1,32 @@
+- Fixed edge cases in `compute_face_normal` to use stable edge definition for quad faces (corners[1]-corners[0] x corners[3]-corners[0]).
+- Fixed edge cases in compute_face_normal to use stable edge definition for quad faces (corners[1]-corners[0] x corners[3]-corners[0]).
+- Added explicit shape validation and zero-norm guards in rotation_align_vectors.
+- Ensured concrete np.ndarray return types with explicit astype(np.float64) to satisfy type checking.
+
+## Type Checking Warnings
+- `basedpyright` reports numerous warnings, mostly related to `Any` types from `cv2` and `pyzed.sl` bindings which lack full type stubs.
+- Deprecation warnings for `List`, `Dict`, `Tuple` (Python 3.9+) are present but existing style uses them. Kept consistent with existing code.
+- `reportUnknownVariableType` is common due to dynamic nature of OpenCV/ZED returns.
+
+## Parquet Metadata Handling
+- `awkward` library used for parquet reading returns jagged arrays for list columns like `ids`.
+- `ak.to_list()` is necessary to convert these to standard Python lists for dictionary values.
+
+## Backward Compatibility
+- While `FACE_MARKER_MAP` constant remains in `aruco/alignment.py` for potential external consumers, it is no longer used by the CLI tool.
+- Users with old parquet files will now see a warning and no alignment, rather than silent fallback to potentially incorrect hardcoded IDs.
+
+
+
+- None encountered during test implementation. API signatures were consistent with the implementation in `aruco/alignment.py`.
+
+
+## Runtime Errors
+
+## Messaging Consistency
+
+## Iteration Speed
+- Processing full SVO files (thousands of frames) is too slow for verifying simple logic changes. The `--max-samples` option addresses this by allowing early exit after a few successful samples.
+
+
+
@@ -0,0 +1,63 @@
+- Implemented core alignment utilities in aruco/alignment.py.
+- Used Rodrigues' rotation formula for vector alignment with explicit handling for parallel and anti-parallel cases.
+- Implemented `FACE_MARKER_MAP` and `get_face_normal_from_geometry` to support multi-marker face normal averaging.
+- Implemented `detect_ground_face` using dot-product scoring against camera up-vector with `loguru` debug logging.
+- Integrated ground-plane alignment into `calibrate_extrinsics.py` with CLI-toggled heuristic and explicit face/marker selection.
+
+## SVO Directory Expansion
+- Implemented directory expansion for `--svo` argument.
+- Iterates through provided paths, checks if directory, and finds `.svo` and `.svo2` files.
+- Maintains backward compatibility for single file paths.
+- Sorts found files to ensure deterministic processing order.
+
+## ArUco Dictionary Selection
+- Added `--aruco-dictionary` CLI option mapping string names to `cv2.aruco` constants.
+- Defaults to `DICT_4X4_50` but supports all standard dictionaries including AprilTags.
+- Passed to `create_detector` to allow flexibility for different marker sets.
+
+## Minimum Markers Configuration
+- Added `--min-markers` CLI option (default 1).
+- Passed to `estimate_pose_from_detections` to filter out poses with insufficient marker support.
+- Useful for improving robustness or allowing single-marker poses when necessary.
+
+## Logging Improvements
+- Added `loguru` debug logs for:
+  - Number of detected markers per frame.
+  - Pose acceptance/rejection with specific reasons (reprojection error, marker count).
+
+## Dynamic Face Mapping
+- Implemented `load_face_mapping` in `aruco/marker_geometry.py` to read face definitions from parquet metadata.
+- Parquet file must contain `name` (string) and `ids` (list of ints) columns.
+- `calibrate_extrinsics.py` now loads this map at runtime and passes it to alignment functions.
+- `aruco/alignment.py` functions (`get_face_normal_from_geometry`, `detect_ground_face`) now accept an optional `face_marker_map` argument.
+
+## Strict Data-Driven Alignment
+- Removed implicit fallback to `FACE_MARKER_MAP` in `aruco/alignment.py`.
+- `calibrate_extrinsics.py` now explicitly checks for loaded face mapping.
+- If mapping is missing (e.g., old parquet without `name`/`ids`), alignment is skipped with a warning instead of using hardcoded defaults.
+- This enforces the requirement that ground alignment configuration must come from the marker definition file.
+
+
+
+- Alignment tests verify that `rotation_align_vectors` correctly handles identity, 90-degree, and anti-parallel cases.
+- `detect_ground_face` and `get_face_normal_from_geometry` are now data-driven, requiring an explicit `face_marker_map` at runtime.
+- Unit tests use mock geometry to verify normal computation and face selection logic without requiring real SVO/parquet data.
+
+- **Parquet Schema**: The marker configuration parquet file (`standard_box_markers_600mm.parquet`) uses a schema with `name` (string), `ids` (list<int64>), and `corners` (list<list<list<float64>>>).
+- **Dual Loading Strategy**: The system loads this single file in two ways:
+  1. `load_marker_geometry`: Flattens `ids` and `corners` to build a global map of Marker ID -> 3D Corners.
+  2. `load_face_mapping`: Uses `name` and `ids` to group markers by face (e.g., "bottom"), which is critical for ground plane alignment.
+
+## Runtime Stability
+- Fixed `AttributeError: 'FrameData' object has no attribute 'confidence_map'` by explicitly adding it to the dataclass and populating it in `SVOReader`.
+- Added `--debug` flag to control log verbosity, defaulting to cleaner INFO level output.
+
+## Consistency Hardening
+- Removed "using default fallback" messaging from `calibrate_extrinsics.py` to align with the strict data-driven requirement.
+
+## Fast Iteration
+- Added `--max-samples` CLI option to `calibrate_extrinsics.py` to allow processing a limited number of samples (e.g., 1 or 3) instead of the full SVO.
+- This significantly speeds up the development loop when testing changes to pose estimation or alignment logic that don't require the full dataset.
+
+
+