diff --git a/py_workspace/tests/test_depth_bias.py b/py_workspace/tests/test_depth_bias.py index 81c2d30..2139525 100644 --- a/py_workspace/tests/test_depth_bias.py +++ b/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