How to convert a RealSense camera pixel to a 3D point and robot coordinates

💡 How-to
, ,
1

Based on contributions by salvacmp.

For pick-and-place at varying heights, a flat-plane homography isn’t enough — you need real depth. An Intel RealSense depth camera gives you per-pixel depth, so you can deproject any pixel straight into a 3D point in camera space, then shift it into robot space.

What you need

  • An Intel RealSense camera (D400 series or similar).
  • Intel RealSense SDK 2.0 and the pyrealsense2 Python package (pip install pyrealsense2).
  • numpy.

Steps

1. Open the streams and align depth to color

Aligning depth to the color frame means every pixel in the color image has a matching depth value at the same pixel coordinates, which is the simplest way to work with the two together.

import numpy as np
import pyrealsense2 as rs

pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)

profile = pipeline.start(config)

depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()

align = rs.align(rs.stream.color)
color_intrinsics = profile.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics()

2. Grab a frame and deproject a pixel to 3D

frames = pipeline.wait_for_frames()
aligned_frames = align.process(frames)
color_frame = aligned_frames.get_color_frame()
depth_frame = aligned_frames.get_depth_frame()

if not color_frame or not depth_frame:
    raise RuntimeError("No frames received from the camera.")

color_image = np.asanyarray(color_frame.get_data())

x_center, y_center = 320, 240  # pixel coordinates to convert, e.g. from your object detector

depth = depth_frame.get_distance(x_center, y_center)
if depth == 0:
    raise ValueError("No valid depth at this pixel (hole in the depth map) - try a neighboring pixel.")

x_m, y_m, z_m = rs.rs2_deproject_pixel_to_point(color_intrinsics, [x_center, y_center], depth)
# x_m, y_m, z_m are in metres, in the camera's own coordinate frame

Because depth was aligned to color, depth_frame and color_frame share the same resolution and pixel grid, so the point can be deprojected directly with the color stream’s intrinsics — no separate depth-pixel lookup is needed.

Advanced: mapping without aligning frames

If you skip align and keep the depth stream at its native resolution, a color pixel does not map 1:1 to a depth pixel. Use rs2_project_color_pixel_to_depth_pixel to find the matching depth pixel first — this uses the original (unaligned) depth stream’s intrinsics, not the color intrinsics:

depth_min, depth_max = 0.11, 1.0  # metres -- the sensor's valid depth range
depth_intrinsics = profile.get_stream(rs.stream.depth).as_video_stream_profile().get_intrinsics()
depth_to_color_extrinsics = profile.get_stream(rs.stream.depth).as_video_stream_profile().get_extrinsics_to(profile.get_stream(rs.stream.color))
color_to_depth_extrinsics = profile.get_stream(rs.stream.color).as_video_stream_profile().get_extrinsics_to(profile.get_stream(rs.stream.depth))

depth_pixel = rs.rs2_project_color_pixel_to_depth_pixel(
    depth_frame.get_data(), depth_scale, depth_min, depth_max,
    depth_intrinsics, color_intrinsics,
    depth_to_color_extrinsics, color_to_depth_extrinsics,
    [x_center, y_center],
)
x_depth_px, y_depth_px = depth_pixel
depth = depth_frame.get_distance(int(x_depth_px), int(y_depth_px))
point = rs.rs2_deproject_pixel_to_point(depth_intrinsics, [x_depth_px, y_depth_px], depth)

3. Convert the camera-space point to robot coordinates

For the simplest case — camera axes exactly parallel to the robot’s, no rotation — this is just a translation:

camera_point = np.array([x_m, y_m, z_m])  # metres, camera frame

R = np.eye(3)  # identity if the camera's X/Y/Z axes are exactly parallel to the robot's -- edit if rotated
T = np.array([-0.15170, -0.54743, 0.0])  # translation from camera origin to robot origin, in metres -- measure for your own setup

robot_x, robot_y, robot_z = R @ camera_point + T

Common mistakes

  • depth == 0. This means a hole in the depth map (reflective/transparent surfaces, or the point being outside the sensor’s valid range) — always check for it before deprojecting, rather than letting it silently produce a (0, 0, 0) point.
  • Using the wrong intrinsics. After aligning depth to color, use the color stream’s intrinsics for deprojection (step 2). Only use the original depth stream’s intrinsics if you deliberately skip alignment (see the advanced details block).
  • Camera rotated relative to the robot. If R = np.eye(3) doesn’t hold for your setup, you need a full 3D or two-stage calibration instead — see How to do high-precision robot-camera calibration with ArUco markers or fit a 2D transform per height layer as in How to map camera coordinates to robot coordinates with a least-squares fit.
  • Mismatched resolutions. Keep the color and depth stream resolutions you enable_stream with consistent across your whole pipeline.

Related

Rewritten and consolidated (Sept 2026) from the original student how-to’s: How to convert from 2D coordinates to 3D coordinates using realsense camera.

Choosing a method to convert camera pixels to robot coordinates
How to map camera coordinates to robot coordinates with a least-squares fit
How to calibrate a camera and map pixels to world coordinates with a ChArUco board
Start here: index of all how-to articles