diff --git a/py_workspace/.beads/issues.jsonl b/py_workspace/.beads/issues.jsonl index 7c9932e..5158165 100644 --- a/py_workspace/.beads/issues.jsonl +++ b/py_workspace/.beads/issues.jsonl @@ -27,12 +27,14 @@ {"id":"py_workspace-gii","title":"Implement Y-down auto-align and add metadata to extrinsics output","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-09T04:10:40.0580733Z","created_by":"crosstyan","updated_at":"2026-02-09T04:11:31.853050842Z","closed_at":"2026-02-09T04:11:31.853050842Z","close_reason":"Implemented Y-down auto-align and added metadata to extrinsics output JSON."} {"id":"py_workspace-gv2","title":"Create apply_calibration_to_fusion_config.py script","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-09T03:20:08.635031083Z","created_by":"crosstyan","updated_at":"2026-02-09T03:21:20.005139771Z","closed_at":"2026-02-09T03:21:20.005139771Z","close_reason":"Script created and verified with smoke test and type checking."} {"id":"py_workspace-hnw","title":"Integrate --save-depth flag into calibrate_extrinsics.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-09T07:21:32.541956991Z","created_by":"crosstyan","updated_at":"2026-02-09T07:25:26.398780574Z","closed_at":"2026-02-09T07:25:26.398780574Z","close_reason":"Implemented --save-depth flag and integration tests"} +{"id":"py_workspace-i5y","title":"Remove --metrics-json from refine_ground_plane.py","description":"Remove the --metrics-json flag from refine_ground_plane.py and make _meta.ground_refined the only metrics sink. Update tests and documentation accordingly.","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-09T09:56:47.71058499Z","created_by":"crosstyan","updated_at":"2026-02-09T09:58:12.546776193Z","closed_at":"2026-02-09T09:58:12.546776193Z","close_reason":"Removed --metrics-json flag from refine_ground_plane.py, updated tests, and verified CLI help output. Documentation does not mention --metrics-json for refine_ground_plane.py."} {"id":"py_workspace-j8b","title":"Research scipy.optimize.least_squares robust optimization for depth residuals","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T04:54:04.720996955Z","created_by":"crosstyan","updated_at":"2026-02-07T04:55:22.995644Z","closed_at":"2026-02-07T04:55:22.995644Z","close_reason":"Research completed and recommendations provided."} {"id":"py_workspace-kpa","title":"Unit Hardening (P0)","status":"closed","priority":0,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T05:01:46.342605011Z","created_by":"crosstyan","updated_at":"2026-02-07T05:01:51.303022101Z","closed_at":"2026-02-07T05:01:51.303022101Z","close_reason":"Implemented unit hardening in SVOReader: set coordinate_units=METER and guarded manual conversion in _retrieve_depth. Added depth sanity logs."} {"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-kv8","title":"Update compare_pose_sets.py with Plotly visualization","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-08T07:55:38.911520186Z","created_by":"crosstyan","updated_at":"2026-02-08T07:57:13.711754402Z","closed_at":"2026-02-08T07:57:13.711754402Z","close_reason":"Added Plotly visualization to compare_pose_sets.py with camera frustums, axes, and ground plane overlay."} {"id":"py_workspace-ld1","title":"Search for depth unit conversion and scaling patterns","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T04:53:53.211242053Z","created_by":"crosstyan","updated_at":"2026-02-07T04:54:56.840335809Z","closed_at":"2026-02-07T04:54:56.840335809Z","close_reason":"Exhaustive search completed. Identified manual scaling in svo_sync.py and SDK-level scaling in depth_sensing.py. Documented risks in learnings.md."} {"id":"py_workspace-lo0","title":"Add --cv-to-opengl option to apply_calibration_to_fusion_config.py","status":"closed","priority":2,"issue_type":"feature","owner":"crosstyan@outlook.com","created_at":"2026-02-09T03:33:40.435844317Z","created_by":"crosstyan","updated_at":"2026-02-09T03:34:37.514923778Z","closed_at":"2026-02-09T03:34:37.514923778Z","close_reason":"Added --cv-to-opengl option with matrix conversion logic and documentation."} +{"id":"py_workspace-myo","title":"Update docs/calibrate-extrinsics-workflow.md with implementation details","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-09T09:08:37.688432325Z","created_by":"crosstyan","updated_at":"2026-02-09T09:13:33.810314641Z","closed_at":"2026-02-09T09:13:33.810314641Z","close_reason":"Updated documentation with implementation details"} {"id":"py_workspace-nlu","title":"Produce A/B visualization comparison for CV world basis","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-08T03:50:56.386223999Z","created_by":"crosstyan","updated_at":"2026-02-08T03:52:41.232154353Z","closed_at":"2026-02-08T03:52:41.232154353Z","close_reason":"Generated A/B comparison images and analyzed visual differences. Source files remain unchanged."} {"id":"py_workspace-nvw","title":"Update documentation for robust depth refinement","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T05:41:32.963615133Z","created_by":"crosstyan","updated_at":"2026-02-07T05:43:55.707975317Z","closed_at":"2026-02-07T05:43:55.707975317Z","close_reason":"Documentation updated with robust refinement details"} {"id":"py_workspace-nxy","title":"Remove --cv-to-opengl option from apply_calibration_to_fusion_config.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-09T04:01:56.334044487Z","created_by":"crosstyan","updated_at":"2026-02-09T04:02:55.01899015Z","closed_at":"2026-02-09T04:02:55.01899015Z","close_reason":"Removed --cv-to-opengl option and associated logic as requested."} diff --git a/py_workspace/docs/calibrate-extrinsics-workflow.md b/py_workspace/docs/calibrate-extrinsics-workflow.md index 2a0ecc0..07dd7e1 100644 --- a/py_workspace/docs/calibrate-extrinsics-workflow.md +++ b/py_workspace/docs/calibrate-extrinsics-workflow.md @@ -150,8 +150,9 @@ uv run calibrate_extrinsics.py ... --save-depth output/calibration_depth.h5 - `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]. + - `pooled_depth`: The aggregated depth map used for verification (gzip compressed). + - `raw_frames/`: (Optional) Individual frames if pooling wasn't used. This allows `refine_ground_plane.py` to run repeatedly with different parameters without re-processing the raw SVO files. @@ -227,3 +228,65 @@ The system now explicitly sets `InitParameters.coordinate_units = sl.UNIT.METER` If `refine_depth` shows `success: false` but `nfev` (evaluations) is high, the optimizer may have hit a flat region or local minimum. - **Check**: Look at `termination_message` in the JSON output. - **Fix**: Try enabling `--use-confidence-weights` or checking if the initial ArUco pose is too far off (reprojection error > 2.0). + +## Implementation Details + +### 1. Depth Data Structure (`--save-depth`) + +The system uses HDF5 for efficient, compressed storage of depth data required for decoupled refinement. + +**File Structure:** +- **`meta/`**: Global metadata. + - `schema_version`: Integer version (currently 1). + - `units`: Explicitly "meters". + - `coordinate_frame`: "world_from_cam". +- **`cameras/{serial}/`**: Per-camera data. + - `intrinsics`: 3x3 camera matrix. + - `resolution`: [width, height]. + - `pooled_depth`: Aggregated depth map (gzip compressed, level 4). + - `raw_frames/`: (Optional) Individual frames if pooling wasn't used. + +This structure allows `refine_ground_plane.py` to load pre-processed depth maps without needing the original SVO files or re-running the ArUco detection pipeline. + +### 2. Ground Plane Refinement Pipeline + +The `refine_ground_plane.py` tool implements a robust multi-camera consensus algorithm to align the floor plane. + +**Core Algorithm (`aruco/ground_plane.py`):** + +1. **Per-Camera Plane Detection**: + - **Unprojection**: Converts the depth map to a point cloud in the *world frame* using the initial ArUco extrinsics. + - **RANSAC**: Uses Open3D's `segment_plane` to find the dominant plane. + - **Quality Gates**: + - `min_inliers`: Requires at least 500 points. + - `normal_vertical_thresh`: Normal must be roughly vertical (>0.9 dot product with Y-axis). + +2. **Robust Consensus**: + - Computes the **geometric median** of all valid plane normals and distances to reject outliers. + - **Outlier Rejection**: Discards planes deviating >15° in angle or >0.5m in distance from the median. + - **Weighted Average**: Computes the final consensus plane from the remaining inliers. + +3. **Correction Calculation**: + - Computes a rigid transform $T_{corr}$ for each camera to align its detected floor to the consensus plane (or absolute Y=0). + - **Constraints**: + - **Rotation**: Corrects only pitch and roll to align the normal. Yaw is preserved. + - **Translation**: Corrects only vertical height. X/Z positions are preserved. + - **Consensus-Relative Correction**: By default, aligns cameras to the *consensus plane* to ensure relative consistency. + - **Safety Bounds**: The correction is **rejected** if it exceeds safety limits (default: 5° rotation, 0.1m translation). + +### 3. Observed Behavior & Tuning + +**Real-World Performance:** +- **Legacy/Unstable Behavior**: In early versions (before unit standardization), the system often reported 0 corrections or attempted extreme translations (>1m) due to mm/m confusion or depth noise. +- **Hardened Behavior**: In validated runs, the system now applies small, precise corrections (e.g., max ~0.078m translation, < 1° rotation), effectively "snapping" the floor to a consistent level without disrupting the lateral calibration. + +**Why No ICP?** +Iterative Closest Point (ICP) is **not enabled** by default for ground plane alignment. +- **Reason**: ICP on featureless planar surfaces is ill-constrained; it can "slide" along the floor, introducing drift in X/Z. +- **Approach**: Plane-to-plane alignment is analytically exact for the vertical dimension and rotation, which are the only degrees of freedom we want to correct. + +**When to Escalate:** +If the ground plane refinement fails or produces large corrections (>5°), it usually indicates: +1. **Poor Initial Calibration**: The ArUco markers were moved or poorly detected. Re-run `calibrate_extrinsics.py`. +2. **Non-Planar Floor**: The floor has significant slopes or steps. +3. **Obstacles**: Large objects are occluding the floor in the depth map. diff --git a/py_workspace/refine_ground_plane.py b/py_workspace/refine_ground_plane.py index f9cd2e6..8119d9b 100644 --- a/py_workspace/refine_ground_plane.py +++ b/py_workspace/refine_ground_plane.py @@ -11,7 +11,6 @@ from aruco.ground_plane import ( refine_ground_from_depth, create_ground_diagnostic_plot, save_diagnostic_plot, - GroundPlaneMetrics, Mat44, ) from aruco.depth_save import load_depth_data @@ -39,11 +38,6 @@ from aruco.depth_save import load_depth_data type=click.Path(dir_okay=False), help="Output extrinsics JSON file.", ) -@click.option( - "--metrics-json", - type=click.Path(dir_okay=False), - help="Optional path to save metrics JSON.", -) @click.option( "--plot/--no-plot", default=True, @@ -100,7 +94,6 @@ def main( input_extrinsics: str, input_depth: str, output_extrinsics: str, - metrics_json: Optional[str], plot: bool, plot_output: Optional[str], max_rotation_deg: float, @@ -197,6 +190,8 @@ def main( # 5. Save Output Extrinsics output_data = extrinsics_data.copy() + per_camera_diagnostics = {} + for serial, T_new in new_extrinsics.items(): if serial in output_data: pose_str = " ".join(f"{x:.6f}" for x in T_new.flatten()) @@ -209,13 +204,13 @@ def main( rot_deg = float(np.rad2deg(np.arccos(cos_angle))) trans_m = float(np.linalg.norm(T_corr[:3, 3])) - output_data[serial]["ground_refine"] = { + per_camera_diagnostics[serial] = { "corrected": True, "delta_rot_deg": rot_deg, "delta_trans_m": trans_m, } else: - output_data[serial]["ground_refine"] = { + per_camera_diagnostics[serial] = { "corrected": False, "reason": "skipped_or_failed", } @@ -239,32 +234,14 @@ def main( "max_rotation_deg": metrics.rotation_deg, "max_translation_m": metrics.translation_m, }, + "per_camera": per_camera_diagnostics, } logger.info(f"Saving refined extrinsics to {output_extrinsics}") with open(output_extrinsics, "w") as f: json.dump(output_data, f, indent=4, sort_keys=True) - # 6. Save Metrics JSON (Optional) - if metrics_json: - metrics_data = { - "success": metrics.success, - "message": metrics.message, - "num_cameras_total": metrics.num_cameras_total, - "num_cameras_valid": metrics.num_cameras_valid, - "skipped_cameras": metrics.skipped_cameras, - "max_rotation_deg": metrics.rotation_deg, - "max_translation_m": metrics.translation_m, - "camera_corrections": { - s: " ".join(f"{x:.6f}" for x in T.flatten()) - for s, T in metrics.camera_corrections.items() - }, - } - logger.info(f"Saving metrics to {metrics_json}") - with open(metrics_json, "w") as f: - json.dump(metrics_data, f, indent=4) - - # 7. Generate Plot (Optional) + # 6. Generate Plot (Optional) if plot: if not plot_output: plot_output = str(Path(output_extrinsics).with_suffix(".html")) @@ -286,4 +263,4 @@ def main( if __name__ == "__main__": - main() + main() # pylint: disable=no-value-for-parameter diff --git a/py_workspace/tests/test_refine_ground_cli.py b/py_workspace/tests/test_refine_ground_cli.py index 09c4b57..9bf7308 100644 --- a/py_workspace/tests/test_refine_ground_cli.py +++ b/py_workspace/tests/test_refine_ground_cli.py @@ -85,7 +85,6 @@ def test_cli_help(): def test_refine_ground_basic(tmp_path, mock_data): extrinsics_path, depth_path = mock_data output_path = tmp_path / "refined.json" - metrics_path = tmp_path / "metrics.json" runner = CliRunner() result = runner.invoke( @@ -97,8 +96,6 @@ def test_refine_ground_basic(tmp_path, mock_data): str(depth_path), "-o", str(output_path), - "--metrics-json", - str(metrics_path), "--no-plot", "--debug", ], @@ -108,23 +105,20 @@ def test_refine_ground_basic(tmp_path, mock_data): # Check output exists assert output_path.exists() - assert metrics_path.exists() # Load output with open(output_path) as f: out_data = json.load(f) - # Check metadata assert "_meta" in out_data assert "ground_refined" in out_data["_meta"] assert out_data["_meta"]["ground_refined"]["metrics"]["success"] is True + assert "per_camera" in out_data["_meta"]["ground_refined"] + assert "1001" in out_data["_meta"]["ground_refined"]["per_camera"] + assert "1002" in out_data["_meta"]["ground_refined"]["per_camera"] - # Check metrics - with open(metrics_path) as f: - metrics = json.load(f) - - assert metrics["success"] is True - assert metrics["num_cameras_valid"] == 2 + assert "ground_refine" not in out_data["1001"] + assert "ground_refine" not in out_data["1002"] # We expect some correction because we simulated floor at Y=1.5m (cam frame) # And input extrinsics were Identity (Cam Y down = World Y down)