"""Camera geometry and position/pixel alignment-error helpers."""
from collections.abc import Iterable
from dataclasses import dataclass
from datetime import datetime, timezone
from math import hypot
from typing import Any, Literal, Protocol
import numpy as np
from olo.core.images import Image
from olo.spatial import Point, Transform, Vector
class FrameLookup(Protocol):
"""Protocol for namespace-scoped frame lookup handles."""
def lookup(
self,
target_frame: str,
source_frame: str,
*,
at: datetime | None = None,
timeout: float = 5.0,
) -> Transform: ...
_DEPTH_U16_SCALE_M = 0.001
_AGGREGATIONS = {"median", "trimmed_mean"}
[docs]
@dataclass(frozen=True, slots=True)
class Pixel:
"""Pixel coordinate, zero-based from the top-left of the image."""
x: float
y: float
[docs]
@dataclass(frozen=True, slots=True)
class BoundingBox:
"""Half-open image bounding box with XYXY pixel coordinates."""
minimum: Pixel
maximum: Pixel
def __post_init__(self) -> None:
if self.maximum.x <= self.minimum.x or self.maximum.y <= self.minimum.y:
raise ValueError("BoundingBox maximum must be greater than minimum")
[docs]
@classmethod
def from_xyxy(cls, x_min: float, y_min: float, x_max: float, y_max: float) -> "BoundingBox":
"""Create a half-open bounding box from ``x_min, y_min, x_max, y_max``."""
return cls(minimum=Pixel(x_min, y_min), maximum=Pixel(x_max, y_max))
[docs]
@classmethod
def from_xywh(cls, x: float, y: float, width: float, height: float) -> "BoundingBox":
"""Create a half-open bounding box from ``x, y, width, height``."""
if width <= 0 or height <= 0:
raise ValueError("BoundingBox width and height must be positive")
return cls.from_xyxy(x, y, x + width, y + height)
@property
def width(self) -> float:
"""Return the box width in pixels."""
return self.maximum.x - self.minimum.x
@property
def height(self) -> float:
"""Return the box height in pixels."""
return self.maximum.y - self.minimum.y
@property
def center(self) -> Pixel:
"""Return the box centre pixel."""
return Pixel(x=(self.minimum.x + self.maximum.x) / 2.0, y=(self.minimum.y + self.maximum.y) / 2.0)
[docs]
def to_xyxy(self) -> tuple[float, float, float, float]:
"""Return ``x_min, y_min, x_max, y_max``."""
return (self.minimum.x, self.minimum.y, self.maximum.x, self.maximum.y)
[docs]
def to_xywh(self) -> tuple[float, float, float, float]:
"""Return ``x, y, width, height``."""
return (self.minimum.x, self.minimum.y, self.width, self.height)
[docs]
@dataclass(frozen=True, slots=True)
class CameraIntrinsics:
"""Pinhole camera calibration parsed from sensor_msgs/CameraInfo."""
width: int
height: int
fx: float
fy: float
cx: float
cy: float
frame_id: str = ""
stamp: datetime | None = None
distortion_model: str = ""
distortion: tuple[float, ...] = ()
[docs]
@dataclass(frozen=True, slots=True)
class PointObservation:
"""Stamped 3D point observation in the camera optical frame."""
point: Point
frame_id: str
pixel: Pixel
depth_metres: float
stamp: datetime | None = None
[docs]
@dataclass(frozen=True, slots=True)
class AlignmentError:
"""Image-space alignment error."""
x_px: float
y_px: float
distance_px: float
converged: bool
[docs]
@dataclass(frozen=True, slots=True)
class PositionError:
"""Cartesian position error."""
vector: Vector
distance: float
converged: bool
frame_id: str = ""
def _header_stamp(header: Any) -> datetime | None:
if not isinstance(header, dict):
return None
stamp = header.get("stamp")
if not isinstance(stamp, dict):
return None
sec = float(stamp.get("sec", 0))
nanosec = float(stamp.get("nanosec", 0))
return datetime.fromtimestamp(sec + nanosec / 1_000_000_000.0, tz=timezone.utc)
def _header_frame_id(header: Any) -> str:
return str(header.get("frame_id", "")) if isinstance(header, dict) else ""
def _number_array(value: Any, name: str) -> list[float]:
if not isinstance(value, list):
raise ValueError(f"{name} must be an array")
try:
return [float(item) for item in value]
except (TypeError, ValueError) as exc:
raise ValueError(f"{name} must contain only numbers") from exc
def _depth_value(
array: np.ndarray, x: int, y: int, *, min_depth_metres: float | None, max_depth_metres: float | None
) -> float | None:
value = array[y, x]
if np.issubdtype(array.dtype, np.unsignedinteger):
depth = float(value) * _DEPTH_U16_SCALE_M
else:
depth = float(value)
if not np.isfinite(depth) or depth <= 0:
return None
if min_depth_metres is not None and depth < min_depth_metres:
return None
if max_depth_metres is not None and depth > max_depth_metres:
return None
return depth
def _validate_depth_image(image: Image) -> tuple[int, int]:
if image.array.ndim != 2:
raise ValueError("Depth image must be a 2D array")
if image.encoding.lower() not in {"16uc1", "mono16", "32fc1"}:
raise ValueError(f"Unsupported depth encoding: {image.encoding!r}")
height, width = image.array.shape[:2]
return height, width
def _validate_depth_intrinsics(depth_image: Image, intrinsics: CameraIntrinsics) -> str:
height, width = _validate_depth_image(depth_image)
if width != intrinsics.width or height != intrinsics.height:
raise ValueError("Depth image dimensions do not match CameraInfo")
depth_frame = _header_frame_id(depth_image.header)
if depth_frame and intrinsics.frame_id and depth_frame != intrinsics.frame_id:
raise ValueError(f"Depth image frame {depth_frame!r} does not match CameraInfo frame {intrinsics.frame_id!r}")
return depth_frame or intrinsics.frame_id
def _finite_non_negative(value: float, name: str) -> None:
if value < 0 or not np.isfinite(value):
raise ValueError(f"{name} must be a non-negative finite number")
def _region_pixels(region: BoundingBox | np.ndarray, *, width: int, height: int) -> Iterable[tuple[int, int]]:
if isinstance(region, BoundingBox):
x_min = max(0, int(np.floor(region.minimum.x)))
y_min = max(0, int(np.floor(region.minimum.y)))
x_max = min(width, int(np.ceil(region.maximum.x)))
y_max = min(height, int(np.ceil(region.maximum.y)))
if x_min >= x_max or y_min >= y_max:
raise ValueError("BoundingBox does not overlap the image")
for y in range(y_min, y_max):
for x in range(x_min, x_max):
yield x, y
return
mask = np.asarray(region)
if mask.shape != (height, width):
raise ValueError(f"Mask shape {mask.shape} does not match depth image shape {(height, width)}")
for y, x in np.argwhere(mask.astype(bool)):
yield int(x), int(y)
def _aggregate(values: list[float], aggregation: Literal["median", "trimmed_mean"], *, trim_fraction: float = 0.1) -> float:
if aggregation not in _AGGREGATIONS:
raise ValueError(f"Unsupported aggregation: {aggregation!r}")
arr = np.asarray(values, dtype=np.float64)
if aggregation == "median":
sorted_values = np.sort(arr)
return float(sorted_values[len(sorted_values) // 2])
sorted_values = np.sort(arr)
trim = int(len(sorted_values) * trim_fraction)
if trim > 0 and trim * 2 < len(sorted_values):
sorted_values = sorted_values[trim:-trim]
return float(np.mean(sorted_values))
[docs]
def camera_info_to_intrinsics(message: dict[str, Any]) -> CameraIntrinsics:
"""Parse a sensor_msgs/CameraInfo message into pinhole intrinsics."""
k = _number_array(message.get("k", message.get("K")), "CameraInfo.k")
if len(k) != 9:
raise ValueError(f"CameraInfo.k must contain 9 values, got {len(k)}")
width = int(message["width"])
height = int(message["height"])
fx, fy, cx, cy = k[0], k[4], k[2], k[5]
if width <= 0 or height <= 0 or fx <= 0 or fy <= 0:
raise ValueError("CameraInfo has invalid dimensions or focal length")
header = message.get("header")
return CameraIntrinsics(
width=width,
height=height,
fx=fx,
fy=fy,
cx=cx,
cy=cy,
frame_id=_header_frame_id(header),
stamp=_header_stamp(header),
distortion_model=str(message.get("distortion_model", message.get("distortionModel", ""))),
distortion=tuple(_number_array(message.get("d", message.get("D", [])), "CameraInfo.d")),
)
[docs]
def sample_depth(
image: Image,
pixel: Pixel,
*,
window_size: int = 1,
min_depth_metres: float | None = None,
max_depth_metres: float | None = None,
) -> float | None:
"""Sample a depth image at a pixel, optionally using a median window."""
height, width = _validate_depth_image(image)
x = round(pixel.x)
y = round(pixel.y)
if x < 0 or y < 0 or x >= width or y >= height:
raise ValueError(f"Pixel out of image bounds: ({pixel.x}, {pixel.y}) for {width}x{height}")
if window_size < 1 or window_size % 2 == 0:
raise ValueError("window_size must be a positive odd integer")
radius = window_size // 2
values: list[float] = []
for yy in range(max(0, y - radius), min(height - 1, y + radius) + 1):
for xx in range(max(0, x - radius), min(width - 1, x + radius) + 1):
depth = _depth_value(
image.array,
xx,
yy,
min_depth_metres=min_depth_metres,
max_depth_metres=max_depth_metres,
)
if depth is not None:
values.append(depth)
if not values:
return None
values.sort()
return values[len(values) // 2]
[docs]
def sample_depth_region(
image: Image,
region: BoundingBox | np.ndarray,
*,
aggregation: Literal["median", "trimmed_mean"] = "median",
min_valid_pixels: int = 1,
min_depth_metres: float | None = None,
max_depth_metres: float | None = None,
) -> float:
"""Sample a depth image region and aggregate valid depths in metres."""
if min_valid_pixels < 1:
raise ValueError("min_valid_pixels must be at least 1")
height, width = _validate_depth_image(image)
values: list[float] = []
for x, y in _region_pixels(region, width=width, height=height):
depth = _depth_value(
image.array,
x,
y,
min_depth_metres=min_depth_metres,
max_depth_metres=max_depth_metres,
)
if depth is not None:
values.append(depth)
if len(values) < min_valid_pixels:
raise ValueError(f"Insufficient valid depth pixels: got {len(values)}, need {min_valid_pixels}")
return _aggregate(values, aggregation)
[docs]
def deproject_pixel(pixel: Pixel, depth_metres: float, intrinsics: CameraIntrinsics) -> Point:
"""Deproject a pixel and depth into the ROS optical camera frame."""
if depth_metres <= 0 or not np.isfinite(depth_metres):
raise ValueError("depth_metres must be a positive finite number")
return Point(
x=(pixel.x - intrinsics.cx) * depth_metres / intrinsics.fx,
y=(pixel.y - intrinsics.cy) * depth_metres / intrinsics.fy,
z=depth_metres,
)
[docs]
def deproject_point(pixel: Pixel, depth_metres: float, intrinsics: CameraIntrinsics) -> Point:
"""Deproject a pixel and scalar depth into the ROS optical camera frame."""
return deproject_pixel(pixel, depth_metres, intrinsics)
[docs]
def project_point(point: Point, intrinsics: CameraIntrinsics) -> Pixel:
"""Project a camera-frame point into pixel coordinates."""
if point.z <= 0 or not np.isfinite(point.z):
raise ValueError("point.z must be positive to project")
return Pixel(
x=point.x * intrinsics.fx / point.z + intrinsics.cx,
y=point.y * intrinsics.fy / point.z + intrinsics.cy,
)
[docs]
def project(point: Point, intrinsics: CameraIntrinsics) -> Pixel:
"""Project a camera-frame point into pixel coordinates."""
return project_point(point, intrinsics)
[docs]
def sample_depth_point(
depth_image: Image,
pixel: Pixel,
intrinsics: CameraIntrinsics,
*,
window_size: int = 1,
min_depth_metres: float | None = None,
max_depth_metres: float | None = None,
) -> PointObservation | None:
"""Sample and deproject a depth point, preserving the depth image observation stamp."""
frame_id = _validate_depth_intrinsics(depth_image, intrinsics)
depth = sample_depth(
depth_image,
pixel,
window_size=window_size,
min_depth_metres=min_depth_metres,
max_depth_metres=max_depth_metres,
)
if depth is None:
return None
return PointObservation(
point=deproject_pixel(pixel, depth, intrinsics),
frame_id=frame_id,
stamp=_header_stamp(depth_image.header),
pixel=pixel,
depth_metres=depth,
)
[docs]
def deproject(
depth_image: Image,
pixel: Pixel,
intrinsics: CameraIntrinsics,
*,
window_size: int = 1,
min_depth_metres: float | None = None,
max_depth_metres: float | None = None,
) -> PointObservation | None:
"""Sample and deproject a depth point, preserving frame metadata."""
return sample_depth_point(
depth_image,
pixel,
intrinsics,
window_size=window_size,
min_depth_metres=min_depth_metres,
max_depth_metres=max_depth_metres,
)
[docs]
def deproject_region(
depth_image: Image,
region: BoundingBox | np.ndarray,
intrinsics: CameraIntrinsics,
*,
aggregation: Literal["median", "trimmed_mean"] = "median",
min_valid_pixels: int = 1,
min_depth_metres: float | None = None,
max_depth_metres: float | None = None,
) -> PointObservation:
"""Deproject a depth region into an aggregated camera-frame observation."""
if min_valid_pixels < 1:
raise ValueError("min_valid_pixels must be at least 1")
frame_id = _validate_depth_intrinsics(depth_image, intrinsics)
height, width = depth_image.array.shape[:2]
points: list[Point] = []
pixels: list[Pixel] = []
depths: list[float] = []
for x, y in _region_pixels(region, width=width, height=height):
depth = _depth_value(
depth_image.array,
x,
y,
min_depth_metres=min_depth_metres,
max_depth_metres=max_depth_metres,
)
if depth is None:
continue
pixel = Pixel(float(x), float(y))
points.append(deproject_pixel(pixel, depth, intrinsics))
pixels.append(pixel)
depths.append(depth)
if len(points) < min_valid_pixels:
raise ValueError(f"Insufficient valid depth pixels: got {len(points)}, need {min_valid_pixels}")
point = Point(
x=_aggregate([p.x for p in points], aggregation),
y=_aggregate([p.y for p in points], aggregation),
z=_aggregate([p.z for p in points], aggregation),
)
pixel = Pixel(
x=_aggregate([p.x for p in pixels], aggregation),
y=_aggregate([p.y for p in pixels], aggregation),
)
return PointObservation(
point=point,
frame_id=frame_id,
stamp=_header_stamp(depth_image.header),
pixel=pixel,
depth_metres=_aggregate(depths, aggregation),
)
[docs]
def deproject_to_frame(
observation: PointObservation,
*,
frames: FrameLookup,
target_frame: str,
timeout: float = 5.0,
) -> PointObservation:
"""Transform a stamped point observation into a target frame."""
if not observation.frame_id:
raise ValueError("PointObservation must include frame_id")
transform = frames.lookup(
target_frame,
observation.frame_id,
at=observation.stamp,
timeout=timeout,
)
return PointObservation(
point=transform.apply(observation.point),
frame_id=target_frame,
pixel=observation.pixel,
depth_metres=observation.depth_metres,
stamp=observation.stamp,
)
[docs]
def pixel_alignment_error(current: Pixel, target: Pixel, *, tolerance_px: float) -> AlignmentError:
"""Return signed pixel error and convergence against a tolerance."""
_finite_non_negative(tolerance_px, "tolerance_px")
x_px = target.x - current.x
y_px = target.y - current.y
distance = hypot(x_px, y_px)
return AlignmentError(x_px=x_px, y_px=y_px, distance_px=distance, converged=distance <= tolerance_px)
def _point_and_frame(value: Point | PointObservation) -> tuple[Point, str | None]:
if isinstance(value, PointObservation):
if not value.frame_id:
raise ValueError("PointObservation must include frame_id")
return value.point, value.frame_id
if isinstance(value, Point):
return value, None
raise TypeError(f"Expected Point or PointObservation, got {type(value).__name__}")
[docs]
def position_error(
current: Point | PointObservation,
target: Point | PointObservation,
*,
tolerance_m: float,
) -> PositionError:
"""Return Cartesian ``target - current`` error and convergence."""
_finite_non_negative(tolerance_m, "tolerance_m")
current_point, current_frame = _point_and_frame(current)
target_point, target_frame = _point_and_frame(target)
if (current_frame is None) != (target_frame is None):
raise ValueError("current and target must both be stamped observations or both be bare Points")
if current_frame is not None and target_frame is not None and current_frame != target_frame:
raise ValueError(f"Point frames do not match: {current_frame!r} != {target_frame!r}")
vector = target_point - current_point
distance = vector.magnitude()
return PositionError(vector=vector, distance=distance, converged=distance <= tolerance_m, frame_id=current_frame or "")
__all__ = [
"AlignmentError",
"BoundingBox",
"CameraIntrinsics",
"FrameLookup",
"Pixel",
"PointObservation",
"PositionError",
"camera_info_to_intrinsics",
"deproject",
"deproject_pixel",
"deproject_point",
"deproject_region",
"deproject_to_frame",
"pixel_alignment_error",
"position_error",
"project",
"project_point",
"sample_depth",
"sample_depth_region",
"sample_depth_point",
]