docs: add ground plane refinement and depth data management sections

2026-02-09 08:59:11 +00:00
parent ae3c9eba76
commit 9061ed3abb
1 changed files with 74 additions and 0 deletions
@@ -136,6 +136,80 @@ uv run calibrate_extrinsics.py \
  --no-preview
 ```

+## Depth Data Management
+
+To enable decoupled refinement workflows, the system supports saving the depth data used during calibration.
+
+### Saving Depth Data
+Use the `--save-depth <path.h5>` flag with `calibrate_extrinsics.py`.
+```bash
+uv run calibrate_extrinsics.py ... --save-depth output/calibration_depth.h5
+```
+
+**HDF5 Format Structure:**
+- `meta/`: Global metadata (schema version, units=meters).
+- `cameras/{serial}/`:
+  - `intrinsics`: Camera matrix (3x3).
+  - `pooled_depth`: The aggregated depth map used for verification (gzip compressed).
+  - `resolution`: [width, height].
+
+This allows `refine_ground_plane.py` to run repeatedly with different parameters without re-processing the raw SVO files.
+
+## Ground Plane Refinement (`refine_ground_plane.py`)
+
+This standalone tool refines camera extrinsics by ensuring all cameras agree on the ground plane location. It addresses common issues where ArUco markers are slightly tilted or not perfectly coplanar with the floor.
+
+### Workflow
+```bash
+uv run refine_ground_plane.py \
+  --input-extrinsics output/calibrated.json \
+  --input-depth output/calibration_depth.h5 \
+  --output-extrinsics output/refined.json \
+  --plot --plot-output output/ground_debug.html
+```
+
+### Algorithm Details
+
+The algorithm proceeds in four stages:
+
+1.  **Plane Detection (Per Camera)**
+    - Unprojects the depth map to a point cloud in the **world frame** (using current extrinsics).
+    - Uses **RANSAC** (via Open3D) to segment the dominant plane.
+    - **Quality Gates**:
+        - Minimum inliers (default: 500 points).
+        - Normal orientation check (must be roughly vertical, `normal_vertical_thresh=0.9`).
+
+2.  **Robust Consensus**
+    - Computes a "consensus plane" from all valid camera detections.
+    - **Method**:
+        - Aligns all normals to the upper hemisphere.
+        - Computes the **geometric median** of normals and distances.
+        - Filters outliers based on deviation from the median (>15° angle or >0.5m distance).
+        - Computes a weighted average of the remaining inlier planes.
+
+3.  **Correction Calculation**
+    - Computes a rigid transform $T_{corr}$ for each camera.
+    - **Constraints**:
+        - **Rotation**: Only corrects pitch and roll (aligns normal to vertical). Yaw is preserved.
+        - **Translation**: Only corrects vertical height (aligns plane distance). X/Z position is preserved.
+    - **Consensus-Relative Correction**: By default, cameras are aligned to the *consensus plane* rather than absolute Y=0. This ensures relative consistency between cameras even if the absolute floor height is slightly off.
+
+4.  **Safety Guardrails**
+    - The correction is **rejected** if:
+        - Rotation > `max_rotation_deg` (default: 5°).
+        - Translation > `max_translation_m` (default: 0.1m).
+        - Deviation from consensus > `max_consensus_deviation` (default: 10°, 0.5m).
+    - **Why no ICP?**: For flat floors, plane-to-plane alignment is more robust than ICP. ICP on featureless planes can drift (slide) along the surface.
+
+### Tuning Guidance
+
+Based on end-to-end observations:
+
+-   **`--stride`**: Default is 8. Decrease to 4 or 2 for higher density if the floor is far away or sparse.
+-   **`--ransac-dist-thresh`**: Default 0.02m (2cm). Increase to 0.03-0.05m if the floor is uneven or depth noise is high.
+-   **`--max-rotation-deg`**: Keep this tight (3-5°). If the floor correction needs >5°, the initial ArUco calibration is likely poor and should be re-run.
+-   **`--target-y`**: Use this if you need the floor to be at a specific absolute height (e.g., -1.5m) instead of just consistent.
+
 ## Known Unexpected Behavior / Troubleshooting

 ### Resolved: Depth Refinement Failure (Unit Mismatch)