feat: add Plotly diagnostic visualization for ground plane refinement

py_workspace/tests/test_ground_plane.py

@@ -6,6 +6,8 @@ from aruco.ground_plane import (
     compute_consensus_plane,
     compute_floor_correction,
     refine_ground_from_depth,
+    create_ground_diagnostic_plot,
+    save_diagnostic_plot,
     FloorPlane,
     FloorCorrection,
     GroundPlaneConfig,
@@ -543,3 +545,46 @@ def test_refine_ground_from_depth_partial_success():
 
     # Cam 1 extrinsics should be changed
     assert not np.array_equal(new_extrinsics["cam1"], extrinsics["cam1"])
+
+
+def test_create_ground_diagnostic_plot_smoke():
+    # Create minimal metrics and data
+    metrics = GroundPlaneMetrics(
+        success=True,
+        consensus_plane=FloorPlane(normal=np.array([0, 1, 0]), d=1.0),
+    )
+    camera_data = {
+        "cam1": {
+            "depth": np.full((10, 10), 2.0, dtype=np.float32),
+            "K": np.eye(3),
+        }
+    }
+    extrinsics_before = {"cam1": np.eye(4)}
+    extrinsics_after = {"cam1": np.eye(4)}
+    extrinsics_after["cam1"][1, 3] = 1.0
+
+    import plotly.graph_objects as go
+
+    fig = create_ground_diagnostic_plot(
+        metrics, camera_data, extrinsics_before, extrinsics_after
+    )
+
+    assert isinstance(fig, go.Figure)
+    # Check for some expected traces
+    trace_names = [t.name for t in fig.data]
+    assert any("World X" in name for name in trace_names if name)
+    assert any("Consensus Plane" in name for name in trace_names if name)
+    assert any("Points cam1" in name for name in trace_names if name)
+    assert any("Cam cam1 (before)" in name for name in trace_names if name)
+    assert any("Cam cam1 (after)" in name for name in trace_names if name)
+
+
+def test_save_diagnostic_plot_smoke(tmp_path):
+    import plotly.graph_objects as go
+
+    fig = go.Figure()
+    plot_path = tmp_path / "diag.html"
+    save_diagnostic_plot(fig, str(plot_path))
+
+    assert plot_path.exists()
+    assert plot_path.stat().st_size > 0