feat: add aruco-svo-calibration plan and utils scripts
- Add comprehensive work plan for ArUco-based multi-camera calibration - Add recording_multi.py for multi-camera SVO recording - Add streaming_receiver.py for network streaming - Add svo_playback.py for synchronized playback - Add zed_network_utils.py for camera configuration - Add AGENTS.md with project context
This commit is contained in:
@@ -0,0 +1,6 @@
|
||||
## 2026-02-04 Init
|
||||
- Use ZED rectified LEFT images (VIEW.LEFT). Distortion handled as zero (since images are rectified).
|
||||
- Output `pose` matrices in the same convention as ZED Fusion `FusionConfiguration.pose`:
|
||||
- Semantics: WORLD Pose of camera = T_world_from_cam.
|
||||
- Storage: row-major 4x4, translation in last column.
|
||||
- Coordinate system/units: defined by InitFusionParameters / InitParameters.
|
||||
@@ -0,0 +1,22 @@
|
||||
## 2026-02-04 Init
|
||||
- Baseline note: ZED SDK stub file `py_workspace/libs/pyzed_pkg/pyzed/sl.pyi` has many LSP/type-stub errors pre-existing in repo.
|
||||
- Do not treat as regressions for this plan.
|
||||
|
||||
- ZED Fusion pose semantics confirmed by librarian:
|
||||
- `/usr/local/zed/include/sl/Fusion.hpp` indicates `FusionConfiguration.pose` is "WORLD Pose of camera" in InitFusionParameters coordinate system/units.
|
||||
- `/usr/local/zed/doc/API/html/classsl_1_1Matrix4f.html` indicates Matrix4f is row-major with translation in last column.
|
||||
|
||||
- Local LSP diagnostics not available: `basedpyright-langserver` is configured but not installed. Use `py_compile` + runtime smoke checks instead.
|
||||
- Git commands currently fail due to missing git-lfs (smudge filter). Avoid git-based verification unless git-lfs is installed.
|
||||
|
||||
- `ak.from_parquet` requires `pyarrow` and `pandas` to be installed in the environment, which were missing initially.
|
||||
|
||||
## Task 6: IndexError in draw_detected_markers
|
||||
- **Bug**: `draw_detected_markers` assumed `ids` was always 2D (`ids[i][0]`) and `corners` was always a list of 3D arrays.
|
||||
- **Fix**: Flattened `ids` using `ids.flatten()` to support both (N,) and (N, 1) shapes. Reshaped `corners` and `int_corners` to ensure compatibility with `cv2.polylines` and text placement regardless of whether input is a list or a 3D/4D numpy array.
|
||||
The calibration loop was hanging because SVOReader loops back to frame 0 upon reaching the end, and 'any(frames)' remained true. Fixed by calculating 'max_frames' based on remaining frames at start and bounding the loop.
|
||||
- Fixed pose parsing in self-check: used np.fromstring instead of np.array on the space-separated string.
|
||||
- Guarded max_frames calculation with a safety limit of 10,000 frames and handled cases where total_frames is -1 or 0.
|
||||
- Improved --validate-markers mode to exit cleanly with a message when no SVOs are provided.
|
||||
- Fixed pose string parsing in self-check distance block to use np.fromstring with sep=' '.
|
||||
- Added max_frames guard for unknown total_frames (<= 0) to prevent infinite loops when SVO length cannot be determined.
|
||||
@@ -0,0 +1,54 @@
|
||||
## 2026-02-04 Init
|
||||
- Repo is script-first, but `aruco/` imports work via Python namespace package even without `__init__.py`.
|
||||
- No existing test suite. `pytest` is not installed; will need to be added (likely as dev dependency) before tests can run.
|
||||
- Existing CLI patterns use `click` (e.g., `streaming_receiver.py`).
|
||||
|
||||
## Pose Math Utilities
|
||||
- Created `aruco/pose_math.py` for common SE(3) operations.
|
||||
- Standardized on 4x4 homogeneous matrices for composition and inversion.
|
||||
- Inversion uses the efficient property: [R | t; 0 | 1]^-1 = [R^T | -R^T * t; 0 | 1].
|
||||
- Reprojection error calculation uses `cv2.projectPoints` and mean Euclidean distance.
|
||||
|
||||
- ArUco marker geometry loading and validation logic implemented in `aruco/marker_geometry.py`.
|
||||
- Use `awkward` and `pyarrow` for efficient parquet loading of multi-dimensional arrays (corners).
|
||||
- Reshaping `ak.to_numpy` output is necessary when dealing with nested structures like corners (4x3).
|
||||
|
||||
## SVO Sync
|
||||
- Added `aruco/svo_sync.py` with `SVOReader` and `FrameData` to open multiple SVO2 files, align starts by timestamp, and grab frames.
|
||||
- Verified with real local SVO2 files: able to open, sync, and grab frames for 2 cameras.
|
||||
|
||||
## ArUco Detector
|
||||
- Added `aruco/detector.py` implementing:
|
||||
- ArUcoDetector creation (DICT_4X4_50)
|
||||
- marker detection (BGR or grayscale input)
|
||||
- ZED intrinsics -> K matrix extraction
|
||||
- multi-marker solvePnP pose estimation + reprojection error
|
||||
- Verified pose estimation with synthetic projected points and with a real SVO-opened camera for intrinsics.
|
||||
- Implemented `PoseAccumulator` in `aruco/pose_averaging.py` for robust SE(3) pose averaging.
|
||||
- Added RANSAC-based filtering for rotation and translation consensus.
|
||||
- Implemented quaternion eigen-mean fallback for rotation averaging when `scipy` is unavailable.
|
||||
- Used median for robust translation averaging.
|
||||
|
||||
## Task 6: ArUco Preview Helpers
|
||||
- Implemented `aruco/preview.py` with `draw_detected_markers`, `draw_pose_axes`, and `show_preview`.
|
||||
- Ensured grayscale images are converted to BGR before drawing to support colored overlays.
|
||||
- Used `cv2.drawFrameAxes` for pose visualization.
|
||||
- `show_preview` handles multiple windows based on camera serial numbers.
|
||||
|
||||
- Removed *.parquet from .gitignore to allow versioning of marker geometry data.
|
||||
|
||||
## Unit Testing
|
||||
- Added pytest as a dev dependency.
|
||||
- Implemented synthetic tests for pose math and averaging.
|
||||
- Discovered that OpenCV's `projectPoints` is strict about `tvec` being floating-point; ensured tests use `dtype=np.float64`.
|
||||
- Verified that `PoseAccumulator` correctly filters outliers using RANSAC and computes robust means.
|
||||
|
||||
## Pytest sys.path Pitfall
|
||||
When running pytest via a console script (e.g., `uv run pytest`), the current working directory is not always automatically added to `sys.path`. This can lead to `ModuleNotFoundError` for local packages like `aruco`.
|
||||
**Fix:** Create a `tests/conftest.py` file that explicitly inserts the project root into `sys.path`:
|
||||
```python
|
||||
import sys
|
||||
import os
|
||||
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), \"..\")))
|
||||
```
|
||||
This ensures deterministic import behavior regardless of how pytest is invoked.
|
||||
@@ -0,0 +1,2 @@
|
||||
## 2026-02-04 Init
|
||||
- (empty)
|
||||
@@ -0,0 +1,3 @@
|
||||
## Architectural Decisions
|
||||
- Implemented SVOReader to encapsulate multi-camera management and synchronization logic.
|
||||
- Used rectified sl.VIEW.LEFT for frame retrieval as requested.
|
||||
@@ -0,0 +1,3 @@
|
||||
## SVO Synchronization Patterns
|
||||
- Use sl.Camera.set_svo_position(0) to loop SVOs when END_OF_SVOFILE_REACHED is encountered.
|
||||
- Synchronization can be achieved by comparing timestamps and skipping frames based on FPS.
|
||||
Reference in New Issue
Block a user