diff --git a/src/multicam_sim/cameras.py b/src/multicam_sim/cameras.py index d0cd3bd..600ca8f 100644 --- a/src/multicam_sim/cameras.py +++ b/src/multicam_sim/cameras.py @@ -170,6 +170,15 @@ 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. + + ``R``'s rows are the camera axes in world coordinates in the order + ``[right, down, forward]`` (OpenCV, +z forward), so the forward axis is + the third row — already unit length for an orthonormal ``R``. + """ + return np.asarray(self.rotation()[2], dtype=np.float64) + 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 2692b05..0831a87 100644 --- a/tests/test_cameras.py +++ b/tests/test_cameras.py @@ -123,3 +123,20 @@ def test_intrinsics_rejects_non_positive_size(width: int, height: int) -> None: def test_intrinsics_direct_construction_still_works() -> None: intr = Intrinsics.from_focal(100.0, 640, 480) assert (intr.width, intr.height) == (640, 480) + + +def test_camera_forward_is_unit_length() -> None: + intr = Intrinsics.from_focal(100.0, 640, 480) + eye = np.array([1.0, 2.0, 3.0]) + target = np.array([4.0, 0.0, 1.0]) + cam = Camera.look_at(0, intr, eye, target) + assert np.linalg.norm(cam.forward()) == pytest.approx(1.0) + + +def test_camera_forward_points_from_eye_to_target() -> None: + intr = Intrinsics.from_focal(100.0, 640, 480) + eye = np.array([1.0, 2.0, 3.0]) + target = np.array([4.0, 0.0, 1.0]) + cam = Camera.look_at(0, intr, eye, target) + expected = (target - eye) / np.linalg.norm(target - eye) + np.testing.assert_allclose(cam.forward(), expected, atol=1e-9)