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:
@@ -11,6 +11,8 @@ import os
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from dora_image_view.payload import as_float32
|
||||
|
||||
# Perceptually uniform and readable in both directions, unlike JET.
|
||||
COLORMAP = cv2.COLORMAP_TURBO
|
||||
|
||||
@@ -85,21 +87,6 @@ def resolve_range(depth_mm: np.ndarray) -> tuple[float, float]:
|
||||
return float(low), float(high)
|
||||
|
||||
|
||||
def as_float32(storage) -> np.ndarray:
|
||||
"""Read the payload as float32, whether it arrived as floats or as bytes.
|
||||
|
||||
A node that sends the map as a raw byte slice -- dora-zed does -- produces a
|
||||
UInt8 arrow array even though its manifest declares Float32, so the buffer
|
||||
has to be reinterpreted rather than converted.
|
||||
"""
|
||||
buffer = storage.to_numpy()
|
||||
|
||||
if buffer.dtype == np.uint8:
|
||||
return buffer.view(np.float32)
|
||||
|
||||
return buffer.astype(np.float32, copy=False)
|
||||
|
||||
|
||||
def _hw(metadata: dict, size: int) -> tuple[int, int]:
|
||||
shape = metadata.get("shape")
|
||||
|
||||
|
||||
@@ -0,0 +1,89 @@
|
||||
"""The pinhole camera model a depth map is interpreted through.
|
||||
|
||||
The node does not need intrinsics to draw anything -- it displays them, and
|
||||
holds them for whatever reads them next. Back-projecting a depth map into a
|
||||
point cloud is the obvious such reader: every pixel needs fx, fy, cx and cy to
|
||||
become an XYZ point, and nothing else in a dataflow carries them.
|
||||
|
||||
Two senders are handled because dora-zed publishes the same numbers twice: once
|
||||
per camera session on `camera_info`, and again in the metadata of every frame.
|
||||
The per-frame copy is what makes a viewer that started after the camera did
|
||||
still end up with intrinsics.
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
from dora_image_view.payload import as_float32
|
||||
|
||||
# [fx, fy, cx, cy], the four numbers dora-zed publishes. Also accepted is a
|
||||
# nine-element row-major K, which is how ROS spells the same model in
|
||||
# sensor_msgs/CameraInfo.
|
||||
COMPACT = 4
|
||||
K_MATRIX = 9
|
||||
|
||||
# Indices of fx, fy, cx, cy in a row-major K:
|
||||
# [fx, 0, cx,
|
||||
# 0, fy, cy,
|
||||
# 0, 0, 1]
|
||||
K_INDICES = (0, 4, 2, 5)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Intrinsics:
|
||||
"""A pinhole model: focal lengths and principal point, both in pixels."""
|
||||
|
||||
fx: float
|
||||
fy: float
|
||||
cx: float
|
||||
cy: float
|
||||
distortion: tuple[float, ...] = ()
|
||||
model: str = ""
|
||||
|
||||
def __str__(self) -> str:
|
||||
"""Render for the overlay, short enough to fit across a panel."""
|
||||
return f"fx {self.fx:.1f} fy {self.fy:.1f} cx {self.cx:.1f} cy {self.cy:.1f}"
|
||||
|
||||
|
||||
def parse(storage, metadata: dict) -> Intrinsics:
|
||||
"""Read one `camera_info` payload.
|
||||
|
||||
Accepts the four-element [fx, fy, cx, cy] dora-zed sends and the
|
||||
nine-element row-major K that ROS does.
|
||||
"""
|
||||
values = as_float32(storage)
|
||||
|
||||
if values.size >= K_MATRIX:
|
||||
fx, fy, cx, cy = (float(values[index]) for index in K_INDICES)
|
||||
elif values.size >= COMPACT:
|
||||
fx, fy, cx, cy = (float(value) for value in values[:COMPACT])
|
||||
else:
|
||||
raise RuntimeError(
|
||||
f"camera_info holds {values.size} values, "
|
||||
f"expected {COMPACT} as [fx, fy, cx, cy] or {K_MATRIX} as K",
|
||||
)
|
||||
|
||||
return Intrinsics(fx, fy, cx, cy, _distortion(metadata), _model(metadata))
|
||||
|
||||
|
||||
def from_metadata(metadata: dict) -> Intrinsics | None:
|
||||
"""Read intrinsics a sender attached to a frame, or None if it attached none."""
|
||||
try:
|
||||
fx, fy, cx, cy = (
|
||||
float(metadata["fx"]),
|
||||
float(metadata["fy"]),
|
||||
float(metadata["cx"]),
|
||||
float(metadata["cy"]),
|
||||
)
|
||||
except (KeyError, TypeError, ValueError):
|
||||
return None
|
||||
|
||||
return Intrinsics(fx, fy, cx, cy, _distortion(metadata), _model(metadata))
|
||||
|
||||
|
||||
def _distortion(metadata: dict) -> tuple[float, ...]:
|
||||
values = metadata.get("distortion") or ()
|
||||
return tuple(float(value) for value in values)
|
||||
|
||||
|
||||
def _model(metadata: dict) -> str:
|
||||
return str(metadata.get("distortion_model", ""))
|
||||
+11
-3
@@ -1,7 +1,8 @@
|
||||
"""A dora display sink: overlays bounding boxes and text on an image input.
|
||||
|
||||
`image` and `depth` each fill one panel of the window and redraw. `bbox` and
|
||||
`text` events only update stored state, which the next redraw draws.
|
||||
`image` and `depth` each fill one panel of the window and redraw. `bbox`,
|
||||
`text` and `camera_info` events only update stored state, which the next redraw
|
||||
draws.
|
||||
"""
|
||||
|
||||
import argparse
|
||||
@@ -10,7 +11,7 @@ import os
|
||||
import numpy as np
|
||||
from dora import Node
|
||||
|
||||
from dora_image_view import bbox, decode, depth
|
||||
from dora_image_view import bbox, decode, depth, intrinsics
|
||||
from dora_image_view.view import View
|
||||
|
||||
|
||||
@@ -37,6 +38,11 @@ def main() -> None:
|
||||
event_id = event["id"]
|
||||
metadata = event["metadata"]
|
||||
|
||||
# A sender that puts intrinsics on every frame keeps them current even
|
||||
# if the camera_info message itself was published before this node
|
||||
# started, which is when dora-zed sends it.
|
||||
view.intrinsics = intrinsics.from_metadata(metadata) or view.intrinsics
|
||||
|
||||
panel = _decode_panel(event_id, event["value"], metadata)
|
||||
|
||||
if panel is not None:
|
||||
@@ -44,6 +50,8 @@ def main() -> None:
|
||||
view.panels[name] = frame
|
||||
if view.show():
|
||||
break
|
||||
elif event_id == "camera_info":
|
||||
view.intrinsics = intrinsics.parse(event["value"], metadata)
|
||||
elif event_id == "bbox":
|
||||
view.bboxes = bbox.parse(event["value"], metadata.get("format", "xyxy").lower())
|
||||
elif event_id == "text":
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
"""Reading raw dora payloads, independently of what they mean."""
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def as_float32(storage) -> np.ndarray:
|
||||
"""Read a payload as float32, whether it arrived as floats or as bytes.
|
||||
|
||||
A node that sends its payload as a raw byte slice -- which is what a C++
|
||||
node does -- produces a UInt8 arrow array even though its manifest declares
|
||||
Float32, so the buffer has to be reinterpreted rather than converted.
|
||||
"""
|
||||
buffer = storage.to_numpy()
|
||||
|
||||
if buffer.dtype == np.uint8:
|
||||
return buffer.view(np.float32)
|
||||
|
||||
return buffer.astype(np.float32, copy=False)
|
||||
@@ -5,6 +5,8 @@ from dataclasses import dataclass, field
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from dora_image_view.intrinsics import Intrinsics
|
||||
|
||||
WINDOW = "Dora Node: dora-image-view"
|
||||
GREEN = (0, 255, 0)
|
||||
WHITE = (255, 255, 255)
|
||||
@@ -40,6 +42,7 @@ class View:
|
||||
bboxes: Bboxes = field(default_factory=Bboxes)
|
||||
text: str = ""
|
||||
panels: dict[str, np.ndarray] = field(default_factory=dict)
|
||||
intrinsics: Intrinsics | None = None
|
||||
|
||||
def show(self) -> bool:
|
||||
"""Draw the overlays, tile the panels, and display them.
|
||||
@@ -79,6 +82,13 @@ class View:
|
||||
# On the composite, not on a panel: the caption describes the frame as a
|
||||
# whole, and a panel it was drawn on might not be the leftmost one.
|
||||
cv2.putText(frame, self.text, (20, 20), FONT, FONT_SCALE, WHITE, 1)
|
||||
|
||||
# Showing the camera model is the point of receiving it here: a depth
|
||||
# map read through the wrong intrinsics looks entirely plausible, and
|
||||
# this is where that becomes visible.
|
||||
if self.intrinsics is not None:
|
||||
cv2.putText(frame, str(self.intrinsics), (20, 40), FONT, FONT_SCALE, WHITE, 1)
|
||||
|
||||
return frame
|
||||
|
||||
def _label(self, panel: np.ndarray, name: str) -> np.ndarray:
|
||||
|
||||
Reference in New Issue
Block a user