docs: update README and finalize ground plane refinement integration

py_workspace/.sisyphus/notepads/ground-plane-refinement/issues.md

@@ -1,3 +1,7 @@
 
 ## [2026-02-09] Dependency Update
 - Encountered a TOML parse error during the first `edit` attempt due to incorrect escaping of quotes and newlines in the `newString`. Fixed by providing the literal string in a subsequent `edit` call.
+
+## [2026-02-09] Final Integration
+- No regressions found in the full test suite.
+- basedpyright warnings are mostly related to missing stubs for third-party libraries (h5py, open3d, plotly) and deprecated type hints in older Python patterns, which are acceptable given the project's current state and consistency with existing code.

py_workspace/.sisyphus/notepads/ground-plane-refinement/learnings.md

@@ -5,3 +5,28 @@
 - Verified imports:
   - open3d: 0.19.0
   - h5py: 3.15.1
+
+## [2026-02-09] Ground Plane Detection Implementation
+- Implemented `unproject_depth_to_points` using vectorized NumPy operations for efficiency.
+- Implemented `detect_floor_plane` using Open3D's `segment_plane` (RANSAC) with deterministic seeding.
+- Implemented `compute_consensus_plane` with weighted averaging and normal alignment to handle flipped normals.
+- Implemented `compute_floor_correction` using `rotation_align_vectors` for minimal rotation (preserving yaw) and vertical translation.
+- Added comprehensive tests covering edge cases (outliers, bounds, identity transforms).
+- Refactored to use `FloorPlane` and `FloorCorrection` dataclasses for structured outputs.
+- Fixed test logic for `compute_consensus_plane` to correctly account for normal normalization effects on `d`.
+- Verified type safety with `basedpyright` (0 errors, only expected warnings).
+
+## [2026-02-09] Ground Plane Diagnostic Visualization
+- Implemented `create_ground_diagnostic_plot` using Plotly `go.Figure`.
+- Visualization includes:
+  - World origin axes (RGB triad).
+  - Consensus plane surface (semi-transparent gray).
+  - Per-camera floor points (scatter3d).
+  - Camera positions before (red) and after (green) refinement.
+- Added `save_diagnostic_plot` for HTML export.
+- Verified with smoke tests in `tests/test_ground_plane.py`.
+
+## [2026-02-09] Final Integration
+- Full test suite (90 tests) passed successfully.
+- basedpyright verified on new modules (depth_save.py, ground_plane.py, refine_ground_plane.py).
+- README updated with Ground Plane Refinement workflow.

py_workspace/README.md

@@ -131,3 +131,35 @@ Run numerical sanity checks on the poses (orthonormality, coplanarity, consisten
 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.
+

py_workspace/tests/test_depth_save_integration.py

@@ -0,0 +1,160 @@
+import pytest
+import numpy as np
+from unittest.mock import MagicMock, patch
+import sys
+from pathlib import Path
+
+# Add py_workspace to path
+sys.path.append(str(Path(__file__).parent.parent))
+
+from calibrate_extrinsics import apply_depth_verify_refine_postprocess
+
+
+@pytest.fixture
+def mock_dependencies():
+    with (
+        patch("calibrate_extrinsics.verify_extrinsics_with_depth") as mock_verify,
+        patch("calibrate_extrinsics.refine_extrinsics_with_depth") as mock_refine,
+        patch("calibrate_extrinsics.click.echo") as mock_echo,
+        patch("calibrate_extrinsics.save_depth_data") as mock_save_depth,
+    ):
+        # Setup mock return values
+        mock_verify_res = MagicMock()
+        mock_verify_res.rmse = 0.05
+        mock_verify_res.mean_abs = 0.04
+        mock_verify_res.median = 0.03
+        mock_verify_res.depth_normalized_rmse = 0.02
+        mock_verify_res.n_valid = 100
+        mock_verify_res.n_total = 120
+        mock_verify_res.residuals = []
+        mock_verify.return_value = mock_verify_res
+
+        mock_refine.return_value = (np.eye(4), {"success": True})
+
+        yield mock_verify, mock_refine, mock_echo, mock_save_depth
+
+
+def test_save_depth_data_integration(mock_dependencies):
+    """
+    Test that save_depth_data is called with correct data when save_depth_path is provided.
+    """
+    mock_verify, _, _, mock_save_depth = mock_dependencies
+
+    serial = "123456"
+    serial_int = int(serial)
+    results = {serial: {"pose": "1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1"}}
+
+    # Create frames
+    # Frame 1
+    f1 = MagicMock()
+    f1.depth_map = np.ones((10, 20)) * 2.0  # H=10, W=20
+    f1.confidence_map = np.zeros((10, 20))
+
+    # Frame 2
+    f2 = MagicMock()
+    f2.depth_map = np.ones((10, 20)) * 2.2
+    f2.confidence_map = np.zeros((10, 20))
+
+    vfs = [
+        {
+            "frame": f1,
+            "ids": np.array([[1]]),
+            "corners": np.zeros((1, 4, 2)),
+            "score": 100,
+            "frame_index": 10,
+        },
+        {
+            "frame": f2,
+            "ids": np.array([[1]]),
+            "corners": np.zeros((1, 4, 2)),
+            "score": 90,
+            "frame_index": 20,
+        },
+    ]
+
+    verification_frames = {serial_int: vfs}
+    marker_geometry = {1: np.zeros((4, 3))}
+    camera_matrices = {serial_int: np.eye(3)}
+
+    save_path = "output_depth.h5"
+
+    # Run with save_depth_path
+    apply_depth_verify_refine_postprocess(
+        results=results,
+        verification_frames=verification_frames,
+        marker_geometry=marker_geometry,
+        camera_matrices=camera_matrices,
+        verify_depth=True,
+        refine_depth=False,
+        use_confidence_weights=False,
+        depth_confidence_threshold=50,
+        depth_pool_size=2,
+        save_depth_path=save_path,
+    )
+
+    # Verify save_depth_data was called
+    mock_save_depth.assert_called_once()
+
+    # Check arguments
+    call_args = mock_save_depth.call_args
+    assert call_args[0][0] == save_path
+
+    camera_data = call_args[0][1]
+    assert serial in camera_data
+
+    cam_data = camera_data[serial]
+    assert "intrinsics" in cam_data
+    assert "resolution" in cam_data
+    assert cam_data["resolution"] == (20, 10)  # (W, H)
+
+    assert "pooled_depth" in cam_data
+    assert "pooled_confidence" in cam_data
+    assert "pool_metadata" in cam_data
+    assert "raw_frames" in cam_data
+
+    assert len(cam_data["raw_frames"]) == 2
+    assert cam_data["raw_frames"][0]["frame_index"] == 10
+    assert cam_data["raw_frames"][1]["frame_index"] == 20
+
+    # Check that raw frames contain depth maps
+    assert "depth_map" in cam_data["raw_frames"][0]
+    np.testing.assert_array_equal(cam_data["raw_frames"][0]["depth_map"], f1.depth_map)
+
+
+def test_save_depth_skipped_when_no_path(mock_dependencies):
+    """
+    Test that save_depth_data is NOT called when save_depth_path is None.
+    """
+    _, _, _, mock_save_depth = mock_dependencies
+
+    serial = "123456"
+    serial_int = int(serial)
+    results = {serial: {"pose": "1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1"}}
+
+    f1 = MagicMock()
+    f1.depth_map = np.ones((10, 10))
+    f1.confidence_map = np.zeros((10, 10))
+
+    vfs = [
+        {
+            "frame": f1,
+            "ids": np.array([[1]]),
+            "corners": np.zeros((1, 4, 2)),
+            "score": 100,
+            "frame_index": 10,
+        }
+    ]
+
+    apply_depth_verify_refine_postprocess(
+        results=results,
+        verification_frames={serial_int: vfs},
+        marker_geometry={1: np.zeros((4, 3))},
+        camera_matrices={serial_int: np.eye(3)},
+        verify_depth=True,
+        refine_depth=False,
+        use_confidence_weights=False,
+        depth_confidence_threshold=50,
+        save_depth_path=None,
+    )
+
+    mock_save_depth.assert_not_called()