feat(calibration): robust depth refinement pipeline with diagnostics and benchmarking

py_workspace/tests/test_depth_cli_postprocess.py

@@ -14,7 +14,10 @@ sys.path.append(str(Path(__file__).parent.parent))
 # I'll use a dynamic import or just import the module and access the function dynamically if needed,
 # but standard import is better. I'll write the test file, but I won't run it until I refactor the code.
 
-from calibrate_extrinsics import apply_depth_verify_refine_postprocess
+from calibrate_extrinsics import (
+    apply_depth_verify_refine_postprocess,
+    run_benchmark_matrix,
+)
 
 
 @pytest.fixture
@@ -38,6 +41,9 @@ def mock_dependencies():
         mock_refine_res_stats = {
             "delta_rotation_deg": 1.0,
             "delta_translation_norm_m": 0.1,
+            "success": True,
+            "nfev": 10,
+            "termination_message": "Success",
         }
         # refine returns (new_pose_matrix, stats)
         mock_refine.return_value = (np.eye(4), mock_refine_res_stats)
@@ -45,6 +51,50 @@ def mock_dependencies():
         yield mock_verify, mock_refine, mock_echo
 
 
+def test_benchmark_matrix(mock_dependencies):
+    mock_verify, mock_refine, _ = 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"}}
+
+    frame_mock = MagicMock(
+        depth_map=np.zeros((10, 10)), confidence_map=np.zeros((10, 10))
+    )
+    vf = {
+        "frame": frame_mock,
+        "ids": np.array([[1]]),
+        "frame_index": 100,
+    }
+
+    verification_frames = {serial_int: vf}
+    first_frames = {serial_int: vf}
+    marker_geometry = {1: np.zeros((4, 3))}
+    camera_matrices = {serial_int: np.eye(3)}
+
+    bench_results = run_benchmark_matrix(
+        results,
+        verification_frames,
+        first_frames,
+        marker_geometry,
+        camera_matrices,
+        depth_confidence_threshold=50,
+    )
+
+    assert serial in bench_results
+    assert "baseline" in bench_results[serial]
+    assert "robust" in bench_results[serial]
+    assert "robust+confidence" in bench_results[serial]
+    assert "robust+confidence+best-frame" in bench_results[serial]
+
+    # 4 configs * (1 verify_pre + 1 refine + 1 verify_post) = 12 calls to verify, 4 to refine
+    assert (
+        mock_verify.call_count == 8
+    )  # Wait, verify_pre and verify_post are called for each config.
+    # Actually, 4 configs * 2 verify calls = 8.
+    assert mock_refine.call_count == 4
+
+
 def test_verify_only(mock_dependencies, tmp_path):
     mock_verify, mock_refine, _ = mock_dependencies
 
@@ -75,6 +125,7 @@ def test_verify_only(mock_dependencies, tmp_path):
         camera_matrices=camera_matrices,
         verify_depth=True,
         refine_depth=False,
+        use_confidence_weights=False,
         depth_confidence_threshold=50,
         report_csv_path=None,
     )
@@ -130,6 +181,7 @@ def test_refine_depth(mock_dependencies):
         camera_matrices=camera_matrices,
         verify_depth=False,  # refine implies verify usually, but let's check logic
         refine_depth=True,
+        use_confidence_weights=False,
         depth_confidence_threshold=50,
     )
 
@@ -143,6 +195,103 @@ def test_refine_depth(mock_dependencies):
     mock_refine.assert_called_once()
 
 
+def test_refine_depth_warning_negligible_improvement(mock_dependencies):
+    mock_verify, mock_refine, mock_echo = mock_dependencies
+
+    serial = "123456"
+    results = {serial: {"pose": "1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1", "stats": {}}}
+    verification_frames = {
+        serial: {
+            "frame": MagicMock(depth_map=np.zeros((10, 10))),
+            "ids": np.array([[1]]),
+        }
+    }
+    marker_geometry = {1: np.zeros((4, 3))}
+    camera_matrices = {serial: np.eye(3)}
+
+    # RMSE stays almost same
+    res_pre = MagicMock(rmse=0.1, n_valid=10, residuals=[])
+    res_post = MagicMock(rmse=0.099999, n_valid=10, residuals=[])
+    mock_verify.side_effect = [res_pre, res_post]
+
+    # nfev > 5
+    mock_refine.return_value = (
+        np.eye(4),
+        {
+            "delta_rotation_deg": 0.0,
+            "delta_translation_norm_m": 0.0,
+            "success": True,
+            "nfev": 10,
+            "termination_message": "Converged",
+        },
+    )
+
+    apply_depth_verify_refine_postprocess(
+        results=results,
+        verification_frames=verification_frames,
+        marker_geometry=marker_geometry,
+        camera_matrices=camera_matrices,
+        verify_depth=False,
+        refine_depth=True,
+        use_confidence_weights=False,
+        depth_confidence_threshold=50,
+    )
+
+    # Check if warning was echoed
+    # "WARNING: Optimization ran for 10 steps but improvement was negligible"
+    any_negligible = any(
+        "negligible" in str(call.args[0]) for call in mock_echo.call_args_list
+    )
+    assert any_negligible
+
+
+def test_refine_depth_warning_failed_or_stalled(mock_dependencies):
+    mock_verify, mock_refine, mock_echo = mock_dependencies
+
+    serial = "123456"
+    results = {serial: {"pose": "1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1", "stats": {}}}
+    verification_frames = {
+        serial: {
+            "frame": MagicMock(depth_map=np.zeros((10, 10))),
+            "ids": np.array([[1]]),
+        }
+    }
+    marker_geometry = {1: np.zeros((4, 3))}
+    camera_matrices = {serial: np.eye(3)}
+
+    res_pre = MagicMock(rmse=0.1, n_valid=10, residuals=[])
+    res_post = MagicMock(rmse=0.1, n_valid=10, residuals=[])
+    mock_verify.side_effect = [res_pre, res_post]
+
+    # success=False
+    mock_refine.return_value = (
+        np.eye(4),
+        {
+            "delta_rotation_deg": 0.0,
+            "delta_translation_norm_m": 0.0,
+            "success": False,
+            "nfev": 1,
+            "termination_message": "Failed",
+        },
+    )
+
+    apply_depth_verify_refine_postprocess(
+        results=results,
+        verification_frames=verification_frames,
+        marker_geometry=marker_geometry,
+        camera_matrices=camera_matrices,
+        verify_depth=False,
+        refine_depth=True,
+        use_confidence_weights=False,
+        depth_confidence_threshold=50,
+    )
+
+    any_failed = any(
+        "failed or stalled" in str(call.args[0]) for call in mock_echo.call_args_list
+    )
+    assert any_failed
+
+
 def test_csv_output(mock_dependencies, tmp_path):
     mock_verify, _, _ = mock_dependencies
 
@@ -169,6 +318,7 @@ def test_csv_output(mock_dependencies, tmp_path):
         camera_matrices=camera_matrices,
         verify_depth=True,
         refine_depth=False,
+        use_confidence_weights=False,
         depth_confidence_threshold=50,
         report_csv_path=str(csv_path),
     )