ROS 2 depth image pixel to 3D point (XYZ) with camera_info K

한국어로 읽기

What happened when I ran it

I wanted to answer one practical question: you have found a ball in a ROS 2 camera image, you know its pixel (u, v), and you have a depth image. How do you get the ball’s X, Y, Z in metres, and how do you know the answer is right?

So I built a small fake RGB-D camera in ROS 2 Jazzy. One node published a sensor_msgs/CameraInfo and depth images of a ball-sized disc, once as 32FC1 (metres) and once as 16UC1 (millimetres). A second node subscribed, found the ball, read its depth and turned the pixel into a 3D point. It used the correct formula and then several common mistakes. Every result was then projected back into the image to see whether it landed on the original pixel. This is the “reprojection check” that people often suggest as a sanity test.

The correct formula gave the exact point I had placed in the scene. The surprise was the check. Two wrong answers passed it with zero pixels of error. In one, the millimetre depth went in without converting to metres, so the ball came out about 2 km away. In the other, depth was multiplied by a unit ray, the way you might use image_geometry‘s project_pixel_to_3d_ray, and that put the ball almost 5 cm too close. Reprojection only tells you the point is on the right ray. It says nothing about how far along that ray it is.

A third result caught me out too. Reading P with K’s indices should divide by zero. In rclpy it didn’t raise at all: the message fields are numpy arrays, so the code quietly returned -inf.

Here are the raw results. The formula, the explanation and a checklist come after them.

Environment. Image ros:jazzy-ros-base (ros@sha256:066420e07f60aa18262f2479981def87ebcfcec42eefb0c0c57c4a46098348ca), ROS 2 Jazzy, Python 3.12.3, numpy 1.26.4, rclpy 7.1.12-1noble.20260902.053513, RMW rmw_fastrtps_cpp (default), no network, limits 1 CPU and 1 GiB. image_geometry and cv2 are not in this image, so the script implements the formulas itself. Both nodes (fake_rgbd_camera and ball_locator) run in one process and exchange real rclpy topics. The script experiment.py (hash 4d200e620595d44ad04f2b9e836de19d657a2b03d11e53dd89c1840e8edfb668) ran 5 times. All 5 runs exited with code 0 in 1.54 s to 1.61 s, with empty stderr. All five topics arrived in every run (discovery 0.135 s to 0.145 s). Every value below is identical in all 5 runs unless a range is given.

Scene inputs (chosen for explanation, not taken from a real camera): 640×480, fx = fy = 500, cx = 320, cy = 240, no distortion, ball centre at (0.4, −0.2, 2.0) m in the optical frame, radius 0.05 m, background wall at 4.0 m. Pixel (100, 100) is set to “no depth” (0 in 16UC1, NaN in 32FC1). Predictions were written in plan.md before the runs.

Reading CameraInfo

Check Predicted Measured
K[0], K[2], K[4], K[5] 500, 320, 500, 240 fx 500.0, cx 320.0, fy 500.0, cy 240.0; len(k) 9, len(p) 12
Type of msg.k in rclpy not predicted ndarray, dtype float64
P[0], P[2], P[5], P[6] same values as K 500.0, 320.0, 500.0, 240.0; left 3×3 of P equals K: true
P read with K’s indices 4 and 5, using the message’s numpy elements fy = P[4] = 0, division by zero fy 0.0, cy 500.0, no exception, xyz [0.4, "-inf", 2.0]
Same mistake after converting to Python floats division by zero ZeroDivisionError
CameraInfo with K[0] = 0 refused as uncalibrated refused: uncalibrated camera: K[0] == 0

In a trial run recorded in plan.md, the numpy path printed only RuntimeWarning: divide by zero encountered in scalar divide to stderr.

Finding the ball in each depth image (centroid of pixels nearer than 3.0 m)

Topic encoding / dtype Centre pixel Ball pixels Raw value at centre Metres at centre Raw background
depth_32fc1 32FC1 / float32 (420.0, 190.0) 489 2.0 2.0 4.0
depth_16uc1 16UC1 / uint16 (420.0, 190.0) 489 2000.0 2.0 4000.0

Back-projecting from either encoding after decoding to metres gave (0.4, −0.2, 2.0) for both; maximum difference 0.0 m. Plan prediction: pixel (420, 190), raw 2000, (0.4, −0.2, 2.0) m.

Correct formula vs common mistakes. “Reprojection passes” means the residual is under 0.5 px. “Z check passes” means |Z − depth in metres| < 0.005 m.

Case XYZ (m) Reprojected (u, v) Residual (px) Z − depth Reprojection Z check 3D error vs truth (m) Plan prediction
Correct: (u − cx)·Z/fx, (v − cy)·Z/fy, Z (0.4, −0.2, 2.0) (420.0, 190.0) (0.0, 0.0) 0.0 pass pass 0.0 residual 0, Z matches
Principal point left out: u·Z/fx (1.68, 0.76, 2.0) (740.0, 430.0) (320.0, 240.0) 0.0 caught pass 1.6 (740, 430), residual 320 / 240
16UC1 value used as-is (mm) for X, Y and Z (400.0, −200.0, 2000.0) (420.0, 190.0) (0.0, 0.0) 1998.0 missed caught 2047.340763 residual 0, Z 2000 ≠ 2.0
mm for X, Y but metres for Z (400.0, −200.0, 2.0) (100320.0, −49760.0) (99900.0, −49950.0) 0.0 caught pass 446.766382 u = 100320
Unit ray × depth (0.39036, −0.19518, 1.9518) (420.0, 190.0) (0.0, 0.0) −0.0482 missed caught 0.04939 (0.39036, −0.19518, 1.95180), residual 0
Unit ray rescaled so its Z equals depth (0.4, −0.2, 2.0) (420.0, 190.0) (0.0, 0.0) 0.0 pass pass 0.0 not in plan
Wrong camera’s K (610, 610, 316, 244), reprojected with the same wrong K (0.340984, −0.177049, 2.0) (420.0, 190.0) (0.0, 0.0) 0.0 missed missed 0.063322 residual 0
Wrong camera’s K, reprojected with the message’s K (0.340984, −0.177049, 2.0) (405.2459, 195.7377) (−14.7541, 5.7377) 0.0 caught pass 0.063322 non-zero residual

Depth is the Z component, not the length of the ray (depth 2.0 m)

Pixel Z from unit ray × depth Z too short by Straight-line distance √(X²+Y²+Z²) of the correct point Plan prediction
(420, 190) 1.9518 2.41 % 2.04939 m 2.41 % short
(0, 0) image corner 1.561738 21.913 % 2.56125 m 1.5617, 21.91 % short

One pixel of detection error, as X error (u moved by 1 px)

Z (m) 0.5 1.0 2.0 4.0
Measured ΔX (m) 0.001 0.002 0.004 0.008
Predicted ΔX (m) 0.001 0.002 0.004 0.008

Invalid depth at pixel (100, 100). 16UC1 raw 0 → Z 0.0 → reprojection raised ZeroDivisionError. 32FC1 raw NaN → Z NaN → reprojection returned ["nan", "nan"] with no exception. The validity mask (16UC1 ≠ 0, 32FC1 finite) skipped both. Plan prediction: divide by zero for the 16UC1 case.

A real sphere instead of a flat disc. A ray-cast sphere of radius 0.05 m centred at (0.41, −0.205, 2.05) projects its centre to pixel (420.0, 190.0). The depth image at that pixel reads 2.001205. Centre Z minus that reading: 0.048795 m. Mask centroid (420.062, 190.014), 484 pixels. Plan prediction (editor’s reasoning): about one radius, 0.05 m.

Cost. One back-projection in pure Python: 0.186 µs to 0.191 µs. The whole 640×480 image with numpy: 6.413 ms to 6.859 ms, giving 307199 valid points. Publish-to-callback latency ranged from 3.724 ms (CameraInfo) to 6.218 ms (1228800-byte 32FC1 image).

Raw stdout lines from run 1:

{"metric": "k_indices", "frame_id": "camera_color_optical_frame", "fx_k0": 500.0, "cx_k2": 320.0, "fy_k4": 500.0, "cy_k5": 240.0, "k_len": 9, "p_len": 12, "rclpy_field_type": "ndarray", "rclpy_field_dtype": "float64"}
{"metric": "p_with_k_indices", "source": "numpy_elements_from_msg", "fy_from_p4": 0.0, "cy_from_p5": 500.0, "exception": null, "xyz": [0.4, "-inf", 2.0]}
{"metric": "case", "name": "16uc1_mm_used_consistently", "xyz": [400.0, -200.0, 2000.0], "reproj_uv": [420.0, 190.0], "residual_px": [0.0, 0.0], "reprojection_pass": true, "z_minus_depth": 1998.0, "z_check_pass": false, "error_vs_truth_m": 2047.340763, "caught_by_reprojection": false, "caught_by_z_check": true}
{"metric": "case", "name": "unit_ray_times_depth", "xyz": [0.39036, -0.19518, 1.9518], "reproj_uv": [420.0, 190.0], "residual_px": [0.0, 0.0], "reprojection_pass": true, "z_minus_depth": -0.0482, "z_check_pass": false, "error_vs_truth_m": 0.04939, "caught_by_reprojection": false, "caught_by_z_check": true}
{"metric": "case", "name": "mismatched_k_reprojected_with_same_k", "xyz": [0.340984, -0.177049, 2.0], "reproj_uv": [420.0, 190.0], "residual_px": [0.0, 0.0], "reprojection_pass": true, "z_minus_depth": 0.0, "z_check_pass": true, "error_vs_truth_m": 0.063322, "caught_by_reprojection": false, "caught_by_z_check": false}

Every numeric prediction in the plan matched. The one thing the plan got wrong was how the P-index mistake fails: it predicted a division-by-zero error, and with rclpy’s numpy fields there was no error at all. The point just came back with Y = -inf. All 5 runs gave the same values. Only the timing and latency numbers moved, and those depend on the container’s CPU, so treat them as rough.

The formula: pixel + depth to X, Y, Z

The pinhole camera model maps a point (X, Y, Z) in the camera’s optical frame to a pixel:

u = fx * X / Z + cx
v = fy * Y / Z + cy

Solve it for X and Y and you get the back-projection:

X = (u - cx) * Z / fx
Y = (v - cy) * Z / fy
Z = depth at (u, v), in metres

A pixel on its own only gives you a ray, not a point. The depth image is what picks the point on that ray. REP-118 says this directly: a normal image pixel only projects to a 3D ray, but a depth image plus calibration can be turned into a point cloud. ROS’s own depth_image_proc uses the same formula: (u - center_x) * depth * constant_x with constant_x = unit_scaling / fx (conversions.hpp L55–L94).

There is no fixed “pixels per metre”

A common first instinct is to look for a DPI-style constant for the camera. A Robotics Stack Exchange question from 2015 about locating a ball with a Kinect in Gazebo started exactly there: the asker wanted the Kinect’s DPI to turn pixel coordinates into metres. That constant doesn’t exist. One pixel covers Z / fx metres, so the scale grows with distance. The one-pixel table above shows it doubling with every doubling of Z: 1 mm at 0.5 m, 8 mm at 4 m with fx = 500. At 2 m, a 0.1 m ball is only about 500 × 0.1 / 2.0 = 25 px wide. Being off by a pixel or two in detection is already a few millimetres of position error there.

Where fx, fy, cx, cy live in CameraInfo

CameraInfo.k is a float64[9], a 3×3 matrix stored row by row (CameraInfo.msg L66–L73):

    [fx  0 cx]        index:  0 1 2
K = [ 0 fy cy]                3 4 5
    [ 0  0  1]                6 7 8

fx = K[0]   cx = K[2]   fy = K[4]   cy = K[5]

P is a 3×4 matrix, also row by row, so the same values sit at different indices:

fx' = P[0]   cx' = P[2]   fy' = P[5]   cy' = P[6]

If you copy K’s indices onto P, P[4] is 0, which becomes fy, and P[5], the real fy, becomes cy. In the run, that didn’t crash. rclpy hands you msg.k and msg.p as numpy float64 arrays, and numpy division by zero gives inf with a warning instead of an exception. A node written like this keeps publishing points with Y = -inf. If you convert the fields to Python floats first ([float(c) for c in msg.p]), the same bug fails loudly with ZeroDivisionError. That’s a small reason to do the conversion.

Also check K[0] before anything else. The message definition says an uncalibrated camera leaves K zeroed and that clients may treat K[0] == 0.0 as “uncalibrated” (L21–L23). There’s nothing to compute from in that case.

K or P?

K describes the raw, distorted image. The left 3×3 of P describes the rectified image, and its values “may differ from the values in K” (L81–L105). ROS’s own tools use P. image_geometry‘s fx(), cx(), fy(), cy() all read from P (cameramodels.py L262–L291), and depth_image_proc uses that model.

The rule I’d follow: pick the matrix that matches the image your pixel came from. A pixel from an image_rect* topic goes with P. A pixel from image_raw goes with K, and if the lens distortion isn’t negligible, undistort first or detect on the rectified image. For a monocular camera with no distortion, the two are normally the same (L81–L105), as they were in this test. I didn’t test a distorted lens.

Get the depth units right first: 16UC1 vs 32FC1

REP-118 defines two depth image formats (REP-118 L23–L52):

  • 32FC1: 32-bit float, metres, depth along the camera Z axis. NaN or Inf means no reading.
  • 16UC1: 16-bit unsigned integer, millimetres. 0 means no reading.

depth_image_proc implements exactly that: uint16 is multiplied by 0.001 and 0 is invalid; float is used as-is and must be finite (depth_traits.hpp L45–L60). The RealSense ROS wrapper publishes its Z16 depth as 16UC1 (base_realsense_node.cpp L214). If your depth values look like 2000 when the object is 2 m away, that’s why.

So read msg.encoding before you read a single pixel, and decode by it. Whether one unit really is 1 mm on a given device depends on that device’s depth-units setting. I didn’t check that on hardware. A quick test is to put an object at a known distance and read the value.

Invalid pixels behave differently in the two encodings. A 0 from 16UC1 becomes Z = 0 and blows up the moment you divide by it. A NaN from 32FC1 flows through every calculation without raising anything. Mask them both before using the value.

Depth is Z, not the distance to the object

REP-118 says each depth pixel is “a depth (along the camera Z axis)”. RealSense’s projection documentation says the same thing, and spells out that distance is √(x²+y²+z²) while depth “is simply the Z-component” (RealSense projection docs).

This matters if you start from a ray. image_geometry‘s project_pixel_to_3d_ray returns a unit vector: it computes ((u − cx)/fx, (v − cy)/fy, 1) and divides by its length (cameramodels.py L131–L147). Multiply that by the depth and you’ve treated depth as ray length. Z comes out short, and more so the further the pixel is from the image centre: 2.41 % short near the ball in this test, almost 22 % short at the image corner.

If you want to use the ray, scale it so that its Z equals the depth:

ray = model.project_pixel_to_3d_ray((u, v))  # unit vector
point = [c * (depth_m / ray[2]) for c in ray]

In the run, that rescaled ray gave exactly (0.4, −0.2, 2.0), the same as the direct formula. I ran this computation with my own copy of the function, since image_geometry isn’t in the ros-base image, not with the package itself.

What a reprojection check catches, and what it misses

Projecting your computed point back to a pixel is a good test, but only of direction. The projection u = fx·X/Z + cx gives the same pixel if you multiply X, Y and Z by any common factor. Every point on the same ray reprojects to the same pixel. So:

  • Caught: mistakes that change the direction. Leaving out cx and cy moved the reprojected pixel by 320 and 240 px. Mixing millimetre X, Y with a metre Z sent it to u = 100320.
  • Missed: mistakes that only change the distance along the ray. Using millimetres consistently for all three axes and multiplying a unit ray by depth both reprojected to exactly (420, 190).
  • Missed by everything: using the wrong camera’s intrinsics everywhere. If you back-project and reproject with the same wrong K, both checks pass and the point is still about 6 cm off. Reprojecting with the K from the message you actually received does expose it (a residual of about 15 px here). This is the mistake you make when you pair a colour pixel with a depth camera’s camera_info, or the other way round.

A scale check covers the second group: confirm that the Z you computed equals the depth you decoded, in metres. That check is only as good as the decoding, though. For the millimetre case, what really catches it is reading encoding and noticing that 2000 is not a plausible range for an indoor RGB-D camera. My reading of these results is that you need three checks, each covering a different failure:

  1. Units: decode depth by msg.encoding, mask invalid pixels, and sanity-check the range.
  2. Scale: computed Z equals depth in metres.
  3. Direction: reprojection with the K (or P) from the camera_info that belongs to the image the pixel came from lands back on the pixel.

Code: the pieces that ran in the test

These functions are taken from the experiment script without changes. They ran in the Jazzy container shown above.

import math
import numpy as np


def decode_depth(msg):
    """Return (raw array, metres array, valid mask) following REP-118 encodings."""
    if msg.encoding == "16UC1":
        raw = np.frombuffer(bytes(msg.data), dtype=np.uint16).reshape(msg.height, msg.width)
        return raw, raw.astype(np.float64) * 0.001, raw != 0
    if msg.encoding == "32FC1":
        raw = np.frombuffer(bytes(msg.data), dtype=np.float32).reshape(msg.height, msg.width)
        return raw, raw.astype(np.float64), np.isfinite(raw)
    raise ValueError(f"unsupported encoding {msg.encoding}")


def intrinsics_from_k(k):
    if k[0] == 0.0:
        raise ValueError("uncalibrated camera: K[0] == 0")
    return k[0], k[4], k[2], k[5]


def back_project(u, v, z, fx, fy, cx, cy):
    return (u - cx) * z / fx, (v - cy) * z / fy, z


def reproject(x, y, z, fx, fy, cx, cy):
    return fx * x / z + cx, fy * y / z + cy

Here is a condensed version of how the script applies them to the received messages. In the script the ball pixel comes from a depth mask, and the two checks at the end use the harness thresholds (0.5 px and 0.005 m):

k = [float(c) for c in info.k]          # Python floats: a bad index fails loudly
fx, fy, cx, cy = intrinsics_from_k(k)

raw, metres, valid = decode_depth(depth_msg)
ui, vi = int(round(u)), int(round(v))
if not valid[vi, ui]:
    ...                                  # skip: 0 in 16UC1, NaN in 32FC1
z_m = float(metres[vi, ui])

x, y, z = back_project(u, v, z_m, fx, fy, cx, cy)

ru, rv = reproject(x, y, z, fx, fy, cx, cy)
direction_ok = math.hypot(ru - u, rv - v) < 0.5
scale_ok = abs(z - z_m) < 0.005

Both depth images in the test came from the same camera as the CameraInfo. With a real camera, read the colour pixel from a depth image that is registered (aligned) to the colour camera, and use that aligned stream’s camera_info. On RealSense that is the aligned_depth_to_color/image_raw and aligned_depth_to_color/camera_info pair, published when alignment is turned on. The accepted answer on the 2015 Stack Exchange question also says registration is the key step. I didn’t run a real camera, so registration accuracy is not part of these results.

If you need every pixel, not one point, depth_image_proc‘s point cloud nodes already do this for the whole image. In plain numpy the full 640×480 image took about 6.5 ms per frame here, so doing it yourself is cheap too.

Which frame the answer is in

The result is in the camera’s optical frame: origin at the optical centre, +x to the right of the image, +y down, +z forward into the scene (CameraInfo.msg L29–L35). That’s the frame named in header.frame_id, for example camera_color_optical_frame. In this example, Y = −0.2 m means the ball is 0.2 m above the optical axis.

Robot bodies use a different convention: x forward, y left, z up (REP-103 L102–L132). Just swapping axes, the same ball would be (2.0, −0.4, 0.2) in a body-style frame at the same origin. In practice, publish the point as a PointStamped in the optical frame and let tf2 transform it into your robot’s frame. Hand-written sign flips are easy to get wrong.

One more thing for balls: the depth hits the surface

A depth pixel measures where the ray hits the surface. The surface faces the camera, so the ball’s centre is further away. With a real sphere of 0.05 m radius, the centre pixel read 2.001205 while the true centre was at Z = 2.05. That’s about 4.9 cm short, nearly one radius. If you’re grasping the ball, adding roughly the radius to Z is a reasonable correction near the optical axis. That correction is my own geometric reasoning, not something from the official docs, and I only checked it on this synthetic sphere. Pixels on the ball’s edge also tend to mix with the background in real sensors. Taking the median of valid depths in a small window around the centre is a common workaround. I didn’t test it here.

Limits of this test

  • The depth images were synthetic and the intrinsics were made up for the example. No real RGB-D camera, Gazebo or other simulator, and no robot was involved.
  • No lens distortion, depth noise, missing data at edges, or colour-to-depth registration error was modelled.
  • The ball was found by thresholding depth and taking the centroid, not by detecting it in a colour image.
  • Only ROS 2 Jazzy with rclpy was run. image_geometry wasn’t installed, so its behaviour comes from reading its source (rolling branch, which Jazzy’s docs are built from), not from running it.
  • Timing numbers depend on the container’s CPU and are only meant for rough comparison.
  • A correct-looking XYZ isn’t enough to decide that it’s safe to move a robot arm or base toward it. Nothing here was tested on real hardware.

Reproduce it

The script, plan and raw results are in experiments/rgbd-camera-pixel-depth-to-xyz-camera-info-k. These links point to the files at the commit used for this article:

  • experiment.py: both nodes, the synthetic scene, every case and check, about 340 lines
  • plan.md: predictions written before the runs, and the trial-run fixes
  • results.json: full stdout of all 5 runs and the environment record

From the directory that contains experiment.py:

docker run --rm --network none --cpus 1 --memory 1g -v /path/to/experiment-dir:/work \
  ros@sha256:066420e07f60aa18262f2479981def87ebcfcec42eefb0c0c57c4a46098348ca \
  bash -c "source /opt/ros/jazzy/setup.bash && python3 /work/experiment.py"

Each run prints one JSON object per line and finishes in under two seconds.

Sources