Fiducial tag detection

Detect ArUco, AprilTag or QR markers with the appliance-side detector. The detector subscribes to the camera inside ROS and streams back compact tag frames, so image bytes never leave the appliance. Supplying the camera’s CameraInfo topic and the printed tag size adds a pose per tag, expressed in the camera optical frame and ready to re-express in any TF frame.

Corners are reported top-left, top-right, bottom-right, bottom-left in the tag’s own orientation. The tag frame has its origin at the tag centre with +x right, +y up and +z pointing out of the tag toward the camera.

The example tracks the ArUco cube on the office world’s table, just in front of the pick blocks in front of the first fixed-base spawn point. The cube is 50 mm across with one 36 mm DICT_4X4_50 marker per face: IDs 0 to 5 are the front, right, back, left, top and bottom faces. Because every marker sits flush on its face, the cube centre is 25 mm behind each visible tag along its -z axis.

from olo import Client
from olo.spatial import Point
from olo.vision import camera_info_to_intrinsics

RGB_TOPIC = "wrist_rgbd/image"
CAMERA_INFO_TOPIC = "wrist_rgbd/camera_info"

# The cube is 50 mm with one DICT_4X4_50 marker flush on each face.
TAG_FAMILY = "aruco_4x4_50"
MARKER_SIZE_M = 0.036  # Outer black border of each marker, excluding the white margin.
CUBE_HALF_SIZE_M = 0.025  # Every marker centre sits this far out from the cube centre.
FACE_BY_ID = {0: "front", 1: "right", 2: "back", 3: "left", 4: "top", 5: "bottom"}
MAX_FRAMES = 10

with Client() as client:
    robot = client.robot()
    rgb_topic = robot.core.topic(RGB_TOPIC, "sensor_msgs/msg/Image")
    camera_info_topic = robot.core.topic(CAMERA_INFO_TOPIC, "sensor_msgs/msg/CameraInfo")
    base_frame = robot.frames.base_frame() or "base_link"

    # Detection runs on the appliance next to the camera and streams poses back
    detector = robot.vision.tags(TAG_FAMILY, size_m=MARKER_SIZE_M)
    with detector.subscribe(rgb_topic, camera_info_topic) as stream:
        for index, frame in enumerate(stream):
            if index >= MAX_FRAMES:
                break
            faces = [tag for tag in frame.tags if tag.id in FACE_BY_ID and tag.transform is not None]
            if not faces:
                print(f"{frame.frame_id}: cube not in view")
                continue

            # Each tag frame has +z out of its face, so the cube centre lies
            # CUBE_HALF_SIZE_M behind every visible marker. Average the estimates.
            centres = [tag.to_frame(robot.frames, base_frame).apply(Point(z=-CUBE_HALF_SIZE_M)) for tag in faces]
            centre = Point(
                x=sum(point.x for point in centres) / len(centres),
                y=sum(point.y for point in centres) / len(centres),
                z=sum(point.z for point in centres) / len(centres),
            )
            seen = ", ".join(f"{FACE_BY_ID[tag.id]} (id {tag.id}, {tag.reprojection_error_px:.1f} px)" for tag in faces)
            print(f"Cube centre in {base_frame}: ({centre.x:.3f}, {centre.y:.3f}, {centre.z:.3f}) m from {seen}")

    # One-shot detection works on any image you already hold. Pixel corners always
    # come back; intrinsics and a marker size add a pose.
    intrinsics = camera_info_to_intrinsics(robot.core.get_latest(camera_info_topic))
    snapshot = robot.vision.detect_tags(
        robot.core.get_image(rgb_topic), TAG_FAMILY, intrinsics=intrinsics, size_m=MARKER_SIZE_M
    )
    for tag in snapshot.tags:
        line = f"snapshot: {FACE_BY_ID.get(tag.id, '?')} face (id {tag.id}) at pixel ({tag.center.x:.0f}, {tag.center.y:.0f})"
        if tag.transform is not None:
            line += f", {tag.transform.translation.z:.3f} m from the camera"
        print(line)

import { connect } from "olo/web";
import { Point } from "olo/spatial";
import { cameraInfoToIntrinsics, detectionCenter, tagToFrame } from "olo/vision";

const RGB_TOPIC = "wrist_rgbd/image";
const CAMERA_INFO_TOPIC = "wrist_rgbd/camera_info";

// The cube is 50 mm with one DICT_4X4_50 marker flush on each face.
const TAG_FAMILY = "aruco_4x4_50";
const MARKER_SIZE_M = 0.036; // Outer black border of each marker, excluding the white margin.
const CUBE_HALF_SIZE_M = 0.025; // Every marker centre sits this far out from the cube centre.
const FACE_BY_ID: Record<number, string> = { 0: "front", 1: "right", 2: "back", 3: "left", 4: "top", 5: "bottom" };
const MAX_FRAMES = 10;

const client = connect();
const robot = await client.robot();
const rgbTopic = robot.core.topic(RGB_TOPIC, "sensor_msgs/msg/Image");
const cameraInfoTopic = robot.core.topic(CAMERA_INFO_TOPIC, "sensor_msgs/msg/CameraInfo");
const baseFrame = (await robot.frames.baseFrame()) ?? "base_link";

// Detection runs on the appliance next to the camera and streams poses back
const detector = robot.vision.tags(TAG_FAMILY, { sizeM: MARKER_SIZE_M });
const stream = detector.subscribe(rgbTopic, { cameraInfoTopic });
let index = 0;
for await (const frame of stream) {
  if (index++ >= MAX_FRAMES) {
    stream.close();
    break;
  }
  const faces = frame.tags.filter((tag) => tag.id in FACE_BY_ID && tag.transform !== undefined);
  if (faces.length === 0) {
    console.log(`${frame.frameId}: cube not in view`);
    continue;
  }

  // Each tag frame has +z out of its face, so the cube centre lies
  // CUBE_HALF_SIZE_M behind every visible marker. Average the estimates.
  const centres = await Promise.all(
    faces.map(async (tag) =>
      (await tagToFrame(tag, { frames: robot.frames, targetFrame: baseFrame })).apply(
        new Point(0, 0, -CUBE_HALF_SIZE_M),
      ),
    ),
  );
  const mean = (axis: "x" | "y" | "z") => centres.reduce((sum, point) => sum + point[axis], 0) / centres.length;
  const seen = faces
    .map((tag) => `${FACE_BY_ID[tag.id]} (id ${tag.id}, ${tag.reprojectionErrorPx.toFixed(1)} px)`)
    .join(", ");
  console.log(
    `Cube centre in ${baseFrame}: (${mean("x").toFixed(3)}, ${mean("y").toFixed(3)}, ${mean("z").toFixed(3)}) m from ${seen}`,
  );
}

// One-shot detection works on any image you already hold. Pixel corners always
// come back; intrinsics and a marker size add a pose.
const intrinsics = cameraInfoToIntrinsics(await robot.core.getLatest(cameraInfoTopic));
const snapshot = await robot.vision.detectTags(await robot.core.getImage(rgbTopic), TAG_FAMILY, {
  intrinsics,
  sizeM: MARKER_SIZE_M,
});
for (const tag of snapshot.tags) {
  const center = detectionCenter(tag);
  let line = `snapshot: ${FACE_BY_ID[tag.id] ?? "?"} face (id ${tag.id}) at pixel (${center.x.toFixed(0)}, ${center.y.toFixed(0)})`;
  if (tag.transform !== undefined) {
    line += `, ${tag.transform.translation.z.toFixed(3)} m from the camera`;
  }
  console.log(line);
}