"""Calibration solvers over olo.vision.v1: Tsai-Lenz hand-eye and ChArUco intrinsics.
These consume detections; the capture workflow (moving the arm, pacing frames,
coverage) is the caller's. Results are returned, not persisted: publish them
into TF for the session or write them into your robot description yourself.
"""
from collections.abc import Sequence
from dataclasses import dataclass
from typing import TYPE_CHECKING, Literal
import grpc
from olo_protos.spatial.v1 import spatial_pb2
from olo_protos.vision.v1 import vision_pb2
from olo.errors import wrap_rpc_error
from olo.spatial import Transform
from olo.spatial._convert import transform_from_proto, transform_to_proto
from olo.vision.tags import CharucoBoard, TagFrame, family_to_proto
from olo.vision.types import CameraIntrinsics, FrameLookup
if TYPE_CHECKING:
from olo.vision.detection import Vision
_DEFAULT_TIMEOUT_SEC = 30.0
HandEyeMounting = Literal["eye_in_hand", "eye_to_hand"]
_MOUNTINGS: dict[str, vision_pb2.HandEyeMounting] = {
"eye_in_hand": vision_pb2.HAND_EYE_MOUNTING_EYE_IN_HAND,
"eye_to_hand": vision_pb2.HAND_EYE_MOUNTING_EYE_TO_HAND,
}
[docs]
@dataclass(frozen=True, slots=True)
class HandEyeSample:
"""One synchronised observation: the arm link pose and the target pose at the image stamp."""
base_to_link: Transform
camera_to_target: Transform
[docs]
@classmethod
def from_frame(
cls,
frame: TagFrame,
frames: FrameLookup,
base_frame: str,
link: str,
*,
timeout: float = 5.0,
) -> "HandEyeSample":
"""Pair a frame's target pose with the link pose looked up at that frame's stamp.
Looking TF up at the image stamp, not at call time, is what makes the
pair valid; dwell before capturing so both are still.
"""
if frame.target_transform is None:
raise ValueError("Frame has no target pose; detect with a layout or board and camera intrinsics")
base_to_link = frames.lookup(base_frame, link, at=frame.target_transform.stamp, timeout=timeout)
return cls(base_to_link=base_to_link, camera_to_target=frame.target_transform)
[docs]
@dataclass(frozen=True, slots=True)
class HandEyeResult:
"""Solved camera mounting plus per-sample consistency residuals."""
#: Eye-in-hand: link -> camera. Eye-to-hand: base -> camera.
transform: Transform
residual_translation_m: tuple[float, ...]
residual_rotation_rad: tuple[float, ...]
@property
def max_residual_translation_m(self) -> float:
"""Largest per-sample position disagreement; a quick quality gate."""
return max(self.residual_translation_m, default=0.0)
@property
def max_residual_rotation_rad(self) -> float:
"""Largest per-sample orientation disagreement."""
return max(self.residual_rotation_rad, default=0.0)
[docs]
@dataclass(frozen=True, slots=True)
class IntrinsicsResult:
"""Calibrated pinhole intrinsics with reprojection statistics."""
intrinsics: CameraIntrinsics
rms_reprojection_error_px: float
per_view_error_px: tuple[float, ...]
[docs]
class Calibration:
"""Thin wrappers over the Vision calibration RPCs."""
[docs]
def __init__(self, vision: "Vision") -> None:
"""Bind to a vision client."""
self._vision = vision
[docs]
def solve_hand_eye(
self,
samples: Sequence[HandEyeSample],
*,
mounting: HandEyeMounting = "eye_in_hand",
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> HandEyeResult:
"""Solve the camera mounting with Tsai-Lenz from at least three varied samples.
Eye-in-hand (camera on the arm, target fixed) returns link -> camera;
eye-to-hand (camera fixed, target on the arm) returns base -> camera.
Both are ready for ``frames.publish_static`` once given frame ids.
"""
if mounting not in _MOUNTINGS:
raise ValueError(f"mounting must be one of {', '.join(_MOUNTINGS)}, got {mounting!r}")
request = vision_pb2.SolveHandEyeRequest(mounting=_MOUNTINGS[mounting])
for sample in samples:
request.samples.add(
base_to_link=transform_to_proto(sample.base_to_link),
camera_to_target=transform_to_proto(sample.camera_to_target),
)
try:
response = self._vision._stub.SolveHandEye(request, timeout=timeout)
except grpc.RpcError as exc:
raise wrap_rpc_error(exc) from exc
first = samples[0] if samples else None
transform = transform_from_proto(spatial_pb2.TransformStamped(transform=response.transform))
if first is not None:
child = first.camera_to_target.frame_id
parent = first.base_to_link.child_frame_id if mounting == "eye_in_hand" else first.base_to_link.frame_id
transform = Transform(
translation=transform.translation, rotation=transform.rotation, frame_id=parent, child_frame_id=child
)
return HandEyeResult(
transform=transform,
residual_translation_m=tuple(response.residual_translation_m),
residual_rotation_rad=tuple(response.residual_rotation_rad),
)
[docs]
def calibrate_intrinsics(
self,
frames: Sequence[TagFrame],
board: CharucoBoard,
*,
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> IntrinsicsResult:
"""Calibrate pinhole intrinsics from ChArUco frames captured across varied views."""
if not frames:
raise ValueError("calibrate_intrinsics needs at least one frame")
width, height = frames[0].width, frames[0].height
request = vision_pb2.CalibrateIntrinsicsRequest(
board=board.to_proto(), family=family_to_proto(board.family), width=width, height=height
)
for frame in frames:
if (frame.width, frame.height) != (width, height):
raise ValueError("All frames must share one image size")
observation = request.observations.add()
for corner in frame.charuco_corners:
observation.corners.add(id=corner.id, pixel=vision_pb2.Pixel(x=corner.pixel.x, y=corner.pixel.y))
try:
response = self._vision._stub.CalibrateIntrinsics(request, timeout=timeout)
except grpc.RpcError as exc:
raise wrap_rpc_error(exc) from exc
k = response.intrinsics
return IntrinsicsResult(
intrinsics=CameraIntrinsics(
width=k.width,
height=k.height,
fx=k.fx,
fy=k.fy,
cx=k.cx,
cy=k.cy,
frame_id=frames[0].frame_id,
distortion_model=k.distortion_model,
distortion=tuple(k.distortion),
),
rms_reprojection_error_px=response.rms_reprojection_error_px,
per_view_error_px=tuple(response.per_view_error_px),
)
__all__ = ["Calibration", "HandEyeMounting", "HandEyeResult", "HandEyeSample", "IntrinsicsResult"]