zed-playground/workspaces/py_workspace/README.md

# ZED SDK - Depth Sensing

This sample shows how to retreive the current point cloud.

## Getting Started

 - Get the latest [ZED SDK](https://www.stereolabs.com/developers/release/) and [pyZED Package](https://www.stereolabs.com/docs/app-development/python/install/)
 - Check the [Documentation](https://www.stereolabs.com/docs/)

## Setting up (Optional)

For improved data retrieval and handling on GPU:
 - Install CuPy using pip
    ```sh
    pip install cupy-cuda11x  # For CUDA 11.x
    pip install cupy-cuda12x  # For CUDA 12.x
    ```
 - Install cuda bindings using pip
    ```sh
    pip install cuda-python
    ```

## Run the program

To run the program, use the following command in your terminal : 
```bash
python depth_sensing.py
```
If you wish to run the program from an input_svo_file, or an IP adress, or specify a resolution run : 

```bash
python depth_sensing.py --input_svo_file <input_svo_file> --ip_address <ip_address> --resolution <resolution> 
```
Arguments: 
  - `--input_svo_file` A path to an existing .svo file, that will be playbacked. If this parameter and ip_adress are not specified, the soft will use the camera wired as default.
  - `--ip_address` IP Address, in format a.b.c.d:port or a.b.c.d. If specified, the soft will try to connect to the IP.
  - `--resolution` Resolution, can be either HD2K, HD1200, HD1080, HD720, SVGA or VGA.
  - `--disable_gpu` Disable GPU acceleration even if CuPy is available.

### Features
 - Camera live point cloud is retreived
 - An OpenGL windows displays it in 3D

### Camera Extrinsics

```bash
uv run calibrate_extrinsics.py \
   -s output/ -m aruco/markers/standard_box_markers_600mm.parquet \
   --aruco-dictionary DICT_APRILTAG_36h11 \
   --verify-depth \
   --refine-depth \
   --no-preview \
   --report-csv output/e2e_refine_depth.csv \
   --auto-align \
   --output output/e2e_refine_depth.json
```

### Visualize Extrinsics

Visualize camera poses and frustums from a JSON extrinsics file using Plotly.

```bash
uv run visualize_extrinsics.py \
    --input output/e2e_refine_depth.json \
    --zed-configs ../zed_settings \
    --origin-axes-scale 2 \
    --output output/e2e_refine_depth.html

uv run compare_pose_sets.py \
    --pose-a-json output/e2e_refine_depth.json \
    --pose-b-json ../zed_settings/inside_network.json \
    --report-json output/ab_report.json \
    --plot-output output/ab_report.html
```

**Basic 3D Visualization (Interactive HTML):**
```bash
uv run visualize_extrinsics.py -i output/extrinsics.json --show
```

**Static Image Export (PNG):**
Requires `kaleido` (installed by default).
```bash
uv run visualize_extrinsics.py -i output/extrinsics.json -o output/viz.png
```

**Bird-Eye View:**
Use `--birdseye` for a top-down X-Z view (looking down Y axis).
```bash
uv run visualize_extrinsics.py -i output/extrinsics.json --birdseye --show
```

**Ground Plane & Origin Overlay:**
Render a semi-transparent X-Z ground plane and/or a world origin triad.
- `--show-ground/--no-show-ground`: Toggle ground plane (default: show).
- `--ground-y FLOAT`: Set the Y height of the plane (default: 0.0).
- `--ground-size FLOAT`: Set the side length of the plane in meters (default: 8.0).
- `--show-origin-axes/--no-show-origin-axes`: Toggle world origin triad (X:red, Y:green, Z:blue) (default: show).

*Example: Ground plane at Y=-1.5 with 10m size*
```bash
uv run visualize_extrinsics.py -i output/extrinsics.json --ground-y -1.5 --ground-size 10 --show
```

**Using ZED Configs for Accurate Frustums:**
Load intrinsics from ZED calibration files to render accurate frustum shapes.
- **Directory**: Matches `SN<serial>.conf` files to camera serials in the JSON.
- **Single File**: Applies one config to ALL cameras (useful for homogeneous setups).
- **Defaults**: Uses `--resolution FHD1200` and `--eye left` unless overridden.

*Example: Load from config directory*
```bash
uv run visualize_extrinsics.py -i output/extrinsics.json \
    --zed-configs /workspaces/zed-playground/zed_settings \
    --resolution HD1080 --show
```

**Troubleshooting:**
- **Cameras bunched up?** Check `--pose-convention`. `world_from_cam` is the standard convention for `calibrate_extrinsics.py` outputs. `cam_from_world` is deprecated.
- **Axes flipped?** Use `--world-basis opengl` to match C++ viewer conventions (X right, Y up, Z backward). Default is `cv` (X right, Y down, Z forward). The Plotly scene axes and labels are explicitly aligned to the selected basis.
- **Config not matching?** Ensure JSON keys match the serial numbers in `SN<serial>.conf` filenames.

For full options:
```bash
uv run visualize_extrinsics.py --help
```

**Diagnostics Mode:**
Run numerical sanity checks on the poses (orthonormality, coplanarity, consistency).
```bash
uv run visualize_extrinsics.py -i output/extrinsics.json --diagnose
```

### Ground Plane Refinement

Refine camera extrinsics by aligning the ground plane detected in depth maps. This is useful when ArUco markers are not perfectly coplanar with the floor or when you want to ensure all cameras agree on the floor height.

**1. Calibrate with Depth Saving:**
Run calibration and save the depth data used for refinement/verification.
```bash
uv run calibrate_extrinsics.py \
   -s output/ -m aruco/markers/standard_box_markers_600mm.parquet \
   --aruco-dictionary DICT_APRILTAG_36h11 \
   --verify-depth \
   --refine-depth \
   --save-depth output/depth_data.h5 \
   --output output/extrinsics.json
```

**2. Refine Ground Plane:**
Use the saved depth data to detect the floor and compute a global correction.
```bash
uv run refine_ground_plane.py \
    --input-extrinsics output/extrinsics.json \
    --input-depth output/depth_data.h5 \
    --output-extrinsics output/extrinsics_refined.json \
    --plot --plot-output output/ground_diagnostic.html
```

**Options:**
- `--plot`: Generates an interactive 3D visualization of the detected floor points, consensus plane, and camera pose corrections.
- `--max-rotation-deg`: Safety limit for the correction rotation (default: 5.0).
- `--max-translation-m`: Safety limit for the correction translation (default: 0.5).
- `--stride`: Pixel stride for depth sampling (default: 4). Increase for faster processing.
- `--icp`: Enable ICP refinement after ground plane alignment.
- `--icp-region`: Region to use for ICP registration (floor, hybrid, full) (default: hybrid).
- `--icp-global-init`: Enable FPFH+RANSAC global pre-alignment (default: False).
- `--icp-min-overlap`: Minimum overlap area/volume to accept a pair (default: 0.5).
- `--icp-band-height`: Height of the floor band in meters (default: 0.3).
- `--icp-robust-kernel`: Robust kernel for ICP optimization (none, tukey) (default: none).
- `--icp-robust-k`: Parameter k for robust kernel (default: 0.1).
- `--icp-depth-bias/--no-icp-depth-bias`: Enable/disable automatic depth bias estimation and pre-correction (default: enabled). This estimates per-camera depth offsets to improve ICP convergence in multi-camera setups with unit-to-unit inconsistencies.

**Hybrid Mode Example:**
Refine using both floor and vertical structures (walls/pillars) with global initialization and depth bias correction:
```bash
uv run refine_ground_plane.py \
    --input-extrinsics output/extrinsics.json \
    --input-depth output/depth_data.h5 \
    --output-extrinsics output/extrinsics_refined.json \
    --icp --icp-region hybrid --icp-global-init --icp-depth-bias
```