Depth camera helpers

Use calibrated RGB-D data to parse camera intrinsics, sample depth at a pixel, and deproject into the ROS optical camera frame. Deprojected points use +x right, +y down, and +z forward.

Python

from olo import Client
from olo.vision import (
    Pixel,
    camera_info_to_intrinsics,
    deproject_pixel,
    project,
    sample_depth,
    sample_depth_point,
)
# @olo-playground: autofill-sim-topics

with Client() as client:
    robot = client.robot()

    depth_topic = robot.core.topic("wrist_rgbd/depth_image", "sensor_msgs/msg/Image")
    camera_info_topic = robot.core.topic("wrist_rgbd/camera_info", "sensor_msgs/msg/CameraInfo")

    depth = robot.core.get_image(depth_topic)
    camera_info = robot.core.get_latest(camera_info_topic)
    intrinsics = camera_info_to_intrinsics(camera_info)

    # Pick a pixel of interest (e.g. from caller-owned detection).
    height, width = depth.array.shape[:2]
    pixel = Pixel(x=(width - 1) / 2.0, y=(height - 1) / 2.0)

    # sample_depth returns a depth scalar in metres; use a median window on noisy data.
    depth_metres = sample_depth(depth, pixel, window_size=3)
    if depth_metres is None:
        print(f"No valid depth at pixel ({pixel.x:.0f}, {pixel.y:.0f})")
    else:
        print(f"Depth at centre pixel: {depth_metres:.3f} m")

    # sample_depth_point combines sampling and deprojection into a stamped observation.
    observation = sample_depth_point(depth, pixel, intrinsics, window_size=3)
    if observation is None:
        print("Could not build a 3D observation for the centre pixel.")
    else:
        point = observation.point
        print(f"3D point in {observation.frame_id}: ({point.x:.3f}, {point.y:.3f}, {point.z:.3f})")

    # deproject_pixel and project are available for manual workflows.
    if depth_metres is not None:
        point = deproject_pixel(pixel, depth_metres, intrinsics)
        round_trip = project(point, intrinsics)
        print(f"Round-trip pixel: ({round_trip.x:.1f}, {round_trip.y:.1f})")

TypeScript

import { connect } from "olo/web";
import {
  cameraInfoToIntrinsics,
  deprojectPixel,
  project,
  sampleDepth,
  sampleDepthPoint,
  type Pixel,
} from "olo/vision";
// @olo-playground: autofill-sim-topics

const client = connect();
const robot = await client.robot();

const depthTopic = robot.core.topic("wrist_rgbd/depth_image", "sensor_msgs/msg/Image");
const cameraInfoTopic = robot.core.topic("wrist_rgbd/camera_info", "sensor_msgs/msg/CameraInfo");

const depth = await robot.core.getImage(depthTopic);
const cameraInfo = await robot.core.getLatest(cameraInfoTopic);
const intrinsics = cameraInfoToIntrinsics(cameraInfo);

// Pick a pixel of interest (e.g. from caller-owned detection).
const pixel: Pixel = {
  x: (depth.width - 1) / 2,
  y: (depth.height - 1) / 2,
};

// sampleDepth returns a depth scalar in metres; use a median window on noisy data.
const depthMetres = sampleDepth(depth, pixel, { windowSize: 3 });
if (depthMetres === undefined) {
  console.log(`No valid depth at pixel (${pixel.x.toFixed(0)}, ${pixel.y.toFixed(0)})`);
} else {
  console.log(`Depth at centre pixel: ${depthMetres.toFixed(3)} m`);
}

// sampleDepthPoint combines sampling and deprojection into a stamped observation.
const observation = sampleDepthPoint(depth, pixel, intrinsics, { windowSize: 3 });
if (observation === undefined) {
  console.log("Could not build a 3D observation for the centre pixel.");
} else {
  const { point, frameId } = observation;
  console.log(
    `3D point in ${frameId}: (${point.x.toFixed(3)}, ${point.y.toFixed(3)}, ${point.z.toFixed(3)})`,
  );
}

// deprojectPixel and project are available for manual workflows.
if (depthMetres !== undefined) {
  const point = deprojectPixel(pixel, depthMetres, intrinsics);
  const roundTrip = project(point, intrinsics);
  console.log(`Round-trip pixel: (${roundTrip.x.toFixed(1)}, ${roundTrip.y.toFixed(1)})`);
}