diff --git a/py_workspace/.beads/.gitignore b/py_workspace/.beads/.gitignore new file mode 100644 index 0000000..22f7963 --- /dev/null +++ b/py_workspace/.beads/.gitignore @@ -0,0 +1,45 @@ +# SQLite databases +*.db +*.db?* +*.db-journal +*.db-wal +*.db-shm + +# Daemon runtime files +daemon.lock +daemon.log +daemon.pid +bd.sock +sync-state.json +last-touched + +# Local version tracking (prevents upgrade notification spam after git ops) +.local_version + +# Legacy database files +db.sqlite +bd.db + +# Worktree redirect file (contains relative path to main repo's .beads/) +# Must not be committed as paths would be wrong in other clones +redirect + +# Merge artifacts (temporary files from 3-way merge) +beads.base.jsonl +beads.base.meta.json +beads.left.jsonl +beads.left.meta.json +beads.right.jsonl +beads.right.meta.json + +# Sync state (local-only, per-machine) +# These files are machine-specific and should not be shared across clones +.sync.lock +sync_base.jsonl +export-state/ + +# NOTE: Do NOT add negation patterns (e.g., !issues.jsonl) here. +# They would override fork protection in .git/info/exclude, allowing +# contributors to accidentally commit upstream issue databases. +# The JSONL files (issues.jsonl, interactions.jsonl) and config files +# are tracked by git by default since no pattern above ignores them. diff --git a/py_workspace/.beads/README.md b/py_workspace/.beads/README.md new file mode 100644 index 0000000..50f281f --- /dev/null +++ b/py_workspace/.beads/README.md @@ -0,0 +1,81 @@ +# Beads - AI-Native Issue Tracking + +Welcome to Beads! This repository uses **Beads** for issue tracking - a modern, AI-native tool designed to live directly in your codebase alongside your code. + +## What is Beads? + +Beads is issue tracking that lives in your repo, making it perfect for AI coding agents and developers who want their issues close to their code. No web UI required - everything works through the CLI and integrates seamlessly with git. + +**Learn more:** [github.com/steveyegge/beads](https://github.com/steveyegge/beads) + +## Quick Start + +### Essential Commands + +```bash +# Create new issues +bd create "Add user authentication" + +# View all issues +bd list + +# View issue details +bd show + +# Update issue status +bd update --status in_progress +bd update --status done + +# Sync with git remote +bd sync +``` + +### Working with Issues + +Issues in Beads are: +- **Git-native**: Stored in `.beads/issues.jsonl` and synced like code +- **AI-friendly**: CLI-first design works perfectly with AI coding agents +- **Branch-aware**: Issues can follow your branch workflow +- **Always in sync**: Auto-syncs with your commits + +## Why Beads? + +✨ **AI-Native Design** +- Built specifically for AI-assisted development workflows +- CLI-first interface works seamlessly with AI coding agents +- No context switching to web UIs + +🚀 **Developer Focused** +- Issues live in your repo, right next to your code +- Works offline, syncs when you push +- Fast, lightweight, and stays out of your way + +🔧 **Git Integration** +- Automatic sync with git commits +- Branch-aware issue tracking +- Intelligent JSONL merge resolution + +## Get Started with Beads + +Try Beads in your own projects: + +```bash +# Install Beads +curl -sSL https://raw.githubusercontent.com/steveyegge/beads/main/scripts/install.sh | bash + +# Initialize in your repo +bd init + +# Create your first issue +bd create "Try out Beads" +``` + +## Learn More + +- **Documentation**: [github.com/steveyegge/beads/docs](https://github.com/steveyegge/beads/tree/main/docs) +- **Quick Start Guide**: Run `bd quickstart` +- **Examples**: [github.com/steveyegge/beads/examples](https://github.com/steveyegge/beads/tree/main/examples) + +--- + +*Beads: Issue tracking that moves at the speed of thought* ⚡ diff --git a/py_workspace/.beads/config.yaml b/py_workspace/.beads/config.yaml new file mode 100644 index 0000000..ff8bc92 --- /dev/null +++ b/py_workspace/.beads/config.yaml @@ -0,0 +1,67 @@ +# Beads Configuration File +# This file configures default behavior for all bd commands in this repository +# All settings can also be set via environment variables (BD_* prefix) +# or overridden with command-line flags + +# Issue prefix for this repository (used by bd init) +# If not set, bd init will auto-detect from directory name +# Example: issue-prefix: "myproject" creates issues like "myproject-1", "myproject-2", etc. +# issue-prefix: "" + +# Use no-db mode: load from JSONL, no SQLite, write back after each command +# When true, bd will use .beads/issues.jsonl as the source of truth +# instead of SQLite database +# no-db: false + +# Disable daemon for RPC communication (forces direct database access) +# no-daemon: false + +# Disable auto-flush of database to JSONL after mutations +# no-auto-flush: false + +# Disable auto-import from JSONL when it's newer than database +# no-auto-import: false + +# Enable JSON output by default +# json: false + +# Default actor for audit trails (overridden by BD_ACTOR or --actor) +# actor: "" + +# Path to database (overridden by BEADS_DB or --db) +# db: "" + +# Auto-start daemon if not running (can also use BEADS_AUTO_START_DAEMON) +# auto-start-daemon: true + +# Debounce interval for auto-flush (can also use BEADS_FLUSH_DEBOUNCE) +# flush-debounce: "5s" + +# Export events (audit trail) to .beads/events.jsonl on each flush/sync +# When enabled, new events are appended incrementally using a high-water mark. +# Use 'bd export --events' to trigger manually regardless of this setting. +# events-export: false + +# Git branch for beads commits (bd sync will commit to this branch) +# IMPORTANT: Set this for team projects so all clones use the same sync branch. +# This setting persists across clones (unlike database config which is gitignored). +# Can also use BEADS_SYNC_BRANCH env var for local override. +# If not set, bd sync will require you to run 'bd config set sync.branch '. +# sync-branch: "beads-sync" + +# Multi-repo configuration (experimental - bd-307) +# Allows hydrating from multiple repositories and routing writes to the correct JSONL +# repos: +# primary: "." # Primary repo (where this database lives) +# additional: # Additional repos to hydrate from (read-only) +# - ~/beads-planning # Personal planning repo +# - ~/work-planning # Work planning repo + +# Integration settings (access with 'bd config get/set') +# These are stored in the database, not in this file: +# - jira.url +# - jira.project +# - linear.url +# - linear.api-key +# - github.org +# - github.repo diff --git a/py_workspace/.beads/interactions.jsonl b/py_workspace/.beads/interactions.jsonl new file mode 100644 index 0000000..e69de29 diff --git a/py_workspace/.beads/issues.jsonl b/py_workspace/.beads/issues.jsonl new file mode 100644 index 0000000..b0308b2 --- /dev/null +++ b/py_workspace/.beads/issues.jsonl @@ -0,0 +1,7 @@ +{"id":"py_workspace-6sg","title":"Document marker parquet structure","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T02:48:08.95742431Z","created_by":"crosstyan","updated_at":"2026-02-07T02:49:35.897152691Z","closed_at":"2026-02-07T02:49:35.897152691Z","close_reason":"Documented parquet structure in aruco/markers/PARQUET_FORMAT.md"} +{"id":"py_workspace-a85","title":"Add CLI option for ArUco dictionary in calibrate_extrinsics.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-06T10:13:41.896728814Z","created_by":"crosstyan","updated_at":"2026-02-06T10:14:44.083065399Z","closed_at":"2026-02-06T10:14:44.083065399Z","close_reason":"Added CLI option for selectable ArUco dictionary including AprilTag aliases"} +{"id":"py_workspace-cg9","title":"Implement core alignment utilities (Task 1)","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-06T10:40:36.296030875Z","created_by":"crosstyan","updated_at":"2026-02-06T10:40:46.196825039Z","closed_at":"2026-02-06T10:40:46.196825039Z","close_reason":"Implemented compute_face_normal, rotation_align_vectors, and apply_alignment_to_pose in aruco/alignment.py"} +{"id":"py_workspace-kuy","title":"Move parquet documentation to docs/","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T02:52:12.609090777Z","created_by":"crosstyan","updated_at":"2026-02-07T02:52:43.088520272Z","closed_at":"2026-02-07T02:52:43.088520272Z","close_reason":"Moved parquet documentation to docs/marker-parquet-format.md"} +{"id":"py_workspace-q4w","title":"Add type hints and folder-aware --svo input in calibrate_extrinsics.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-06T10:01:13.943518267Z","created_by":"crosstyan","updated_at":"2026-02-06T10:03:09.855307397Z","closed_at":"2026-02-06T10:03:09.855307397Z","close_reason":"Implemented type hints and directory expansion for --svo"} +{"id":"py_workspace-t4e","title":"Add --min-markers CLI and rejection debug logs in calibrate_extrinsics","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-06T10:21:51.846079425Z","created_by":"crosstyan","updated_at":"2026-02-06T10:22:39.870440044Z","closed_at":"2026-02-06T10:22:39.870440044Z","close_reason":"Added --min-markers (default 1), rejection debug logs, and clarified accepted-pose summary label"} +{"id":"py_workspace-z3r","title":"Add debug logs for successful ArUco detection","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-06T10:17:30.195422209Z","created_by":"crosstyan","updated_at":"2026-02-06T10:18:35.263206185Z","closed_at":"2026-02-06T10:18:35.263206185Z","close_reason":"Added loguru debug logs for successful ArUco detections in calibrate_extrinsics loop"} diff --git a/py_workspace/.beads/metadata.json b/py_workspace/.beads/metadata.json new file mode 100644 index 0000000..c787975 --- /dev/null +++ b/py_workspace/.beads/metadata.json @@ -0,0 +1,4 @@ +{ + "database": "beads.db", + "jsonl_export": "issues.jsonl" +} \ No newline at end of file diff --git a/py_workspace/.sisyphus/boulder.json b/py_workspace/.sisyphus/boulder.json new file mode 100644 index 0000000..35ce547 --- /dev/null +++ b/py_workspace/.sisyphus/boulder.json @@ -0,0 +1,8 @@ +{ + "active_plan": "/workspaces/zed-playground/py_workspace/.sisyphus/plans/ground-plane-alignment.md", + "started_at": "2026-02-06T10:34:57.130Z", + "session_ids": [ + "ses_3cd9cdde1ffeQFgrhQqYAExSTn" + ], + "plan_name": "ground-plane-alignment" +} \ No newline at end of file diff --git a/py_workspace/.sisyphus/notepads/ground-plane-alignment/decisions.md b/py_workspace/.sisyphus/notepads/ground-plane-alignment/decisions.md new file mode 100644 index 0000000..769b0c3 --- /dev/null +++ b/py_workspace/.sisyphus/notepads/ground-plane-alignment/decisions.md @@ -0,0 +1,2 @@ +- Alignment is applied via pre-multiplication to the 4x4 pose matrix, consistent with global frame rotation. +- Chose to raise ValueError for degenerate cases (collinear corners) in compute_face_normal. diff --git a/py_workspace/.sisyphus/notepads/ground-plane-alignment/issues.md b/py_workspace/.sisyphus/notepads/ground-plane-alignment/issues.md index 090a819..6e0ccaa 100644 --- a/py_workspace/.sisyphus/notepads/ground-plane-alignment/issues.md +++ b/py_workspace/.sisyphus/notepads/ground-plane-alignment/issues.md @@ -32,7 +32,14 @@ ## Debugging Heuristics ## Documentation Gaps -- Users were unclear on how `--auto-align` made decisions (heuristic vs explicit) and what `--refine-depth` actually did. The new documentation addresses this by explaining the decision flow and the optimization objective function. + +## Jaxtyping Runtime Dependencies +- `jaxtyping` imports failed at runtime because it expects a backend (jax, torch, or tensorflow) to be installed. + +## Depth Refinement Failure +- Depth refinement was failing (0 iterations, no improvement) because the depth map was in millimeters (~2500) while the computed depth from extrinsics was in meters (~2.5). This resulted in huge residuals (~2497.5) that the optimizer couldn't handle effectively. Fixed by normalizing the depth map to meters immediately upon retrieval. + + diff --git a/py_workspace/.sisyphus/notepads/ground-plane-alignment/learnings.md b/py_workspace/.sisyphus/notepads/ground-plane-alignment/learnings.md index 17ed0ed..37a2def 100644 --- a/py_workspace/.sisyphus/notepads/ground-plane-alignment/learnings.md +++ b/py_workspace/.sisyphus/notepads/ground-plane-alignment/learnings.md @@ -64,7 +64,17 @@ ## Debug Visibility ## Documentation -- Created `docs/calibrate-extrinsics-workflow.md` to document the runtime behavior of the calibration tool, specifically detailing the precedence logic for ground plane alignment and the mathematical basis for depth verification/refinement. + +## Type Annotation Hardening +- Integrated `jaxtyping` for shape-aware array annotations (e.g., `Float[np.ndarray, "4 4"]`). +- Used `TYPE_CHECKING` blocks to define these aliases, ensuring they are available for static analysis (like `basedpyright`) while falling back to standard `np.ndarray` at runtime if `jaxtyping` backends are missing. + +## Depth Units +- ZED SDK `retrieve_measure(sl.MEASURE.DEPTH)` returns values in the unit defined in `InitParameters.coordinate_units`. +- The default unit is `MILLIMETER`. +- Since our extrinsics and marker geometry are in meters, we must explicitly convert the retrieved depth map to meters (divide by 1000.0) to avoid massive scale mismatches during verification and refinement. + + diff --git a/py_workspace/.sisyphus/notepads/ground-plane-alignment/problems.md b/py_workspace/.sisyphus/notepads/ground-plane-alignment/problems.md new file mode 100644 index 0000000..e69de29 diff --git a/py_workspace/aruco/alignment.py b/py_workspace/aruco/alignment.py index 91fc028..7ff47e8 100644 --- a/py_workspace/aruco/alignment.py +++ b/py_workspace/aruco/alignment.py @@ -1,8 +1,22 @@ import numpy as np from loguru import logger +from jaxtyping import Float +from typing import TYPE_CHECKING + +# Type aliases for shape-aware annotations +if TYPE_CHECKING: + Vec3 = Float[np.ndarray, "3"] + Mat33 = Float[np.ndarray, "3 3"] + Mat44 = Float[np.ndarray, "4 4"] + CornersNC = Float[np.ndarray, "N 3"] +else: + Vec3 = np.ndarray + Mat33 = np.ndarray + Mat44 = np.ndarray + CornersNC = np.ndarray -def compute_face_normal(corners: np.ndarray) -> np.ndarray: +def compute_face_normal(corners: CornersNC) -> Vec3: """ Compute the normal vector of a face defined by its corners. Assumes corners are in order (e.g., clockwise or counter-clockwise). @@ -37,7 +51,7 @@ def compute_face_normal(corners: np.ndarray) -> np.ndarray: return (normal / norm).astype(np.float64) -def rotation_align_vectors(from_vec: np.ndarray, to_vec: np.ndarray) -> np.ndarray: +def rotation_align_vectors(from_vec: Vec3, to_vec: Vec3) -> Mat33: """ Compute the 3x3 rotation matrix that aligns from_vec to to_vec. @@ -100,7 +114,7 @@ def rotation_align_vectors(from_vec: np.ndarray, to_vec: np.ndarray) -> np.ndarr return R.astype(np.float64) -def apply_alignment_to_pose(T: np.ndarray, R_align: np.ndarray) -> np.ndarray: +def apply_alignment_to_pose(T: Mat44, R_align: Mat33) -> Mat44: """ Apply an alignment rotation to a 4x4 pose matrix. The alignment is applied in the global frame (pre-multiplication of rotation). @@ -127,7 +141,7 @@ 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: +) -> Vec3 | None: """ Compute the average normal vector for a face based on available marker geometry. @@ -171,9 +185,9 @@ def get_face_normal_from_geometry( 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]), + camera_up_vector: Vec3 = np.array([0, -1, 0]), face_marker_map: dict[str, list[int]] | None = None, -) -> tuple[str, np.ndarray] | None: +) -> tuple[str, Vec3] | 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. diff --git a/py_workspace/aruco/markers/standard_box.glb b/py_workspace/aruco/markers/standard_box.glb new file mode 100644 index 0000000..61117eb Binary files /dev/null and b/py_workspace/aruco/markers/standard_box.glb differ diff --git a/py_workspace/aruco/markers/standard_box_markers.parquet b/py_workspace/aruco/markers/standard_box_markers.parquet new file mode 100644 index 0000000..964a3b9 Binary files /dev/null and b/py_workspace/aruco/markers/standard_box_markers.parquet differ diff --git a/py_workspace/aruco/output/.DS_Store b/py_workspace/aruco/output/.DS_Store deleted file mode 100644 index 34a0105..0000000 Binary files a/py_workspace/aruco/output/.DS_Store and /dev/null differ diff --git a/py_workspace/aruco/output/aruco_2d_uv_coords_normalized.json b/py_workspace/aruco/output/aruco_2d_uv_coords_normalized.json deleted file mode 100644 index a37a2cf..0000000 --- a/py_workspace/aruco/output/aruco_2d_uv_coords_normalized.json +++ /dev/null @@ -1 +0,0 @@ -[{"id":15,"center":[0.49987878787878787,0.13660606060606062],"corners":[[0.4749090909090909,0.1615757575757576],[0.524969696969697,0.1615757575757576],[0.524969696969697,0.11151515151515157],[0.4749090909090909,0.11151515151515157]]},{"id":13,"center":[0.43636363636363634,0.20012121212121214],"corners":[[0.41139393939393937,0.22509090909090912],[0.46145454545454545,0.22509090909090912],[0.46145454545454545,0.17503030303030298],[0.41139393939393937,0.17503030303030298]]},{"id":11,"center":[0.49987878787878787,0.3366060606060606],"corners":[[0.4749090909090909,0.36157575757575755],[0.524969696969697,0.36157575757575755],[0.524969696969697,0.3115151515151515],[0.4749090909090909,0.3115151515151515]]},{"id":23,"center":[0.29987878787878786,0.3366060606060606],"corners":[[0.27490909090909094,0.36157575757575755],[0.32496969696969696,0.36157575757575755],[0.32496969696969696,0.3115151515151515],[0.27490909090909094,0.3115151515151515]]},{"id":17,"center":[0.7633939393939394,0.4001212121212121],"corners":[[0.7884848484848485,0.37503030303030305],[0.7384242424242424,0.37503030303030305],[0.7384242424242424,0.4250909090909091],[0.7884848484848485,0.4250909090909091]]},{"id":9,"center":[0.43636363636363634,0.4001212121212121],"corners":[[0.41139393939393937,0.4250909090909091],[0.46145454545454545,0.4250909090909091],[0.46145454545454545,0.37503030303030305],[0.41139393939393937,0.37503030303030305]]},{"id":21,"center":[0.23636363636363636,0.4001212121212121],"corners":[[0.21139393939393938,0.4250909090909091],[0.26145454545454544,0.4250909090909091],[0.26145454545454544,0.37503030303030305],[0.21139393939393938,0.37503030303030305]]},{"id":19,"center":[0.6998787878787879,0.4636363636363636],"corners":[[0.7249696969696969,0.43854545454545457],[0.6749090909090909,0.43854545454545457],[0.6749090909090909,0.4886060606060606],[0.7249696969696969,0.4886060606060606]]},{"id":7,"center":[0.49987878787878787,0.5366060606060605],"corners":[[0.4749090909090909,0.5615757575757576],[0.524969696969697,0.5615757575757576],[0.524969696969697,0.5115151515151515],[0.4749090909090909,0.5115151515151515]]},{"id":5,"center":[0.43636363636363634,0.600121212121212],"corners":[[0.41139393939393937,0.6250909090909091],[0.46145454545454545,0.6250909090909091],[0.46145454545454545,0.575030303030303],[0.41139393939393937,0.575030303030303]]},{"id":3,"center":[0.49987878787878787,0.7366060606060606],"corners":[[0.4749090909090909,0.7615757575757576],[0.524969696969697,0.7615757575757576],[0.524969696969697,0.7115151515151514],[0.4749090909090909,0.7115151515151514]]},{"id":1,"center":[0.43636363636363634,0.8001212121212121],"corners":[[0.41139393939393937,0.8250909090909091],[0.46145454545454545,0.8250909090909091],[0.46145454545454545,0.7750303030303031],[0.41139393939393937,0.7750303030303031]]},{"id":14,"center":[0.5632727272727273,0.20012121212121214],"corners":[[0.5384242424242425,0.22509090909090912],[0.5883636363636363,0.22509090909090912],[0.5883636363636363,0.17503030303030298],[0.5383030303030303,0.17503030303030298]]},{"id":12,"center":[0.49987878787878787,0.2635151515151515],"corners":[[0.4749090909090909,0.28848484848484846],[0.524969696969697,0.28848484848484846],[0.5248484848484849,0.23842424242424243],[0.4749090909090909,0.23842424242424243]]},{"id":16,"center":[0.6998787878787879,0.33672727272727276],"corners":[[0.7249696969696969,0.3116363636363636],[0.6749090909090909,0.3116363636363636],[0.675030303030303,0.36169696969696974],[0.7249696969696969,0.36169696969696974]]},{"id":18,"center":[0.6364848484848484,0.4001212121212121],"corners":[[0.6614545454545454,0.37503030303030305],[0.6115151515151516,0.37503030303030305],[0.6115151515151516,0.4250909090909091],[0.6615757575757576,0.4250909090909091]]},{"id":10,"center":[0.5632727272727273,0.4001212121212121],"corners":[[0.5384242424242425,0.4250909090909091],[0.5883636363636363,0.4250909090909091],[0.5883636363636363,0.37503030303030305],[0.5383030303030303,0.37503030303030305]]},{"id":22,"center":[0.36327272727272725,0.4001212121212121],"corners":[[0.3384242424242424,0.4250909090909091],[0.38836363636363636,0.4250909090909091],[0.38836363636363636,0.37503030303030305],[0.3383030303030303,0.37503030303030305]]},{"id":8,"center":[0.49987878787878787,0.46351515151515155],"corners":[[0.4749090909090909,0.4884848484848485],[0.524969696969697,0.4884848484848485],[0.5248484848484849,0.4384242424242424],[0.4749090909090909,0.4384242424242424]]},{"id":20,"center":[0.29987878787878786,0.46351515151515155],"corners":[[0.27490909090909094,0.4884848484848485],[0.32496969696969696,0.4884848484848485],[0.32484848484848483,0.4384242424242424],[0.27490909090909094,0.4384242424242424]]},{"id":6,"center":[0.5632727272727273,0.600121212121212],"corners":[[0.5384242424242425,0.6250909090909091],[0.5883636363636363,0.6250909090909091],[0.5883636363636363,0.575030303030303],[0.5383030303030303,0.575030303030303]]},{"id":4,"center":[0.49987878787878787,0.6635151515151515],"corners":[[0.4749090909090909,0.6884848484848485],[0.524969696969697,0.6884848484848485],[0.5248484848484849,0.6384242424242424],[0.4749090909090909,0.6384242424242424]]},{"id":2,"center":[0.5632727272727273,0.8001212121212121],"corners":[[0.5384242424242425,0.8250909090909091],[0.5883636363636363,0.8250909090909091],[0.5883636363636363,0.7750303030303031],[0.5383030303030303,0.7750303030303031]]},{"id":0,"center":[0.49987878787878787,0.8635151515151516],"corners":[[0.4749090909090909,0.8884848484848484],[0.524969696969697,0.8884848484848484],[0.5248484848484849,0.8384242424242424],[0.4749090909090909,0.8384242424242424]]}] \ No newline at end of file diff --git a/py_workspace/aruco/output/aruco_3d_coords.json b/py_workspace/aruco/output/aruco_3d_coords.json deleted file mode 100644 index a37a2cf..0000000 --- a/py_workspace/aruco/output/aruco_3d_coords.json +++ /dev/null @@ -1 +0,0 @@ -[{"id":15,"center":[0.49987878787878787,0.13660606060606062],"corners":[[0.4749090909090909,0.1615757575757576],[0.524969696969697,0.1615757575757576],[0.524969696969697,0.11151515151515157],[0.4749090909090909,0.11151515151515157]]},{"id":13,"center":[0.43636363636363634,0.20012121212121214],"corners":[[0.41139393939393937,0.22509090909090912],[0.46145454545454545,0.22509090909090912],[0.46145454545454545,0.17503030303030298],[0.41139393939393937,0.17503030303030298]]},{"id":11,"center":[0.49987878787878787,0.3366060606060606],"corners":[[0.4749090909090909,0.36157575757575755],[0.524969696969697,0.36157575757575755],[0.524969696969697,0.3115151515151515],[0.4749090909090909,0.3115151515151515]]},{"id":23,"center":[0.29987878787878786,0.3366060606060606],"corners":[[0.27490909090909094,0.36157575757575755],[0.32496969696969696,0.36157575757575755],[0.32496969696969696,0.3115151515151515],[0.27490909090909094,0.3115151515151515]]},{"id":17,"center":[0.7633939393939394,0.4001212121212121],"corners":[[0.7884848484848485,0.37503030303030305],[0.7384242424242424,0.37503030303030305],[0.7384242424242424,0.4250909090909091],[0.7884848484848485,0.4250909090909091]]},{"id":9,"center":[0.43636363636363634,0.4001212121212121],"corners":[[0.41139393939393937,0.4250909090909091],[0.46145454545454545,0.4250909090909091],[0.46145454545454545,0.37503030303030305],[0.41139393939393937,0.37503030303030305]]},{"id":21,"center":[0.23636363636363636,0.4001212121212121],"corners":[[0.21139393939393938,0.4250909090909091],[0.26145454545454544,0.4250909090909091],[0.26145454545454544,0.37503030303030305],[0.21139393939393938,0.37503030303030305]]},{"id":19,"center":[0.6998787878787879,0.4636363636363636],"corners":[[0.7249696969696969,0.43854545454545457],[0.6749090909090909,0.43854545454545457],[0.6749090909090909,0.4886060606060606],[0.7249696969696969,0.4886060606060606]]},{"id":7,"center":[0.49987878787878787,0.5366060606060605],"corners":[[0.4749090909090909,0.5615757575757576],[0.524969696969697,0.5615757575757576],[0.524969696969697,0.5115151515151515],[0.4749090909090909,0.5115151515151515]]},{"id":5,"center":[0.43636363636363634,0.600121212121212],"corners":[[0.41139393939393937,0.6250909090909091],[0.46145454545454545,0.6250909090909091],[0.46145454545454545,0.575030303030303],[0.41139393939393937,0.575030303030303]]},{"id":3,"center":[0.49987878787878787,0.7366060606060606],"corners":[[0.4749090909090909,0.7615757575757576],[0.524969696969697,0.7615757575757576],[0.524969696969697,0.7115151515151514],[0.4749090909090909,0.7115151515151514]]},{"id":1,"center":[0.43636363636363634,0.8001212121212121],"corners":[[0.41139393939393937,0.8250909090909091],[0.46145454545454545,0.8250909090909091],[0.46145454545454545,0.7750303030303031],[0.41139393939393937,0.7750303030303031]]},{"id":14,"center":[0.5632727272727273,0.20012121212121214],"corners":[[0.5384242424242425,0.22509090909090912],[0.5883636363636363,0.22509090909090912],[0.5883636363636363,0.17503030303030298],[0.5383030303030303,0.17503030303030298]]},{"id":12,"center":[0.49987878787878787,0.2635151515151515],"corners":[[0.4749090909090909,0.28848484848484846],[0.524969696969697,0.28848484848484846],[0.5248484848484849,0.23842424242424243],[0.4749090909090909,0.23842424242424243]]},{"id":16,"center":[0.6998787878787879,0.33672727272727276],"corners":[[0.7249696969696969,0.3116363636363636],[0.6749090909090909,0.3116363636363636],[0.675030303030303,0.36169696969696974],[0.7249696969696969,0.36169696969696974]]},{"id":18,"center":[0.6364848484848484,0.4001212121212121],"corners":[[0.6614545454545454,0.37503030303030305],[0.6115151515151516,0.37503030303030305],[0.6115151515151516,0.4250909090909091],[0.6615757575757576,0.4250909090909091]]},{"id":10,"center":[0.5632727272727273,0.4001212121212121],"corners":[[0.5384242424242425,0.4250909090909091],[0.5883636363636363,0.4250909090909091],[0.5883636363636363,0.37503030303030305],[0.5383030303030303,0.37503030303030305]]},{"id":22,"center":[0.36327272727272725,0.4001212121212121],"corners":[[0.3384242424242424,0.4250909090909091],[0.38836363636363636,0.4250909090909091],[0.38836363636363636,0.37503030303030305],[0.3383030303030303,0.37503030303030305]]},{"id":8,"center":[0.49987878787878787,0.46351515151515155],"corners":[[0.4749090909090909,0.4884848484848485],[0.524969696969697,0.4884848484848485],[0.5248484848484849,0.4384242424242424],[0.4749090909090909,0.4384242424242424]]},{"id":20,"center":[0.29987878787878786,0.46351515151515155],"corners":[[0.27490909090909094,0.4884848484848485],[0.32496969696969696,0.4884848484848485],[0.32484848484848483,0.4384242424242424],[0.27490909090909094,0.4384242424242424]]},{"id":6,"center":[0.5632727272727273,0.600121212121212],"corners":[[0.5384242424242425,0.6250909090909091],[0.5883636363636363,0.6250909090909091],[0.5883636363636363,0.575030303030303],[0.5383030303030303,0.575030303030303]]},{"id":4,"center":[0.49987878787878787,0.6635151515151515],"corners":[[0.4749090909090909,0.6884848484848485],[0.524969696969697,0.6884848484848485],[0.5248484848484849,0.6384242424242424],[0.4749090909090909,0.6384242424242424]]},{"id":2,"center":[0.5632727272727273,0.8001212121212121],"corners":[[0.5384242424242425,0.8250909090909091],[0.5883636363636363,0.8250909090909091],[0.5883636363636363,0.7750303030303031],[0.5383030303030303,0.7750303030303031]]},{"id":0,"center":[0.49987878787878787,0.8635151515151516],"corners":[[0.4749090909090909,0.8884848484848484],[0.524969696969697,0.8884848484848484],[0.5248484848484849,0.8384242424242424],[0.4749090909090909,0.8384242424242424]]}] \ No newline at end of file diff --git a/py_workspace/aruco/output/object_points.parquet b/py_workspace/aruco/output/object_points.parquet deleted file mode 100644 index d6ed3d7..0000000 Binary files a/py_workspace/aruco/output/object_points.parquet and /dev/null differ diff --git a/py_workspace/aruco/output/standard_box_markers.parquet b/py_workspace/aruco/output/standard_box_markers.parquet deleted file mode 100644 index 8c4d375..0000000 Binary files a/py_workspace/aruco/output/standard_box_markers.parquet and /dev/null differ diff --git a/py_workspace/aruco/pose_averaging.py b/py_workspace/aruco/pose_averaging.py index ca8faa9..37fc8fa 100644 --- a/py_workspace/aruco/pose_averaging.py +++ b/py_workspace/aruco/pose_averaging.py @@ -234,9 +234,9 @@ class PoseAccumulator: stats = { "n_total": n_total, "n_inliers": n_inliers, - "median_reproj_error": float(np.median(inlier_errors)) - if inlier_errors - else 0.0, + "median_reproj_error": ( + float(np.median(inlier_errors)) if inlier_errors else 0.0 + ), } return T_mean, stats diff --git a/py_workspace/aruco/svo_sync.py b/py_workspace/aruco/svo_sync.py index 47fabbd..7634642 100644 --- a/py_workspace/aruco/svo_sync.py +++ b/py_workspace/aruco/svo_sync.py @@ -182,7 +182,11 @@ class SVOReader: return None depth_mat = sl.Mat() cam.retrieve_measure(depth_mat, sl.MEASURE.DEPTH) - return depth_mat.get_data().copy() + depth_data = depth_mat.get_data().copy() + + # ZED SDK defaults to MILLIMETER units if not specified in InitParameters. + # We convert to meters to match the extrinsics coordinate system. + return depth_data / 1000.0 def _retrieve_confidence(self, cam: sl.Camera) -> np.ndarray | None: if not self.enable_depth: diff --git a/py_workspace/calibrate_extrinsics.py b/py_workspace/calibrate_extrinsics.py index a43dcea..a04e136 100644 --- a/py_workspace/calibrate_extrinsics.py +++ b/py_workspace/calibrate_extrinsics.py @@ -29,8 +29,21 @@ from aruco.alignment import ( detect_ground_face, rotation_align_vectors, apply_alignment_to_pose, + Vec3, + Mat44, ) from loguru import logger +from jaxtyping import Float +from typing import TYPE_CHECKING + +# Type aliases +if TYPE_CHECKING: + Mat33 = Float[np.ndarray, "3 3"] + CornersNC = Float[np.ndarray, "N 3"] +else: + Mat33 = np.ndarray + CornersNC = np.ndarray + ARUCO_DICT_MAP = { "DICT_4X4_50": cv2.aruco.DICT_4X4_50, @@ -590,11 +603,11 @@ def main( ) if ground_normal is not None: - R_align = rotation_align_vectors(ground_normal, np.array([0, 1, 0])) + R_align: Mat33 = 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_mean: Mat44 = 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}") diff --git a/py_workspace/depth_sensing.py b/py_workspace/depth_sensing.py index f3b7f78..570488c 100644 --- a/py_workspace/depth_sensing.py +++ b/py_workspace/depth_sensing.py @@ -1,26 +1,6 @@ -######################################################################## -# -# Copyright (c) 2022, STEREOLABS. -# -# All rights reserved. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -######################################################################## - """ - This sample demonstrates how to capture a live 3D point cloud - with the ZED SDK and display the result in an OpenGL window. +This sample demonstrates how to capture a live 3D point cloud +with the ZED SDK and display the result in an OpenGL window. """ import sys @@ -28,47 +8,56 @@ import ogl_viewer.viewer as gl import pyzed.sl as sl import argparse + def parse_args(init, opt): - if len(opt.input_svo_file)>0 and opt.input_svo_file.endswith((".svo", ".svo2")): + if len(opt.input_svo_file) > 0 and opt.input_svo_file.endswith((".svo", ".svo2")): init.set_from_svo_file(opt.input_svo_file) print("[Sample] Using SVO File input: {0}".format(opt.input_svo_file)) - elif len(opt.ip_address)>0 : + elif len(opt.ip_address) > 0: ip_str = opt.ip_address - if ip_str.replace(':','').replace('.','').isdigit() and len(ip_str.split('.'))==4 and len(ip_str.split(':'))==2: - init.set_from_stream(ip_str.split(':')[0],int(ip_str.split(':')[1])) - print("[Sample] Using Stream input, IP : ",ip_str) - elif ip_str.replace(':','').replace('.','').isdigit() and len(ip_str.split('.'))==4: + if ( + ip_str.replace(":", "").replace(".", "").isdigit() + and len(ip_str.split(".")) == 4 + and len(ip_str.split(":")) == 2 + ): + init.set_from_stream(ip_str.split(":")[0], int(ip_str.split(":")[1])) + print("[Sample] Using Stream input, IP : ", ip_str) + elif ( + ip_str.replace(":", "").replace(".", "").isdigit() + and len(ip_str.split(".")) == 4 + ): init.set_from_stream(ip_str) - print("[Sample] Using Stream input, IP : ",ip_str) - else : + print("[Sample] Using Stream input, IP : ", ip_str) + else: print("Unvalid IP format. Using live stream") - if ("HD2K" in opt.resolution): + if "HD2K" in opt.resolution: init.camera_resolution = sl.RESOLUTION.HD2K print("[Sample] Using Camera in resolution HD2K") - elif ("HD1200" in opt.resolution): + elif "HD1200" in opt.resolution: init.camera_resolution = sl.RESOLUTION.HD1200 print("[Sample] Using Camera in resolution HD1200") - elif ("HD1080" in opt.resolution): + elif "HD1080" in opt.resolution: init.camera_resolution = sl.RESOLUTION.HD1080 print("[Sample] Using Camera in resolution HD1080") - elif ("HD720" in opt.resolution): + elif "HD720" in opt.resolution: init.camera_resolution = sl.RESOLUTION.HD720 print("[Sample] Using Camera in resolution HD720") - elif ("SVGA" in opt.resolution): + elif "SVGA" in opt.resolution: init.camera_resolution = sl.RESOLUTION.SVGA print("[Sample] Using Camera in resolution SVGA") - elif ("VGA" in opt.resolution): + elif "VGA" in opt.resolution: init.camera_resolution = sl.RESOLUTION.VGA print("[Sample] Using Camera in resolution VGA") - elif len(opt.resolution)>0: + elif len(opt.resolution) > 0: print("[Sample] No valid resolution entered. Using default") - else : + else: print("[Sample] Using default resolution") - def main(opt): - print("Running Depth Sensing sample ... Press 'Esc' to quit\nPress 's' to save the point cloud") + print( + "Running Depth Sensing sample ... Press 'Esc' to quit\nPress 's' to save the point cloud" + ) # Determine memory type based on CuPy availability and user preference use_gpu = gl.GPU_ACCELERATION_AVAILABLE and not opt.disable_gpu_data_transfer @@ -76,9 +65,11 @@ def main(opt): if use_gpu: print("🚀 Using GPU data transfer with CuPy") - init = sl.InitParameters(depth_mode=sl.DEPTH_MODE.NEURAL, - coordinate_units=sl.UNIT.METER, - coordinate_system=sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP) + init = sl.InitParameters( + depth_mode=sl.DEPTH_MODE.NEURAL, + coordinate_units=sl.UNIT.METER, + coordinate_system=sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP, + ) parse_args(init, opt) zed = sl.Camera() status = zed.open(init) @@ -107,9 +98,11 @@ def main(opt): if viewer.save_data: # For saving, we take CPU memory regardless of processing type point_cloud_to_save = sl.Mat() - zed.retrieve_measure(point_cloud_to_save, sl.MEASURE.XYZRGBA, sl.MEM.CPU) - err = point_cloud_to_save.write('Pointcloud.ply') - if(err == sl.ERROR_CODE.SUCCESS): + zed.retrieve_measure( + point_cloud_to_save, sl.MEASURE.XYZRGBA, sl.MEM.CPU + ) + err = point_cloud_to_save.write("Pointcloud.ply") + if err == sl.ERROR_CODE.SUCCESS: print("Current .ply file saving succeed") else: print("Current .ply file failed") @@ -120,12 +113,33 @@ def main(opt): if __name__ == "__main__": parser = argparse.ArgumentParser() - parser.add_argument('--input_svo_file', type=str, help='Path to an .svo file, if you want to replay it',default = '') - parser.add_argument('--ip_address', type=str, help='IP Adress, in format a.b.c.d:port or a.b.c.d, if you have a streaming setup', default = '') - parser.add_argument('--resolution', type=str, help='Resolution, can be either HD2K, HD1200, HD1080, HD720, SVGA or VGA', default = '') - parser.add_argument('--disable-gpu-data-transfer', action='store_true', help='Disable GPU data transfer acceleration with CuPy even if CuPy is available') + parser.add_argument( + "--input_svo_file", + type=str, + help="Path to an .svo file, if you want to replay it", + default="", + ) + parser.add_argument( + "--ip_address", + type=str, + help="IP Adress, in format a.b.c.d:port or a.b.c.d, if you have a streaming setup", + default="", + ) + parser.add_argument( + "--resolution", + type=str, + help="Resolution, can be either HD2K, HD1200, HD1080, HD720, SVGA or VGA", + default="", + ) + parser.add_argument( + "--disable-gpu-data-transfer", + action="store_true", + help="Disable GPU data transfer acceleration with CuPy even if CuPy is available", + ) opt = parser.parse_args() - if len(opt.input_svo_file)>0 and len(opt.ip_address)>0: - print("Specify only input_svo_file or ip_address, or none to use wired camera, not both. Exit program") + if len(opt.input_svo_file) > 0 and len(opt.ip_address) > 0: + print( + "Specify only input_svo_file or ip_address, or none to use wired camera, not both. Exit program" + ) exit() main(opt) diff --git a/py_workspace/docs/calibrate-extrinsics-workflow.md b/py_workspace/docs/calibrate-extrinsics-workflow.md index 34bef94..4285f16 100644 --- a/py_workspace/docs/calibrate-extrinsics-workflow.md +++ b/py_workspace/docs/calibrate-extrinsics-workflow.md @@ -131,7 +131,7 @@ If you suspect a unit mismatch, check the `depth_verify` RMSE in the output JSON *Note: Confidence filtering (`--depth-confidence-threshold`) is orthogonal to this issue. A unit mismatch affects all valid pixels regardless of confidence.* -## Findings Summary (2026-02-07 exhaustive search) +## Findings Summary (2026-02-07) This section summarizes the latest deep investigation across local code, outputs, and external docs. diff --git a/py_workspace/ogl_viewer/viewer.py b/py_workspace/ogl_viewer/viewer.py index 881de91..499e925 100755 --- a/py_workspace/ogl_viewer/viewer.py +++ b/py_workspace/ogl_viewer/viewer.py @@ -34,7 +34,7 @@ void main() { } """ -POINTCLOUD_VERTEX_SHADER =""" +POINTCLOUD_VERTEX_SHADER = """ #version 330 core layout(location = 0) in vec4 in_VertexRGBA; uniform mat4 u_mvpMatrix; @@ -184,7 +184,9 @@ try: try: if cudart is not None: # Check if cudart is still available check_cudart_err( - cudart.cudaGraphicsUnmapResources(1, self._graphics_ressource, stream) + cudart.cudaGraphicsUnmapResources( + 1, self._graphics_ressource, stream + ) ) self._cuda_buffer = None except Exception: @@ -193,7 +195,7 @@ try: return self class CudaOpenGLMappedArray(CudaOpenGLMappedBuffer): - def __init__(self, dtype, shape, gl_buffer, flags=0, strides=None, order='C'): + def __init__(self, dtype, shape, gl_buffer, flags=0, strides=None, order="C"): super().__init__(gl_buffer, flags) self._dtype = dtype self._shape = shape @@ -227,19 +229,27 @@ class Shader: glAttachShader(self.program_id, vertex_id) glAttachShader(self.program_id, fragment_id) - glBindAttribLocation( self.program_id, 0, "in_vertex") - glBindAttribLocation( self.program_id, 1, "in_texCoord") + glBindAttribLocation(self.program_id, 0, "in_vertex") + glBindAttribLocation(self.program_id, 1, "in_texCoord") glLinkProgram(self.program_id) if glGetProgramiv(self.program_id, GL_LINK_STATUS) != GL_TRUE: info = glGetProgramInfoLog(self.program_id) - if (self.program_id is not None) and (self.program_id > 0) and glIsProgram(self.program_id): + if ( + (self.program_id is not None) + and (self.program_id > 0) + and glIsProgram(self.program_id) + ): glDeleteProgram(self.program_id) if (vertex_id is not None) and (vertex_id > 0) and glIsShader(vertex_id): glDeleteShader(vertex_id) - if (fragment_id is not None) and (fragment_id > 0) and glIsShader(fragment_id): + if ( + (fragment_id is not None) + and (fragment_id > 0) + and glIsShader(fragment_id) + ): glDeleteShader(fragment_id) - raise RuntimeError('Error linking program: %s' % (info)) + raise RuntimeError("Error linking program: %s" % (info)) if (vertex_id is not None) and (vertex_id > 0) and glIsShader(vertex_id): glDeleteShader(vertex_id) if (fragment_id is not None) and (fragment_id > 0) and glIsShader(fragment_id): @@ -257,9 +267,13 @@ class Shader: glCompileShader(shader_id) if glGetShaderiv(shader_id, GL_COMPILE_STATUS) != GL_TRUE: info = glGetShaderInfoLog(shader_id) - if (shader_id is not None) and (shader_id > 0) and glIsShader(shader_id): + if ( + (shader_id is not None) + and (shader_id > 0) + and glIsShader(shader_id) + ): glDeleteShader(shader_id) - raise RuntimeError('Shader compilation failed: %s' % (info)) + raise RuntimeError("Shader compilation failed: %s" % (info)) return shader_id except: if (shader_id is not None) and (shader_id > 0) and glIsShader(shader_id): @@ -269,8 +283,9 @@ class Shader: def get_program_id(self): return self.program_id + class Simple3DObject: - def __init__(self, _is_static, pts_size = 3, clr_size = 3): + def __init__(self, _is_static, pts_size=3, clr_size=3): self.is_init = False self.drawing_type = GL_TRIANGLES self.is_static = _is_static @@ -285,7 +300,7 @@ class Simple3DObject: for pt in _pts: self.vertices.append(pt) - def add_clr(self, _clrs): # _clr [r,g,b] + def add_clr(self, _clrs): # _clr [r,g,b] for clr in _clrs: self.colors.append(clr) @@ -315,15 +330,30 @@ class Simple3DObject: if len(self.vertices): glBindBuffer(GL_ARRAY_BUFFER, self.vboID[0]) - glBufferData(GL_ARRAY_BUFFER, len(self.vertices) * self.vertices.itemsize, (GLfloat * len(self.vertices))(*self.vertices), type_draw) - + glBufferData( + GL_ARRAY_BUFFER, + len(self.vertices) * self.vertices.itemsize, + (GLfloat * len(self.vertices))(*self.vertices), + type_draw, + ) + if len(self.colors): glBindBuffer(GL_ARRAY_BUFFER, self.vboID[1]) - glBufferData(GL_ARRAY_BUFFER, len(self.colors) * self.colors.itemsize, (GLfloat * len(self.colors))(*self.colors), type_draw) + glBufferData( + GL_ARRAY_BUFFER, + len(self.colors) * self.colors.itemsize, + (GLfloat * len(self.colors))(*self.colors), + type_draw, + ) if len(self.indices): glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, self.vboID[2]) - glBufferData(GL_ELEMENT_ARRAY_BUFFER,len(self.indices) * self.indices.itemsize,(GLuint * len(self.indices))(*self.indices), type_draw) + glBufferData( + GL_ELEMENT_ARRAY_BUFFER, + len(self.indices) * self.indices.itemsize, + (GLuint * len(self.indices))(*self.indices), + type_draw, + ) self.elementbufferSize = len(self.indices) @@ -341,33 +371,52 @@ class Simple3DObject: # Initialize vertex buffer (for XYZRGBA data) glBindBuffer(GL_ARRAY_BUFFER, self.vboID[0]) - glBufferData(GL_ARRAY_BUFFER, self.elementbufferSize * self.pt_type * self.vertices.itemsize, None, type_draw) + glBufferData( + GL_ARRAY_BUFFER, + self.elementbufferSize * self.pt_type * self.vertices.itemsize, + None, + type_draw, + ) # Try to set up GPU acceleration if available if self.use_gpu: try: - flags = cudart.cudaGraphicsRegisterFlags.cudaGraphicsRegisterFlagsWriteDiscard + flags = ( + cudart.cudaGraphicsRegisterFlags.cudaGraphicsRegisterFlagsWriteDiscard + ) self.cuda_mapped_buffer = CudaOpenGLMappedArray( - dtype=np.float32, - shape=(self.elementbufferSize, self.pt_type), - gl_buffer=self.vboID[0], - flags=flags + dtype=np.float32, + shape=(self.elementbufferSize, self.pt_type), + gl_buffer=self.vboID[0], + flags=flags, ) except Exception as e: - print(f"Failed to initialize GPU acceleration, falling back to CPU: {e}") + print( + f"Failed to initialize GPU acceleration, falling back to CPU: {e}" + ) self.use_gpu = False self.cuda_mapped_buffer = None # Initialize color buffer (not used for point clouds with XYZRGBA) if self.clr_type: glBindBuffer(GL_ARRAY_BUFFER, self.vboID[1]) - glBufferData(GL_ARRAY_BUFFER, self.elementbufferSize * self.clr_type * self.colors.itemsize, None, type_draw) + glBufferData( + GL_ARRAY_BUFFER, + self.elementbufferSize * self.clr_type * self.colors.itemsize, + None, + type_draw, + ) - for i in range (0, self.elementbufferSize): + for i in range(0, self.elementbufferSize): self.indices.append(i) glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, self.vboID[2]) - glBufferData(GL_ELEMENT_ARRAY_BUFFER,len(self.indices) * self.indices.itemsize,(GLuint * len(self.indices))(*self.indices), type_draw) + glBufferData( + GL_ELEMENT_ARRAY_BUFFER, + len(self.indices) * self.indices.itemsize, + (GLuint * len(self.indices))(*self.indices), + type_draw, + ) def setPoints(self, pc): """Update point cloud data from sl.Mat""" @@ -375,7 +424,11 @@ class Simple3DObject: return try: - if self.use_gpu and self.cuda_mapped_buffer and pc.get_memory_type() in (sl.MEM.GPU, sl.MEM.BOTH): + if ( + self.use_gpu + and self.cuda_mapped_buffer + and pc.get_memory_type() in (sl.MEM.GPU, sl.MEM.BOTH) + ): self.setPointsGPU(pc) else: self.setPointsCPU(pc) @@ -423,7 +476,9 @@ class Simple3DObject: # Get CPU pointer and upload to GPU buffer glBindBuffer(GL_ARRAY_BUFFER, self.vboID[0]) data_ptr = pc.get_pointer(sl.MEM.CPU) - buffer_size = self.elementbufferSize * self.pt_type * 4 # 4 bytes per float32 + buffer_size = ( + self.elementbufferSize * self.pt_type * 4 + ) # 4 bytes per float32 glBufferSubData(GL_ARRAY_BUFFER, 0, buffer_size, ctypes.c_void_p(data_ptr)) glBindBuffer(GL_ARRAY_BUFFER, 0) @@ -432,9 +487,9 @@ class Simple3DObject: raise def clear(self): - self.vertices = array.array('f') - self.colors = array.array('f') - self.indices = array.array('I') + self.vertices = array.array("f") + self.colors = array.array("f") + self.indices = array.array("I") self.elementbufferSize = 0 def set_drawing_type(self, _type): @@ -444,55 +499,57 @@ class Simple3DObject: if self.elementbufferSize: glEnableVertexAttribArray(0) glBindBuffer(GL_ARRAY_BUFFER, self.vboID[0]) - glVertexAttribPointer(0,self.pt_type,GL_FLOAT,GL_FALSE,0,None) + glVertexAttribPointer(0, self.pt_type, GL_FLOAT, GL_FALSE, 0, None) - if(self.clr_type): + if self.clr_type: glEnableVertexAttribArray(1) glBindBuffer(GL_ARRAY_BUFFER, self.vboID[1]) - glVertexAttribPointer(1,self.clr_type,GL_FLOAT,GL_FALSE,0,None) - + glVertexAttribPointer(1, self.clr_type, GL_FLOAT, GL_FALSE, 0, None) + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, self.vboID[2]) - glDrawElements(self.drawing_type, self.elementbufferSize, GL_UNSIGNED_INT, None) - + glDrawElements( + self.drawing_type, self.elementbufferSize, GL_UNSIGNED_INT, None + ) + glDisableVertexAttribArray(0) if self.clr_type: glDisableVertexAttribArray(1) def __del__(self): """Cleanup GPU resources""" - if hasattr(self, 'cuda_mapped_buffer') and self.cuda_mapped_buffer: + if hasattr(self, "cuda_mapped_buffer") and self.cuda_mapped_buffer: try: self.cuda_mapped_buffer.unregister() except: pass + class GLViewer: def __init__(self): self.available = False self.mutex = Lock() self.camera = CameraGL() - self.wheelPosition = 0. + self.wheelPosition = 0.0 self.mouse_button = [False, False] - self.mouseCurrentPosition = [0., 0.] - self.previousMouseMotion = [0., 0.] - self.mouseMotion = [0., 0.] + self.mouseCurrentPosition = [0.0, 0.0] + self.previousMouseMotion = [0.0, 0.0] + self.mouseMotion = [0.0, 0.0] self.zedModel = Simple3DObject(True) self.point_cloud = Simple3DObject(False, 4) self.save_data = False - def init(self, _argc, _argv, res): # _params = sl.CameraParameters + def init(self, _argc, _argv, res): # _params = sl.CameraParameters glutInit(_argc, _argv) - wnd_w = int(glutGet(GLUT_SCREEN_WIDTH)*0.9) - wnd_h = int(glutGet(GLUT_SCREEN_HEIGHT) *0.9) + wnd_w = int(glutGet(GLUT_SCREEN_WIDTH) * 0.9) + wnd_h = int(glutGet(GLUT_SCREEN_HEIGHT) * 0.9) glutInitWindowSize(wnd_w, wnd_h) - glutInitWindowPosition(int(wnd_w*0.05), int(wnd_h*0.05)) + glutInitWindowPosition(int(wnd_w * 0.05), int(wnd_h * 0.05)) glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH) glutCreateWindow(b"ZED Depth Sensing") glViewport(0, 0, wnd_w, wnd_h) - glutSetOption(GLUT_ACTION_ON_WINDOW_CLOSE, - GLUT_ACTION_CONTINUE_EXECUTION) + glutSetOption(GLUT_ACTION_ON_WINDOW_CLOSE, GLUT_ACTION_CONTINUE_EXECUTION) glEnable(GL_DEPTH_TEST) @@ -504,17 +561,21 @@ class GLViewer: # Compile and create the shader for 3D objects self.shader_image = Shader(VERTEX_SHADER, FRAGMENT_SHADER) - self.shader_image_MVP = glGetUniformLocation(self.shader_image.get_program_id(), "u_mvpMatrix") + self.shader_image_MVP = glGetUniformLocation( + self.shader_image.get_program_id(), "u_mvpMatrix" + ) self.shader_pc = Shader(POINTCLOUD_VERTEX_SHADER, POINTCLOUD_FRAGMENT_SHADER) - self.shader_pc_MVP = glGetUniformLocation(self.shader_pc.get_program_id(), "u_mvpMatrix") + self.shader_pc_MVP = glGetUniformLocation( + self.shader_pc.get_program_id(), "u_mvpMatrix" + ) - self.bckgrnd_clr = np.array([223/255., 230/255., 233/255.]) + self.bckgrnd_clr = np.array([223 / 255.0, 230 / 255.0, 233 / 255.0]) # Create the camera model Z_ = -0.15 - Y_ = Z_ * math.tan(95. * M_PI / 180. / 2.) - X_ = Y_ * 16./9. + Y_ = Z_ * math.tan(95.0 * M_PI / 180.0 / 2.0) + X_ = Y_ * 16.0 / 9.0 A = np.array([0, 0, 0]) B = np.array([X_, Y_, Z_]) @@ -522,7 +583,7 @@ class GLViewer: D = np.array([-X_, -Y_, Z_]) E = np.array([X_, -Y_, Z_]) - lime_clr = np.array([217 / 255, 255/255, 66/255]) + lime_clr = np.array([217 / 255, 255 / 255, 66 / 255]) self.zedModel.add_line(A, B, lime_clr) self.zedModel.add_line(A, C, lime_clr) @@ -578,39 +639,40 @@ class GLViewer: def keyPressedCallback(self, key, x, y): if ord(key) == 27: self.close_func() - if (ord(key) == 83 or ord(key) == 115): + if ord(key) == 83 or ord(key) == 115: self.save_data = True - - def on_mouse(self,*args,**kwargs): - (key,Up,x,y) = args - if key==0: - self.mouse_button[0] = (Up == 0) - elif key==2 : - self.mouse_button[1] = (Up == 0) - elif(key == 3): + def on_mouse(self, *args, **kwargs): + key, Up, x, y = args + if key == 0: + self.mouse_button[0] = Up == 0 + elif key == 2: + self.mouse_button[1] = Up == 0 + elif key == 3: self.wheelPosition = self.wheelPosition + 1 - elif(key == 4): + elif key == 4: self.wheelPosition = self.wheelPosition - 1 self.mouseCurrentPosition = [x, y] self.previousMouseMotion = [x, y] - def on_mousemove(self,*args,**kwargs): - (x,y) = args + def on_mousemove(self, *args, **kwargs): + x, y = args self.mouseMotion[0] = x - self.previousMouseMotion[0] self.mouseMotion[1] = y - self.previousMouseMotion[1] self.previousMouseMotion = [x, y] glutPostRedisplay() - def on_resize(self,Width,Height): + def on_resize(self, Width, Height): glViewport(0, 0, Width, Height) self.camera.setProjection(Height / Width) def draw_callback(self): if self.available: glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) - glClearColor(self.bckgrnd_clr[0], self.bckgrnd_clr[1], self.bckgrnd_clr[2], 1.) + glClearColor( + self.bckgrnd_clr[0], self.bckgrnd_clr[1], self.bckgrnd_clr[2], 1.0 + ) self.mutex.acquire() self.update() @@ -621,21 +683,21 @@ class GLViewer: glutPostRedisplay() def update(self): - if(self.mouse_button[0]): + if self.mouse_button[0]: r = sl.Rotation() - vert=self.camera.vertical_ + vert = self.camera.vertical_ tmp = vert.get() - vert.init_vector(tmp[0] * 1.,tmp[1] * 1., tmp[2] * 1.) + vert.init_vector(tmp[0] * 1.0, tmp[1] * 1.0, tmp[2] * 1.0) r.init_angle_translation(self.mouseMotion[0] * 0.02, vert) self.camera.rotate(r) r.init_angle_translation(self.mouseMotion[1] * 0.02, self.camera.right_) self.camera.rotate(r) - if(self.mouse_button[1]): + if self.mouse_button[1]: t = sl.Translation() tmp = self.camera.right_.get() - scale = self.mouseMotion[0] *-0.05 + scale = self.mouseMotion[0] * -0.05 t.init_vector(tmp[0] * scale, tmp[1] * scale, tmp[2] * scale) self.camera.translate(t) @@ -644,7 +706,7 @@ class GLViewer: t.init_vector(tmp[0] * scale, tmp[1] * scale, tmp[2] * scale) self.camera.translate(t) - if (self.wheelPosition != 0): + if self.wheelPosition != 0: t = sl.Translation() tmp = self.camera.forward_.get() scale = self.wheelPosition * -0.065 @@ -653,34 +715,39 @@ class GLViewer: self.camera.update() - self.mouseMotion = [0., 0.] + self.mouseMotion = [0.0, 0.0] self.wheelPosition = 0 def draw(self): vpMatrix = self.camera.getViewProjectionMatrix() glUseProgram(self.shader_image.get_program_id()) - glUniformMatrix4fv(self.shader_image_MVP, 1, GL_TRUE, (GLfloat * len(vpMatrix))(*vpMatrix)) + glUniformMatrix4fv( + self.shader_image_MVP, 1, GL_TRUE, (GLfloat * len(vpMatrix))(*vpMatrix) + ) glPolygonMode(GL_FRONT_AND_BACK, GL_FILL) self.zedModel.draw() glUseProgram(0) glUseProgram(self.shader_pc.get_program_id()) - glUniformMatrix4fv(self.shader_pc_MVP, 1, GL_TRUE, (GLfloat * len(vpMatrix))(*vpMatrix)) - glPointSize(1.) + glUniformMatrix4fv( + self.shader_pc_MVP, 1, GL_TRUE, (GLfloat * len(vpMatrix))(*vpMatrix) + ) + glPointSize(1.0) self.point_cloud.draw() glUseProgram(0) - + + class CameraGL: def __init__(self): self.ORIGINAL_FORWARD = sl.Translation() - self.ORIGINAL_FORWARD.init_vector(0,0,1) + self.ORIGINAL_FORWARD.init_vector(0, 0, 1) self.ORIGINAL_UP = sl.Translation() - self.ORIGINAL_UP.init_vector(0,1,0) + self.ORIGINAL_UP.init_vector(0, 1, 0) self.ORIGINAL_RIGHT = sl.Translation() - self.ORIGINAL_RIGHT.init_vector(1,0,0) + self.ORIGINAL_RIGHT.init_vector(1, 0, 0) self.znear = 0.5 - self.zfar = 100. - self.horizontalFOV = 70. + self.zfar = 100.0 + self.horizontalFOV = 70.0 self.orientation_ = sl.Orientation() self.position_ = sl.Translation() self.forward_ = sl.Translation() @@ -689,58 +756,62 @@ class CameraGL: self.vertical_ = sl.Translation() self.vpMatrix_ = sl.Matrix4f() self.offset_ = sl.Translation() - self.offset_.init_vector(0,0,5) + self.offset_.init_vector(0, 0, 5) self.projection_ = sl.Matrix4f() self.projection_.set_identity() self.setProjection(1.78) - self.position_.init_vector(0., 0., 0.) + self.position_.init_vector(0.0, 0.0, 0.0) tmp = sl.Translation() - tmp.init_vector(0, 0, -.1) + tmp.init_vector(0, 0, -0.1) tmp2 = sl.Translation() tmp2.init_vector(0, 1, 0) - self.setDirection(tmp, tmp2) + self.setDirection(tmp, tmp2) - def update(self): + def update(self): dot_ = sl.Translation.dot_translation(self.vertical_, self.up_) - if(dot_ < 0.): + if dot_ < 0.0: tmp = self.vertical_.get() - self.vertical_.init_vector(tmp[0] * -1.,tmp[1] * -1., tmp[2] * -1.) + self.vertical_.init_vector(tmp[0] * -1.0, tmp[1] * -1.0, tmp[2] * -1.0) transformation = sl.Transform() tmp_position = self.position_.get() tmp = (self.offset_ * self.orientation_).get() new_position = sl.Translation() - new_position.init_vector(tmp_position[0] + tmp[0], tmp_position[1] + tmp[1], tmp_position[2] + tmp[2]) + new_position.init_vector( + tmp_position[0] + tmp[0], tmp_position[1] + tmp[1], tmp_position[2] + tmp[2] + ) transformation.init_orientation_translation(self.orientation_, new_position) transformation.inverse() self.vpMatrix_ = self.projection_ * transformation - - def setProjection(self, im_ratio): - fov_x = self.horizontalFOV * 3.1416 / 180. - fov_y = self.horizontalFOV * im_ratio * 3.1416 / 180. - self.projection_[(0,0)] = 1. / math.tan(fov_x * .5) - self.projection_[(1,1)] = 1. / math.tan(fov_y * .5) - self.projection_[(2,2)] = -(self.zfar + self.znear) / (self.zfar - self.znear) - self.projection_[(3,2)] = -1. - self.projection_[(2,3)] = -(2. * self.zfar * self.znear) / (self.zfar - self.znear) - self.projection_[(3,3)] = 0. - + def setProjection(self, im_ratio): + fov_x = self.horizontalFOV * 3.1416 / 180.0 + fov_y = self.horizontalFOV * im_ratio * 3.1416 / 180.0 + + self.projection_[(0, 0)] = 1.0 / math.tan(fov_x * 0.5) + self.projection_[(1, 1)] = 1.0 / math.tan(fov_y * 0.5) + self.projection_[(2, 2)] = -(self.zfar + self.znear) / (self.zfar - self.znear) + self.projection_[(3, 2)] = -1.0 + self.projection_[(2, 3)] = -(2.0 * self.zfar * self.znear) / ( + self.zfar - self.znear + ) + self.projection_[(3, 3)] = 0.0 + def getViewProjectionMatrix(self): tmp = self.vpMatrix_.m - vpMat = array.array('f') + vpMat = array.array("f") for row in tmp: for v in row: vpMat.append(v) return vpMat - + def getViewProjectionMatrixRT(self, tr): tmp = self.vpMatrix_ tmp.transpose() tr.transpose() - tmp = (tr * tmp).m - vpMat = array.array('f') + tmp = (tr * tmp).m + vpMat = array.array("f") for row in tmp: for v in row: vpMat.append(v) @@ -749,15 +820,15 @@ class CameraGL: def setDirection(self, dir, vert): dir.normalize() tmp = dir.get() - dir.init_vector(tmp[0] * -1.,tmp[1] * -1., tmp[2] * -1.) + dir.init_vector(tmp[0] * -1.0, tmp[1] * -1.0, tmp[2] * -1.0) self.orientation_.init_translation(self.ORIGINAL_FORWARD, dir) self.updateVectors() self.vertical_ = vert - if(sl.Translation.dot_translation(self.vertical_, self.up_) < 0.): + if sl.Translation.dot_translation(self.vertical_, self.up_) < 0.0: tmp = sl.Rotation() tmp.init_angle_translation(3.14, self.ORIGINAL_FORWARD) self.rotate(tmp) - + def translate(self, t): ref = self.position_.get() tmp = t.get() @@ -766,7 +837,7 @@ class CameraGL: def setPosition(self, p): self.position_ = p - def rotate(self, r): + def rotate(self, r): tmp = sl.Orientation() tmp.init_rotation(r) self.orientation_ = tmp * self.orientation_ @@ -781,5 +852,5 @@ class CameraGL: self.up_ = self.ORIGINAL_UP * self.orientation_ right = self.ORIGINAL_RIGHT tmp = right.get() - right.init_vector(tmp[0] * -1.,tmp[1] * -1., tmp[2] * -1.) + right.init_vector(tmp[0] * -1.0, tmp[1] * -1.0, tmp[2] * -1.0) self.right_ = right * self.orientation_ diff --git a/py_workspace/recording_multi.py b/py_workspace/recording_multi.py index 90daf44..9f054b1 100755 --- a/py_workspace/recording_multi.py +++ b/py_workspace/recording_multi.py @@ -8,7 +8,11 @@ import threading import signal import time import sys +import click import zed_network_utils +import cv2 +import queue +import os # Global variable to handle exit exit_app = False @@ -21,15 +25,27 @@ def signal_handler(signal, frame): print("\nCtrl+C pressed. Exiting...") -def acquisition(zed): +def acquisition(zed, frame_queue=None): """Acquisition thread function to continuously grab frames""" infos = zed.get_camera_information() + mat = sl.Mat() while not exit_app: if zed.grab() == sl.ERROR_CODE.SUCCESS: - # If needed, add more processing here - # But be aware that any processing involving the GiL will slow down the multi threading performance - pass + if frame_queue is not None: + # Retrieve left image + zed.retrieve_image(mat, sl.VIEW.LEFT) + # Convert to numpy and copy to ensure thread safety when passing to main + try: + # Keep latest frame only + if frame_queue.full(): + try: + frame_queue.get_nowait() + except queue.Empty: + pass + frame_queue.put_nowait(mat.get_data().copy()) + except queue.Full: + pass print(f"{infos.camera_model}[{infos.serial_number}] QUIT") @@ -39,8 +55,8 @@ def acquisition(zed): zed.close() -def open_camera(zed, config): - """Open a camera with given configuration and enable streaming""" +def open_camera(zed, config, save_dir): + """Open a camera with given configuration and enable recording""" ip, port = zed_network_utils.extract_ip_port(config) if not ip or not port: @@ -59,7 +75,7 @@ def open_camera(zed, config): print(f"ZED SN{serial} Opened from {ip}:{port}") # Enable Recording - output_svo_file = f"ZED_SN{serial}.svo2" + output_svo_file = os.path.join(save_dir, f"ZED_SN{serial}.svo2") recording_param = sl.RecordingParameters( output_svo_file.replace(" ", ""), sl.SVO_COMPRESSION_MODE.H265 ) @@ -76,41 +92,63 @@ def open_camera(zed, config): return True -def main(): +@click.command() +@click.option( + "--monitor", is_flag=True, help="Enable local monitoring of the camera streams." +) +@click.option( + "--config", + default=zed_network_utils.DEFAULT_CONFIG_PATH, + help="Path to the network configuration JSON file.", + type=click.Path(exists=True), +) +@click.option( + "--save-dir", + default=os.getcwd(), + help="Directory where SVO files will be saved.", + type=click.Path(exists=True, file_okay=False, dir_okay=True, writable=True), +) +def main(monitor, config, save_dir): global exit_app # Read network configuration using utility - network_config = zed_network_utils.parse_network_config() + network_config = zed_network_utils.parse_network_config(config) if not network_config: - return 1 + return print(f"Found {len(network_config)} cameras in configuration") if len(network_config) == 0: print("No ZED configured, exit program") - return 1 + return zed_open = False # Open all cameras zeds = [] threads = [] + queues = {} # serial -> queue for serial, config in network_config.items(): zed = sl.Camera() - if open_camera(zed, config): + if open_camera(zed, config, save_dir): zeds.append(zed) zed_open = True + fq = None + if monitor: + fq = queue.Queue(maxsize=1) + queues[serial] = fq + # Start acquisition thread immediately - thread = threading.Thread(target=acquisition, args=(zed,)) + thread = threading.Thread(target=acquisition, args=(zed, fq)) thread.start() threads.append(thread) if not zed_open: print("No ZED opened, exit program") - return 1 + return # Set up signal handler for Ctrl+C signal.signal(signal.SIGINT, signal_handler) @@ -118,10 +156,30 @@ def main(): # Main loop while not exit_app: - time.sleep(0.02) + if monitor: + for serial, q in queues.items(): + try: + frame = q.get_nowait() + # Display the frame + # Use serial number as window name to distinguish cameras + # Resize is handled by window automatically usually, or we can resize + cv2.imshow(f"ZED {serial}", frame) + except queue.Empty: + pass + + # Check for quit key + key = cv2.waitKey(10) + if key == 113 or key == ord("q") or key == 27: # q or Esc + exit_app = True + else: + time.sleep(0.02) # Wait for all threads to finish print("Exit signal, closing ZEDs") + + if monitor: + cv2.destroyAllWindows() + time.sleep(0.1) for thread in threads: @@ -132,4 +190,4 @@ def main(): if __name__ == "__main__": - sys.exit(main()) + main() diff --git a/py_workspace/streaming_receiver.py b/py_workspace/streaming_receiver.py index 3e716df..7e180fd 100755 --- a/py_workspace/streaming_receiver.py +++ b/py_workspace/streaming_receiver.py @@ -1,23 +1,3 @@ -######################################################################## -# -# Copyright (c) 2022, STEREOLABS. -# -# All rights reserved. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -######################################################################## - """ Read a stream and display the left images using OpenCV """ diff --git a/py_workspace/svo_playback.py b/py_workspace/svo_playback.py index e644c2e..03a7cbc 100755 --- a/py_workspace/svo_playback.py +++ b/py_workspace/svo_playback.py @@ -1,10 +1,10 @@ -# ... existing code ... import sys import pyzed.sl as sl import cv2 import argparse import os import math +from pathlib import Path def progress_bar(percent_done, bar_length=50): @@ -16,8 +16,38 @@ def progress_bar(percent_done, bar_length=50): def main(opt): - svo_files = opt.input_svo_files + input_paths = [Path(p) for p in opt.input_svo_files] + svo_files = [] + + for path in input_paths: + if path.is_dir(): + print(f"Searching for SVO files in {path}...") + found = sorted( + [ + str(f) + for f in path.iterdir() + if f.is_file() and f.suffix.lower() in (".svo", ".svo2") + ] + ) + if found: + print(f"Found {len(found)} files in {path}") + svo_files.extend(found) + else: + print(f"No .svo or .svo2 files found in {path}") + elif path.is_file(): + svo_files.append(str(path)) + else: + print(f"Path not found: {path}") + + if not svo_files: + print("No valid SVO files provided. Exiting.") + return + + # Sort files to ensure deterministic order + svo_files.sort() + cameras = [] + cam_data = [] # List of dicts to store camera info print(f"Opening {len(svo_files)} SVO files...") @@ -177,7 +207,7 @@ if __name__ == "__main__": "--input_svo_files", nargs="+", type=str, - help="Path to .svo/.svo2 files", + help="Path to .svo/.svo2 files or directories", required=True, ) opt = parser.parse_args()