Hand-eye calibration

Calibrate where a wrist camera sits on the arm (eye-in-hand) with the Tsai-Lenz method, using the ChArUco board on the office world’s table. The example moves the wrist camera through 15 views of the board, varying its position and roll. Each image is paired with the arm pose at its timestamp. The solver returns the link-to-camera transform.

Use a UR e-Series or Franka arm at the office’s default spawn point.

The result is printed, not persisted. Update the camera transform in your robot description to use it. The same solver handles a fixed camera watching a board on the arm (eye-to-hand), and calibrate_intrinsics() accepts the same ChArUco frames when the camera itself needs calibrating.

Python

import math
import time

from olo import Client
from olo.spatial import Point, Pose, Quaternion
from olo.vision import CharucoBoard, HandEyeSample, camera_info_to_intrinsics

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

# The board on the office table: 7x5 squares of 40 mm with 30 mm DICT_5X5_50 markers.
BOARD = CharucoBoard(squares=(7, 5), square_m=0.04, marker_m=0.03, family="aruco_5x5_50")
BOARD_CENTRE = Point(x=0.45, y=0.43, z=-0.005)  # Printed surface relative to the default spawn.
VIEW_HEIGHT_M = 0.35
MAX_VIEW_ANGLE_RAD = math.radians(20)
MAX_CAMERA_ROLL_RAD = math.radians(30)
VIEW_COUNT = 15
DWELL_S = 0.5
MAX_REPROJECTION_PX = 2.0


def calibration_poses(camera_in_link: Pose) -> list[Pose]:
    """Aim the camera at the board from golden-angle points on a sphere cap."""
    poses: list[Pose] = []
    golden_angle = math.pi * (3 - math.sqrt(5))
    for index in range(VIEW_COUNT):
        fraction = index / (VIEW_COUNT - 1)
        polar = math.acos(1 - fraction * (1 - math.cos(MAX_VIEW_ANGLE_RAD)))
        dx = VIEW_HEIGHT_M * math.sin(polar) * math.cos(index * golden_angle)
        dy = VIEW_HEIGHT_M * math.sin(polar) * math.sin(index * golden_angle)
        dz = VIEW_HEIGHT_M * math.cos(polar)
        roll = -math.atan2(dy, math.hypot(dz, dx))
        pitch = math.atan2(dx, dz)
        camera_roll = [-MAX_CAMERA_ROLL_RAD, MAX_CAMERA_ROLL_RAD][index % 2]
        camera_pose = Pose(
            position=Point(x=BOARD_CENTRE.x + dx, y=BOARD_CENTRE.y + dy, z=BOARD_CENTRE.z + dz),
            orientation=Quaternion.from_rpy(math.pi + roll, pitch, 0.0)
            * Quaternion.from_rpy(0.0, 0.0, camera_roll),
        )
        poses.append(camera_pose * camera_in_link.inverse())
    return poses


with Client() as client:
    robot = client.robot()
    arm = robot.kinematics
    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"
    link = arm.end_effector_link  # The camera is rigid to this link; the result is expressed against it.
    intrinsics = camera_info_to_intrinsics(robot.core.get_latest(camera_info_topic))
    detector = robot.vision.charuco(BOARD)
    camera_in_link = robot.frames.lookup(link, intrinsics.frame_id).as_pose()

    samples: list[HandEyeSample] = []
    for index, pose in enumerate(calibration_poses(camera_in_link)):
        try:
            arm.move_pose(pose)
        except Exception as error:  # Unreachable views are skipped, not fatal.
            print(f"view {index}: skipped ({error})")
            continue
        time.sleep(DWELL_S)  # Let the arm and TF settle so the image and the link pose agree.
        frame = detector.detect(robot.core.get_image(rgb_topic), intrinsics=intrinsics)
        if frame.target_transform is None:
            print(f"view {index}: no board pose ({len(frame.charuco_corners)} corners)")
            continue
        if frame.target_reprojection_error_px > MAX_REPROJECTION_PX:
            print(
                f"view {index}: reprojection error {frame.target_reprojection_error_px:.2f} px"
                f" ({len(frame.charuco_corners)} corners)"
            )
            continue
        # The link pose is looked up at the image stamp, which is what makes the pair valid.
        samples.append(HandEyeSample.from_frame(frame, robot.frames, base_frame, link))
        print(f"view {index}: {len(frame.charuco_corners)} corners, {frame.target_reprojection_error_px:.2f} px")

    if len(samples) < 3:
        raise RuntimeError(f"Only {len(samples)} usable board views; need at least 3 for hand-eye calibration")
    result = client.vision.calibration.solve_hand_eye(samples, mounting="eye_in_hand")
    t = result.transform.translation
    roll, pitch, yaw = result.transform.rotation.to_rpy()
    print(
        f"{link} -> {result.transform.child_frame_id}: xyz ({t.x:.4f}, {t.y:.4f}, {t.z:.4f}) m, rpy ({roll:.4f}, {pitch:.4f}, {yaw:.4f})"
    )
    print(
        f"consistency over {len(samples)} views: max {result.max_residual_translation_m * 1000:.1f} mm,"
        f" {math.degrees(result.max_residual_rotation_rad):.2f} deg"
    )

    # Compare with what the robot description currently claims.
    current = robot.frames.lookup(link, result.transform.child_frame_id)
    delta = current.translation - result.transform.translation
    print(f"difference from the current TF mounting: {delta.magnitude() * 1000:.1f} mm")

TypeScript

import { connect } from "olo/web";
import { Point, Pose, Quaternion } from "olo/spatial";
import {
  type CharucoBoard,
  type HandEyeSample,
  cameraInfoToIntrinsics,
  handEyeSampleFromFrame,
} from "olo/vision";

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

// The board on the office table: 7x5 squares of 40 mm with 30 mm DICT_5X5_50 markers.
const BOARD: CharucoBoard = {
  squares: [7, 5],
  squareM: 0.04,
  markerM: 0.03,
  family: "aruco_5x5_50",
};
const BOARD_CENTRE = new Point(0.45, 0.43, -0.005); // Printed surface relative to the default spawn.
const VIEW_HEIGHT_M = 0.35;
const MAX_VIEW_ANGLE_RAD = (20 * Math.PI) / 180;
const MAX_CAMERA_ROLL_RAD = (30 * Math.PI) / 180;
const VIEW_COUNT = 15;
const DWELL_MS = 500;
const MAX_REPROJECTION_PX = 2.0;

const goldenAngle = Math.PI * (3 - Math.sqrt(5));

const client = connect();
const robot = await client.robot();
const arm = robot.kinematics;
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";
const link = await arm.getEndEffectorLink(); // The camera is rigid to this link; the result is expressed against it.
const intrinsics = cameraInfoToIntrinsics(
  await robot.core.getLatest(cameraInfoTopic),
);
const detector = robot.vision.charuco(BOARD);
const cameraInLink = (
  await robot.frames.lookup(link, intrinsics.frameId)
).asPose();

const samples: HandEyeSample[] = [];
for (let index = 0; index < VIEW_COUNT; index++) {
  const fraction = index / (VIEW_COUNT - 1);
  const polar = Math.acos(1 - fraction * (1 - Math.cos(MAX_VIEW_ANGLE_RAD)));
  const dx = VIEW_HEIGHT_M * Math.sin(polar) * Math.cos(index * goldenAngle);
  const dy = VIEW_HEIGHT_M * Math.sin(polar) * Math.sin(index * goldenAngle);
  const dz = VIEW_HEIGHT_M * Math.cos(polar);
  const roll = -Math.atan2(dy, Math.hypot(dz, dx));
  const pitch = Math.atan2(dx, dz);
  const cameraRoll = index % 2 === 0 ? -MAX_CAMERA_ROLL_RAD : MAX_CAMERA_ROLL_RAD;
  const cameraPose = new Pose(
    new Point(BOARD_CENTRE.x + dx, BOARD_CENTRE.y + dy, BOARD_CENTRE.z + dz),
    Quaternion.fromRpy(Math.PI + roll, pitch, 0).multiply(
      Quaternion.fromRpy(0, 0, cameraRoll),
    ),
  );
  try {
    await arm.movePose(cameraPose.multiply(cameraInLink.inverse()));
  } catch (error) {
    console.log(`view ${index}: skipped (${String(error)})`); // Unreachable views are skipped, not fatal.
    continue;
  }
  await new Promise((resolve) => setTimeout(resolve, DWELL_MS)); // Let the arm and TF settle.
  const frame = await detector.detect(await robot.core.getImage(rgbTopic), {
    intrinsics,
  });
  if (frame.targetTransform === undefined) {
    console.log(
      `view ${index}: no board pose (${frame.charucoCorners.length} corners)`,
    );
    continue;
  }
  if (frame.targetReprojectionErrorPx > MAX_REPROJECTION_PX) {
    console.log(
      `view ${index}: reprojection error ${frame.targetReprojectionErrorPx.toFixed(2)} px (${frame.charucoCorners.length} corners)`,
    );
    continue;
  }
  // The link pose is looked up at the image stamp, which is what makes the pair valid.
  samples.push(
    await handEyeSampleFromFrame(frame, {
      frames: robot.frames,
      baseFrame,
      link,
    }),
  );
  console.log(
    `view ${index}: ${frame.charucoCorners.length} corners, ${frame.targetReprojectionErrorPx.toFixed(2)} px`,
  );
}

if (samples.length < 3) {
  throw new Error(
    `Only ${samples.length} usable board views; need at least 3 for hand-eye calibration`,
  );
}
const result = await client.vision.calibration.solveHandEye(samples, {
  mounting: "eyeInHand",
});
const t = result.transform.translation;
const [roll, pitch, yaw] = result.transform.rotation.toRpy();
console.log(
  `${link} -> ${result.transform.childFrameId}: xyz (${t.x.toFixed(4)}, ${t.y.toFixed(4)}, ${t.z.toFixed(4)}) m,` +
    ` rpy (${roll.toFixed(4)}, ${pitch.toFixed(4)}, ${yaw.toFixed(4)})`,
);
const maxTranslationMm = Math.max(0, ...result.residualTranslationM) * 1000;
const maxRotationDeg =
  (Math.max(0, ...result.residualRotationRad) * 180) / Math.PI;
console.log(
  `consistency over ${samples.length} views: max ${maxTranslationMm.toFixed(1)} mm, ${maxRotationDeg.toFixed(2)} deg`,
);

// Compare with what the robot description currently claims.
const current = await robot.frames.lookup(link, result.transform.childFrameId);
const delta = current.translation.subtract(result.transform.translation);
console.log(
  `difference from the current TF mounting: ${(delta.magnitude() * 1000).toFixed(1)} mm`,
);