test: fix depth bias test configuration and tolerances

py_workspace/tests/test_depth_bias.py

@@ -388,3 +388,90 @@ def test_non_positive_depth_clamp(
     assert len(received_depths) > 0
     # The depth map passed to unproject should have NaNs where it was negative
     assert np.isnan(received_depths[-1]).all()
+
+
+def test_estimate_depth_biases_clipping(
+    mock_preprocessing, mock_scene_extraction, monkeypatch
+):
+    # Test that biases are clipped to max_abs_bias.
+    # Cam1 (Ref) at 2.0. Cam2 at 2.5 (0.5m bias).
+    # max_abs_bias = 0.3. Should be clipped to 0.3.
+
+    import aruco.ground_plane
+
+    rng = np.random.default_rng(42)
+    base_points = rng.uniform(-1, 1, (400, 3))
+    base_points[:, 2] = 2.0
+
+    def mock_unproject(depth, K, stride=1, **kwargs):
+        d = depth[0, 0]
+        if abs(d - 2.0) < 1e-3:
+            return base_points
+        elif abs(d - 2.5) < 1e-3:
+            norms = np.linalg.norm(base_points, axis=1, keepdims=True)
+            rays = base_points / norms
+            return base_points + rays * 0.5
+        return base_points
+
+    monkeypatch.setattr(aruco.ground_plane, "unproject_depth_to_points", mock_unproject)
+
+    camera_data = {
+        "cam1": create_camera_data(2.0),
+        "cam2": create_camera_data(2.5),
+    }
+    extrinsics = {k: np.eye(4) for k in camera_data}
+    floor_planes = {
+        k: FloorPlane(normal=np.array([0, 1, 0]), d=0.0) for k in camera_data
+    }
+
+    config = ICPConfig(voxel_size=0.1, max_abs_bias=0.3)
+    monkeypatch.setattr(icp_reg, "compute_overlap_xz", lambda *a, **k: 10.0)
+    monkeypatch.setattr(icp_reg, "compute_overlap_3d", lambda *a, **k: 10.0)
+
+    biases = estimate_depth_biases(
+        camera_data, extrinsics, floor_planes, config, reference_serial="cam1"
+    )
+
+    assert biases["cam1"] == 0.0
+    assert abs(biases["cam2"] - 0.3) < 1e-3  # Should be exactly clipped
+
+
+def test_estimate_depth_biases_reference_fallback(
+    mock_preprocessing, mock_scene_extraction, monkeypatch
+):
+    # Test fallback when reference_serial is invalid.
+    # Should pick first sorted camera (cam1) as reference (bias 0.0).
+
+    import aruco.ground_plane
+
+    rng = np.random.default_rng(42)
+    base_points = rng.uniform(-1, 1, (400, 3))
+    base_points[:, 2] = 2.0
+
+    def mock_unproject(depth, K, stride=1, **kwargs):
+        # Both cameras see same thing, so 0 bias relative to each other
+        return base_points
+
+    monkeypatch.setattr(aruco.ground_plane, "unproject_depth_to_points", mock_unproject)
+
+    camera_data = {
+        "cam1": create_camera_data(2.0),
+        "cam2": create_camera_data(2.0),
+    }
+    extrinsics = {k: np.eye(4) for k in camera_data}
+    floor_planes = {
+        k: FloorPlane(normal=np.array([0, 1, 0]), d=0.0) for k in camera_data
+    }
+
+    config = ICPConfig(voxel_size=0.1)
+    monkeypatch.setattr(icp_reg, "compute_overlap_xz", lambda *a, **k: 10.0)
+    monkeypatch.setattr(icp_reg, "compute_overlap_3d", lambda *a, **k: 10.0)
+
+    # Pass invalid reference
+    biases = estimate_depth_biases(
+        camera_data, extrinsics, floor_planes, config, reference_serial="invalid_cam"
+    )
+
+    # cam1 should be reference (0.0) because it's first alphabetically
+    assert biases["cam1"] == 0.0
+    assert abs(biases["cam2"]) < 0.02