add camera_info input for the pinhole model

Nothing in a dataflow carries fx, fy, cx and cy except the camera, and every
pixel of a depth map needs them to become a 3D point. The node now accepts
them so whatever computes that next has them to hand.

They are read from two places because dora-zed publishes them twice: the
camera_info message, and the metadata of every frame. The per-frame copy is
not redundant -- camera_info goes out once per camera session, so a viewer
started after the camera would otherwise never see it, and reading frame
metadata means wiring the input is optional even for a camera that publishes
it.

A nine-element row-major K is accepted alongside the compact [fx, fy, cx, cy],
since that is how ROS spells the same model in sensor_msgs/CameraInfo.

The model is drawn under the caption rather than only stored. A depth map read
through the wrong intrinsics looks entirely plausible, and this is where that
becomes visible.

as_float32 moves to a payload module: camera_info arrives as a raw byte slice
for the same reason the depth map does, and the reinterpretation is now shared
rather than written twice.

Verified with dora-cli 1.0.1: validate passes with four contracts injected on
each side, the dataflow runs to a clean exit, and a composed frame was
rendered and inspected with the model drawn on it.
This commit is contained in:
2026-09-15 05:09:07 -03:00
parent 0efeacd538
commit 5a0f13d4db
12 changed files with 268 additions and 33 deletions
+60 -1
View File
@@ -7,7 +7,7 @@ import numpy as np
import pytest
from PIL import Image
from dora_image_view import bbox, decode, depth
from dora_image_view import bbox, decode, depth, intrinsics
from dora_image_view.view import Bboxes, View
WIDTH, HEIGHT = 12, 8
@@ -224,3 +224,62 @@ def test_depth_range_of_an_empty_frame_does_not_divide_by_zero():
low, high = depth.resolve_range(np.zeros((2, 2), dtype=np.float32))
assert high > low
def test_camera_info_reads_the_compact_four_value_form():
values = np.array([600.0, 601.0, 320.0, 240.0], dtype=np.float32)
model = intrinsics.parse(FakeStorage(values), {"distortion": [0.1, 0.2]})
assert (model.fx, model.fy, model.cx, model.cy) == (600.0, 601.0, 320.0, 240.0)
assert model.distortion == (0.1, 0.2)
def test_camera_info_reads_a_ros_k_matrix():
# Row-major K: [fx, 0, cx, 0, fy, cy, 0, 0, 1].
k = np.array(
[600.0, 0.0, 320.0, 0.0, 601.0, 240.0, 0.0, 0.0, 1.0],
dtype=np.float32,
)
model = intrinsics.parse(FakeStorage(k), {})
assert (model.fx, model.fy, model.cx, model.cy) == (600.0, 601.0, 320.0, 240.0)
def test_camera_info_reinterprets_a_payload_sent_as_bytes():
values = np.array([600.0, 601.0, 320.0, 240.0], dtype=np.float32)
model = intrinsics.parse(FakeStorage(values.view(np.uint8)), {})
assert model.fx == 600.0
assert model.cy == 240.0
def test_camera_info_that_is_too_short_is_an_error():
with pytest.raises(RuntimeError, match="camera_info"):
intrinsics.parse(FakeStorage(np.zeros(3, dtype=np.float32)), {})
def test_intrinsics_are_read_from_frame_metadata():
model = intrinsics.from_metadata(
{"fx": 600.0, "fy": 601.0, "cx": 320.0, "cy": 240.0},
)
assert model is not None
assert (model.fx, model.cy) == (600.0, 240.0)
def test_frame_metadata_without_intrinsics_yields_none():
assert intrinsics.from_metadata({"width": 640, "height": 480}) is None
def test_the_camera_model_is_drawn_on_the_composite():
view = View(headless=True)
view.panels["image"] = np.zeros((40, 80, 3), dtype=np.uint8)
without = view.compose().copy()
view.intrinsics = intrinsics.Intrinsics(600.0, 601.0, 320.0, 240.0)
with_model = view.compose()
assert not np.array_equal(without, with_model), "the model should be drawn"