diff --git a/src/multicam_sim/cameras.py b/src/multicam_sim/cameras.py index 57cd937..d7c3f39 100644 --- a/src/multicam_sim/cameras.py +++ b/src/multicam_sim/cameras.py @@ -163,6 +163,10 @@ def centre(self) -> FloatArray: """Camera centre ``C = -R^T @ t`` in world coordinates.""" return camera_centre(self.rotation(), self.translation()) + def forward(self) -> FloatArray: + """World-space unit viewing direction (OpenCV +z / third row of ``R``).""" + return self.rotation()[2].copy() + def projection_matrix(self) -> FloatArray: """The ``3x4`` matrix ``P = K [R | t]``.""" return projection_matrix(self.intrinsics.matrix(), self.rotation(), self.translation()) diff --git a/tests/test_cameras.py b/tests/test_cameras.py index 1a4ad38..be94278 100644 --- a/tests/test_cameras.py +++ b/tests/test_cameras.py @@ -8,6 +8,18 @@ from multicam_sim import Camera, Intrinsics +def test_camera_forward_is_unit_and_matches_look_at() -> None: + intr = Intrinsics.from_fov(90.0, 640, 480) + eye = np.array([0.0, 0.0, 0.0]) + target = np.array([1.0, 0.0, 0.0]) + cam = Camera.look_at(0, intr, eye, target, np.array([0.0, 0.0, 1.0])) + forward = cam.forward() + assert forward.shape == (3,) + assert np.linalg.norm(forward) == pytest.approx(1.0, abs=1e-9) + expected = (target - eye) / np.linalg.norm(target - eye) + assert forward == pytest.approx(expected, abs=1e-9) + + def test_from_fov_acceptance_criteria() -> None: intr = Intrinsics.from_fov(90.0, 640, 480) assert intr.fx == pytest.approx(320.0, abs=1e-9)