diff --git a/src/caliscope/cameras/camera_array.py b/src/caliscope/cameras/camera_array.py index 2d8bb36db..303bfd9ac 100644 --- a/src/caliscope/cameras/camera_array.py +++ b/src/caliscope/cameras/camera_array.py @@ -2,7 +2,6 @@ from __future__ import annotations import logging -from collections import OrderedDict from collections.abc import Mapping from dataclasses import dataclass from pathlib import Path @@ -209,88 +208,6 @@ def undistort_frame(self, frame: NDArray) -> NDArray: return cv2.remap(frame, self._remap_cache["map1"], self._remap_cache["map2"], cv2.INTER_LINEAR) - def get_display_data(self) -> OrderedDict: - # Extracting camera matrix parameters - logger.info("Extracting camera parameters... ") - logger.info(f"Matrix: {self.matrix}") - logger.info(f"Distortion = {self.distortions}") - - if self.matrix is not None: - fx, fy = self.matrix[0, 0], self.matrix[1, 1] - cx, cy = self.matrix[0, 2], self.matrix[1, 2] - else: - fx, fy = None, None - cx, cy = None, None - - def round_or_none(value, places): - if value is None: - return None - else: - return round(value, places) - - # Conditionally create the distortion dictionary based on camera model - distortion_coeffs_dict = OrderedDict() - if self.distortions is not None: - coeffs = self.distortions.ravel().tolist() - logger.info(f"Unpacking distortion coefficients: {coeffs}") - if self.fisheye: - # Fisheye model uses 4 coefficients: k1, k2, k3, k4 - k1, k2, k3, k4 = coeffs - distortion_coeffs_dict["radial_k1"] = round_or_none(k1, 2) - distortion_coeffs_dict["radial_k2"] = round_or_none(k2, 2) - distortion_coeffs_dict["radial_k3"] = round_or_none(k3, 2) - distortion_coeffs_dict["radial_k4"] = round_or_none(k4, 2) - else: - # Standard model uses 5 coefficients: k1, k2, p1, p2, k3 - k1, k2, p1, p2, k3 = coeffs - distortion_coeffs_dict["radial_k1"] = round_or_none(k1, 2) - distortion_coeffs_dict["radial_k2"] = round_or_none(k2, 2) - distortion_coeffs_dict["radial_k3"] = round_or_none(k3, 2) - distortion_coeffs_dict["tangential_p1"] = round_or_none(p1, 2) - distortion_coeffs_dict["tangential_p2"] = round_or_none(p2, 2) - else: - # If distortions are None, populate the dictionary with Nones - # to maintain a consistent structure for the UI. - if self.fisheye: - distortion_coeffs_dict = OrderedDict( - [("radial_k1", None), ("radial_k2", None), ("radial_k3", None), ("radial_k4", None)] - ) - else: - distortion_coeffs_dict = OrderedDict( - [ - ("radial_k1", None), - ("radial_k2", None), - ("radial_k3", None), - ("tangential_p1", None), - ("tangential_p2", None), - ] - ) - - # Creating the main dictionary with the correctly structured distortion info - camera_display_dict = OrderedDict( - [ - ("size", self.size), - ("RMSE", self.error), - ("Grid_Count", self.grid_count), - ("rotation_count", self.rotation_count), - ("fisheye", self.fisheye), - ( - "intrinsic_parameters", - OrderedDict( - [ - ("focal_length_x", round_or_none(fx, 2)), - ("focal_length_y", round_or_none(fy, 2)), - ("optical_center_x", round_or_none(cx, 2)), - ("optical_center_y", round_or_none(cy, 2)), - ] - ), - ), - ("distortion_coefficients", distortion_coeffs_dict), - ] - ) - - return camera_display_dict - def erase_calibration_data(self): self.error = None self.matrix = None diff --git a/src/caliscope/core/bootstrap_pose/paired_pose_network.py b/src/caliscope/core/bootstrap_pose/paired_pose_network.py index c0bbb6ae9..3307b5ea9 100644 --- a/src/caliscope/core/bootstrap_pose/paired_pose_network.py +++ b/src/caliscope/core/bootstrap_pose/paired_pose_network.py @@ -192,7 +192,7 @@ def apply_to(self, camera_array: CameraArray, anchor_cam: int | None = None) -> # Find largest connected component (Legacy behavior used this to filter main group) main_group_cam_ids = self._find_largest_connected_component(cam_ids) - if anchor_cam: + if anchor_cam is not None: error_score, best_cameras_config = self._build_anchored_config(camera_array, anchor_cam) else: anchor_cam, best_cameras_config = self.get_best_anchored_camera_array(main_group_cam_ids, camera_array) diff --git a/src/caliscope/core/capture_volume.py b/src/caliscope/core/capture_volume.py index f42e70e26..291fa25ba 100644 --- a/src/caliscope/core/capture_volume.py +++ b/src/caliscope/core/capture_volume.py @@ -39,7 +39,7 @@ import pandas as pd -logger = logging.getLogger(__file__) +logger = logging.getLogger(__name__) @dataclass(frozen=True) @@ -1182,6 +1182,11 @@ def oriented(self, up: dict[int, NDArray]) -> "CaptureVolume": normalized to a consensus vertical. The anchor camera (lowest posed cam_id) supplies the yaw: its optical axis, projected onto the horizontal plane, becomes +Y. Scale and translation are untouched. + + Cross-camera agreement of the world-frame ups is the accuracy signal for + the vertical, so each camera's angle from the consensus and the maximum + pairwise disagreement are logged in degrees. Real rigs typically land + within roughly 1-3 degrees per camera. """ if not up: raise ValueError("oriented() requires at least one up vector.") @@ -1200,6 +1205,24 @@ def oriented(self, up: dict[int, NDArray]) -> "CaptureVolume": raise ValueError("Consensus up vector is degenerate (per-camera verticals cancel).") consensus_up = consensus / norm + unit_ups = [w / np.linalg.norm(w) for w in world_ups] + angles_deg = { + cam_id: float(np.degrees(np.arccos(np.clip(np.dot(u, consensus_up), -1.0, 1.0)))) + for cam_id, u in zip(up.keys(), unit_ups) + } + max_pairwise_deg = max( + ( + float(np.degrees(np.arccos(np.clip(np.dot(unit_ups[i], unit_ups[j]), -1.0, 1.0)))) + for i in range(len(unit_ups)) + for j in range(i + 1, len(unit_ups)) + ), + default=0.0, + ) + per_cam = ", ".join(f"cam {cam_id}: {deg:.2f}" for cam_id, deg in angles_deg.items()) + logger.info( + f"Vertical agreement (deg from consensus): {per_cam}; max pairwise disagreement {max_pairwise_deg:.2f}" + ) + anchor_cam = self.camera_array.cameras[self._anchor_cam_id()] assert anchor_cam.rotation is not None # _anchor_cam_id returns a posed camera # Optical axis (+Z in the camera frame) expressed in the world frame. @@ -1225,14 +1248,17 @@ def oriented(self, up: dict[int, NDArray]) -> "CaptureVolume": def grounded(self, mode: Literal["lowest_point"] = "lowest_point") -> "CaptureVolume": """Translate so the ground sits at Z=0 and the XY origin lies under the anchor camera. - ``mode="lowest_point"`` places the world point of least Z at Z=0 (call after - ``oriented()`` so Z is vertical). XY origin is set under the anchor camera - (lowest posed cam_id). No rotation or scale change. + ``mode="lowest_point"`` places the floor at Z=0, taken as a robust low + percentile (1st, as an order statistic) of world Z so a single spurious + low triangulation cannot bury the floor. On small point sets the order + statistic degrades to the exact minimum. Call after ``oriented()`` so Z + is vertical. XY origin is set under the anchor camera (lowest posed + cam_id). No rotation or scale change. """ if mode != "lowest_point": raise ValueError(f"grounded() only supports mode='lowest_point', got {mode!r}.") - min_z = float(self.world_points.df["z_coord"].min()) + min_z = float(np.percentile(self.world_points.df["z_coord"].to_numpy(), 1.0, method="lower")) anchor_center = self._camera_center(self._anchor_cam_id()) offset = np.array([anchor_center[0], anchor_center[1], min_z], dtype=np.float64) diff --git a/src/caliscope/workspace_coordinator.py b/src/caliscope/workspace_coordinator.py index f1d0fb7e5..2ab669965 100644 --- a/src/caliscope/workspace_coordinator.py +++ b/src/caliscope/workspace_coordinator.py @@ -1,5 +1,4 @@ import logging -from collections import OrderedDict from pathlib import Path from datetime import datetime from typing import Literal @@ -66,7 +65,6 @@ class WorkspaceCoordinator(QObject): persistence implementation details. """ - new_camera_data = Signal(int, OrderedDict) # cam_id, camera_display_dictionary intrinsic_target_changed = Signal() # Emitted when intrinsic target config is updated extrinsic_target_changed = Signal() # Emitted when extrinsic target config is updated capture_volume_updated = Signal() # Immediate: in-memory state changed, use for UI refresh @@ -471,13 +469,6 @@ def _add_camera_from_source(self, cam_id: int): self.camera_array.cameras[cam_id] = CameraData(cam_id=cam_id, size=size) self.camera_repository.save(self.camera_array) - def push_camera_data(self, cam_id): - """Emit signal with updated camera display data.""" - logger.info(f"Pushing camera data for cam_id {cam_id}") - camera_display_data = self.camera_array.cameras[cam_id].get_display_data() - logger.info(f"camera display data is {camera_display_data}") - self.new_camera_data.emit(cam_id, camera_display_data) - def create_intrinsic_presenter(self, cam_id: int) -> IntrinsicCalibrationPresenter: """Create presenter for intrinsic calibration of a single camera. @@ -659,7 +650,6 @@ def persist_camera_rotation(self, cam_id: int, rotation_count: int) -> None: self.camera_array.cameras[cam_id].rotation_count = rotation_count self.camera_repository.save(self.camera_array) - self.push_camera_data(cam_id) logger.debug(f"Persisted camera rotation: cam_id {cam_id} -> {rotation_count * 90}°") def persist_intrinsic_calibration( @@ -672,7 +662,6 @@ def persist_intrinsic_calibration( Updates the in-memory camera array and saves to disk. Also caches the calibration report for overlay restoration when switching cameras and persists it to disk for reload on app restart. - Emits new_camera_data signal so UI components can update their display. Args: output: Complete calibration output with camera and report @@ -694,7 +683,6 @@ def persist_intrinsic_calibration( self._intrinsic_points[cam_id] = collected_points logger.info(f"Persisted intrinsic calibration for cam_id {cam_id}: rmse={output.report.rmse:.3f}px") - self.push_camera_data(cam_id) self.status_changed.emit() def get_intrinsic_report(self, cam_id: int) -> IntrinsicCalibrationReport | None: diff --git a/tests/synthetic/test_epipolar_bootstrap.py b/tests/synthetic/test_epipolar_bootstrap.py index 4b26dc9ec..f2d1551b7 100644 --- a/tests/synthetic/test_epipolar_bootstrap.py +++ b/tests/synthetic/test_epipolar_bootstrap.py @@ -74,6 +74,25 @@ def _two_camera_box_scene(random_seed: int = 42) -> SyntheticScene: ) +def _gapped_cam_id_box_scene(random_seed: int = 42) -> SyntheticScene: + """Three cameras with non-contiguous cam_ids {1, 2, 5} viewing the box. + + A six-camera ring with cameras 0, 3, and 4 dropped. Canary for code that + conflates cam_id (an arbitrary name) with array index (a contiguous slot): + contiguous-id scenes let that bug pass silently. + """ + camera_array = CameraSynthesizer().add_ring(n=6, radius=2.0, height=0.5).drop_cam_ids(0, 3, 4).build() + calibration_object = box_target(width=0.4, height=0.4, depth=0.4) + trajectory = Trajectory.orbital(n_frames=20, radius=0.2, arc_extent_deg=360.0, tumble_rate=1.0) + return SyntheticScene.single( + camera_array=camera_array, + calibration_object=calibration_object, + trajectory=trajectory, + pixel_noise_sigma=0.5, + random_seed=random_seed, + ) + + @dataclass(frozen=True) class Case: run: ProductionRun @@ -92,6 +111,12 @@ def _two_cam_case() -> Case: return Case(run=run, scene=scene) +def _gapped_cam_id_case() -> Case: + scene = _gapped_cam_id_box_scene(random_seed=42) + run = run_production_pipeline(scene, image_points=_null_obj_loc(scene.image_points_noisy)) + return Case(run=run, scene=scene) + + @pytest.fixture(scope="module") def four_cam_case() -> Case: return _four_cam_case() @@ -102,6 +127,11 @@ def two_cam_case() -> Case: return _two_cam_case() +@pytest.fixture(scope="module") +def gapped_cam_id_case() -> Case: + return _gapped_cam_id_case() + + class TestFourCameraEpipolar: def test_all_cameras_posed(self, four_cam_case: Case) -> None: posed = set(four_cam_case.run.result.capture_volume.camera_array.posed_cameras) @@ -145,6 +175,22 @@ def test_pose_recovery(self, two_cam_case: Case) -> None: assert err.translation_m < 0.010, f"cam {cam_id}: translation {err.translation_m * 1000:.2f} mm > 10 mm" +class TestNonContiguousCamIds: + def test_all_cameras_posed(self, gapped_cam_id_case: Case) -> None: + posed = set(gapped_cam_id_case.run.result.capture_volume.camera_array.posed_cameras) + assert posed == {1, 2, 5} + + def test_pose_recovery(self, gapped_cam_id_case: Case) -> None: + # 0.5 deg / 10 mm: the two-camera epipolar bounds. Three cameras from a + # six-ring give sparser pair redundancy than the full 4-cam ring, so the + # looser translation bound applies. The subject under test is id handling, + # not accuracy: a cam_id/index conflation shows up as a crash or a + # grossly wrong pose, far outside these bounds. + for cam_id, err in gapped_cam_id_case.run.pose_errors.items(): + assert err.rotation_deg < 0.5, f"cam {cam_id}: rotation {err.rotation_deg:.3f} deg > 0.5 deg" + assert err.translation_m < 0.010, f"cam {cam_id}: translation {err.translation_m * 1000:.2f} mm > 10 mm" + + class TestEpipolarGates: def test_uncalibrated_intrinsics_raises(self) -> None: # Blind intrinsics (f=width/2) are geometrically fatal for the essential @@ -244,6 +290,12 @@ def _project(points_world: np.ndarray, rotation: np.ndarray, translation: np.nda TestTwoCameraEpipolar().test_pose_recovery(two) print(f" worst rot {two.run.max_rotation_deg:.4f} deg, trans {two.run.max_translation_m * 1000:.3f} mm") + print("Non-contiguous cam_ids {1, 2, 5}...") + gapped = _gapped_cam_id_case() + TestNonContiguousCamIds().test_all_cameras_posed(gapped) + TestNonContiguousCamIds().test_pose_recovery(gapped) + print(f" worst rot {gapped.run.max_rotation_deg:.4f} deg, trans {gapped.run.max_translation_m * 1000:.3f} mm") + print("Gates...") TestEpipolarGates().test_uncalibrated_intrinsics_raises() TestEpipolarGates().test_insufficient_overlap_raises() diff --git a/tests/test_capture_volume_anchoring.py b/tests/test_capture_volume_anchoring.py index 2a813cc7b..4fa2d8b92 100644 --- a/tests/test_capture_volume_anchoring.py +++ b/tests/test_capture_volume_anchoring.py @@ -7,6 +7,9 @@ from __future__ import annotations +import logging +import re + import numpy as np import pandas as pd import pytest @@ -334,6 +337,26 @@ def test_oriented_levels_known_tilt(): assert np.allclose(segment, [0.0, 0.0, 2.0], atol=1e-10) +def test_oriented_logs_cross_camera_disagreement(caplog: pytest.LogCaptureFixture): + volume, up, _ = _tilted_rig() + + # Nudge camera 2's vote by a known 6 degrees. Cameras 0 and 1 still vote the + # true vertical, so the worst pairwise disagreement is the injected angle. + axis = np.cross(up[2], np.array([1.0, 0.0, 0.0])) + axis = axis / np.linalg.norm(axis) + nudge = Rotation.from_rotvec(np.radians(6.0) * axis).as_matrix() + up[2] = nudge @ up[2] + + with caplog.at_level(logging.INFO, logger="caliscope.core.capture_volume"): + volume.oriented(up=up) + + lines = [r.message for r in caplog.records if "pairwise disagreement" in r.message] + assert lines, "expected a vertical-agreement log line" + match = re.search(r"max pairwise disagreement ([0-9.]+)", lines[0]) + assert match is not None + assert abs(float(match.group(1)) - 6.0) < 0.5 + + # --------------------------------------------------------------------------- # Grounding # --------------------------------------------------------------------------- @@ -358,6 +381,44 @@ def test_grounded_puts_floor_at_zero_and_cam0_over_origin(): assert abs(c0[1]) < 1e-10 +def test_grounded_floor_robust_to_low_outlier(): + cameras = { + 0: make_camera(0, np.eye(3), np.array([5.0, 7.0, 3.0])), + 1: make_camera(1, np.eye(3), np.array([8.0, 7.0, 3.0])), + } + # A clean band of 200 points between z=1 and z=2, plus one spurious + # triangulation far below the real floor. + band_z = np.linspace(1.0, 2.0, 200) + rows = [world_row(i, 10, (1.0, 1.0, float(z))) for i, z in enumerate(band_z)] + rows.append(world_row(999, 10, (1.0, 1.0, -3.0))) + volume = make_volume(cameras, rows) + + grounded = volume.grounded("lowest_point") + + df = grounded.world_points.df + band_min = float(df[df["sync_index"] != 999]["z_coord"].min()) + # The outlier does not drag the floor: the clean band rests at ~0, not +4. + assert abs(band_min) < 0.02 + outlier_z = float(df[df["sync_index"] == 999]["z_coord"].iloc[0]) + assert outlier_z < 0.0 + + +def test_grounded_floor_matches_min_on_clean_dense_volume(): + cameras = { + 0: make_camera(0, np.eye(3), np.array([5.0, 7.0, 3.0])), + 1: make_camera(1, np.eye(3), np.array([8.0, 7.0, 3.0])), + } + band_z = np.linspace(1.0, 2.0, 200) + rows = [world_row(i, 10, (1.0, 1.0, float(z))) for i, z in enumerate(band_z)] + volume = make_volume(cameras, rows) + + grounded = volume.grounded("lowest_point") + + # With no outlier the percentile floor sits within a point-spacing of the + # true minimum (~0.005 here). + assert abs(float(grounded.world_points.df["z_coord"].min())) < 0.02 + + def test_grounded_rejects_unknown_mode(): cameras = { 0: make_camera(0, np.eye(3), np.array([0.0, 0.0, 0.0])), diff --git a/tests/test_paired_pose_network.py b/tests/test_paired_pose_network.py index 766e54aeb..c616b9bcd 100644 --- a/tests/test_paired_pose_network.py +++ b/tests/test_paired_pose_network.py @@ -17,7 +17,8 @@ from caliscope import __root__ from caliscope.core.bootstrap_pose.paired_pose_network import PairedPoseNetwork from caliscope.core.bootstrap_pose.build_paired_pose_network import build_paired_pose_network -from caliscope.cameras.camera_array import CameraArray +from caliscope.core.bootstrap_pose.stereopairs import StereoPair +from caliscope.cameras.camera_array import CameraArray, CameraData from caliscope.core.point_data import ImagePoints logger = logging.getLogger(__name__) @@ -213,6 +214,48 @@ def test_stereopair_graph_against_gold_standard(): verify_results(paired_pose_network, gold_stereocal_all_results) +def _rotation_about_z(angle_rad: float) -> NDArray[np.float64]: + c, s = np.cos(angle_rad), np.sin(angle_rad) + return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]], dtype=np.float64) + + +def test_apply_to_respects_anchor_cam_zero(): + """Camera 0 passed as explicit anchor must land at the origin. + + The network is built so auto-selection would prefer camera 1 (lowest + total error), so this fails if anchor_cam=0 is treated as "not provided". + """ + pair_01 = StereoPair( + primary_cam_id=0, + secondary_cam_id=1, + error_score=1.0, + rotation=_rotation_about_z(np.radians(10)), + translation=np.array([1.0, 0.0, 0.0]), + ) + pair_12 = StereoPair( + primary_cam_id=1, + secondary_cam_id=2, + error_score=1.0, + rotation=_rotation_about_z(np.radians(-10)), + translation=np.array([0.0, 1.0, 0.0]), + ) + network = PairedPoseNetwork.from_raw_estimates({pair_01.pair: pair_01, pair_12.pair: pair_12}) + camera_array = CameraArray(cameras={cam_id: CameraData(cam_id=cam_id, size=(1920, 1080)) for cam_id in (0, 1, 2)}) + + network.apply_to(camera_array, anchor_cam=0) + + cam_0 = camera_array.cameras[0] + assert cam_0.rotation is not None and cam_0.translation is not None + np.testing.assert_allclose(cam_0.rotation, np.eye(3), atol=1e-12) + np.testing.assert_allclose(cam_0.translation, np.zeros(3), atol=1e-12) + + # Camera 1 is posed directly from the 0->1 pair + cam_1 = camera_array.cameras[1] + assert cam_1.rotation is not None and cam_1.translation is not None + np.testing.assert_allclose(cam_1.rotation, pair_01.rotation, atol=1e-12) + np.testing.assert_allclose(cam_1.translation, pair_01.translation, atol=1e-12) + + if __name__ == "__main__": # Allow running directly or via pytest from caliscope.logger import setup_logging