From 9f30516fc9524cf1f1d092d5eb8b9fd060a11ee0 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 20 Nov 2026 13:42:30 +1000 Subject: [PATCH 1/2] feat: ROSBag depth_units returns depth images as float32 metres depth_units="auto" finds the scale for each depth image: the footer of a RealSense bag, else the unit REP 118 gives for the encoding (16UC1 is millimetres, 32FC1 is metres). A number is the scale in metres per unit. The RealSense SDK writes depth as mono16, which has no unit in REP 118, so without a footer (Intel's sample bags) "auto" does not guess: it returns the image as recorded with a RuntimeWarning. The default is unchanged. Checked against the real bags in bags/. Refs #151 Co-Authored-By: Claude Sonnet 5.5 --- src/machinevisiontoolbox/Sources.py | 99 +++++++++++++++- tests/create_ros_fixture.py | 13 ++- tests/test_ros_depth_units.py | 172 ++++++++++++++++++++++++++++ 3 files changed, 282 insertions(+), 2 deletions(-) create mode 100644 tests/test_ros_depth_units.py diff --git a/src/machinevisiontoolbox/Sources.py b/src/machinevisiontoolbox/Sources.py index 08442f07..d8be661c 100644 --- a/src/machinevisiontoolbox/Sources.py +++ b/src/machinevisiontoolbox/Sources.py @@ -197,6 +197,15 @@ def skip(n: int) -> None: return pos +# The unit of a depth image by its encoding, in metres per unit, from REP 118 +# (https://www.ros.org/reps/rep-0118.html): 16-bit depth is in millimetres, and +# 32-bit float depth is in metres. +_REP118_DEPTH_UNITS = {"16UC1": 0.001, "32FC1": 1.0} +# the encodings of a depth image: the RealSense SDK writes ``mono16``, whose unit is +# not defined by REP 118 and is found only from the footer +_DEPTH_ENCODINGS = ("16UC1", "mono16", "32FC1") + + def _footer_depth_units(footer: bytes, order: str = "<") -> float | None: """Decode trailing bytes of an image message as a depth scale, if they could be one @@ -3009,9 +3018,16 @@ class ROSBag(ImageSource): :param colororder: override the color-plane order for all image, or by topic ``{topic: colororder}`` :type colororder: str or dict or None + :param verbose: print progress and decoding problems, defaults to ``False`` + :type verbose: bool + :param depth_units: convert depth images to ``float32`` metres, see below; + ``None`` leaves them as recorded, ``"auto"`` finds the scale, a number is the + scale in metres per unit, defaults to ``None`` + :type depth_units: None, ``"auto"`` or float :param kwargs: options applied to image frames, see :func:`~machinevisiontoolbox.base.imageio.convert` :raises ImportError: if the ``rosbags`` package is not installed + :raises ValueError: if ``depth_units`` is not ``None``, ``"auto"`` or a positive number The resulting object is an iterator that yields: @@ -3033,7 +3049,34 @@ class ROSBag(ImageSource): The end of the pixel data is found from the layout of the image message, whose data array must be ``height * step`` bytes long. The extra bytes are therefore never mistaken for pixels, and bags without them (such as Intel's own sample - bags) are read correctly too. The pixel values are never changed. + bags) are read correctly too. The pixel values are not changed unless + ``depth_units`` is given, see below. + + **Depth in metres** + + A depth image is recorded as ``uint16`` (``16UC1``) in some unit, usually + millimetres, or as ``float32`` (``32FC1``) in metres. By default it is returned + as recorded. With ``depth_units`` it is returned as ``float32`` in metres, so + that the value of a pixel does not depend on the camera or driver that recorded it. + ``depth_units`` is the scale in metres per unit, as in librealsense, for example + 0.001 for millimetres. Note that this is the reciprocal of the ``depth_scale`` of + Open3D and TUM RGB-D, which are units per metre: 1000 and 5000 respectively. + + - a number is used as the scale for every depth image. A ``32FC1`` image is + already in metres, so the number is then a gain + - ``"auto"`` finds the scale for each image: the footer of a RealSense bag if it + has one, otherwise the unit that `REP 118 `_ + gives for the encoding, which is 0.001 for ``16UC1`` and 1.0 for ``32FC1``. The + RealSense SDK writes depth as ``mono16``, which REP 118 does not cover, so + without a footer (Intel's sample bags have none) the scale is not known. The + image is then returned as recorded, with a ``RuntimeWarning`` once for the + topic, rather than guess: give the number instead + + An image is taken to be a depth image if it has a footer scale, or if it is + ``16UC1``, ``mono16`` or ``32FC1`` and its topic name contains ``depth``, in any + case. Other images are never converted. A converted image has ``depth_units`` + set to 1.0, since it is now in metres. Pixels that are zero, the value for no + measurement, stay zero. The extra bytes are interpreted as a depth scale only if the bag is recognised as coming from the RealSense SDK (it has a ``realsense_msgs`` connection) and they @@ -3118,6 +3161,7 @@ class ROSBag(ImageSource): dtype: np.dtype | str | dict | None colororder: str | dict | None verbose: bool + depth_units: str | float | None args: dict _is_realsense: bool @@ -3130,8 +3174,19 @@ def __init__( dtype: np.dtype | str | dict | None = None, colororder: str | dict | None = None, verbose: bool = False, + depth_units: str | float | None = None, **kwargs: Any, ) -> None: + if depth_units is not None and depth_units != "auto": + if ( + isinstance(depth_units, (str, bool)) + or not np.isfinite(depth_units) + or depth_units <= 0 + ): + raise ValueError( + f"depth_units must be None, 'auto' or a positive number, " + f"not {depth_units!r}" + ) if not _rosbags_available: raise ImportError( "rosbags is required for ROS bag support. " @@ -3148,12 +3203,15 @@ def __init__( self.typestore = _resolve_typestore(release) self.dtype = dtype self.colororder = colororder + self.depth_units = depth_units self.args = kwargs self._tmpfile: str | None = None self._is_ros2: bool = False self._is_realsense: bool = False # connections for which unexpected bytes after an image have been reported self._warned_footers: set[int] = set() + # topics whose depth unit is not known, reported once each + self._warned_depth: set[str] = set() def __repr__(self) -> str: return ( @@ -3600,6 +3658,38 @@ def _autodetect_typestore(self, rawdata: bytes, msgtype: str) -> bool: ) return False + def _depth_scale( + self, topic: str, encoding: str | None, footer_units: float | None + ) -> float | None: + """Scale to convert an image to metres, or ``None`` to leave it as recorded. + + :param topic: the topic the image was published on + :param encoding: the ``encoding`` of the message, if it has one + :param footer_units: the scale in the footer of the message, if it has one + :return: metres per unit, or ``None`` if the image is not converted + """ + if self.depth_units is None or encoding not in _DEPTH_ENCODINGS: + return None + if footer_units is None and "depth" not in topic.lower(): + return None # not known to be a depth image + if self.depth_units != "auto": + return float(self.depth_units) + if footer_units is not None: + return footer_units + if encoding in _REP118_DEPTH_UNITS: + return _REP118_DEPTH_UNITS[encoding] + # mono16 depth with no footer: the unit is not recorded, so do not guess + if topic not in self._warned_depth: + self._warned_depth.add(topic) + warnings.warn( + f"ROSBag: the depth unit of {topic!r} ({encoding}) is not recorded in " + "the bag, so it is returned as recorded. Give depth_units= as a " + "number of metres per unit, for example 0.001 for millimetres.", + RuntimeWarning, + stacklevel=2, + ) + return None + def _image_depth_units( self, connection: Any, rawdata: bytes, footer: bytes ) -> float | None: @@ -3800,6 +3890,13 @@ def __iter__(self) -> Iterator[Any]: # Use frombuffer for zero-copy efficiency where possible data = np.frombuffer(msg.data, dtype=dtype) + scale = self._depth_scale( + connection.topic, getattr(msg, "encoding", None), depth_units + ) + if scale is not None: + # a depth image, returned in metres + data = data.astype(np.float32) * np.float32(scale) + depth_units = 1.0 # Reshape to (H, W, C) or (H, W) if channels > 1: diff --git a/tests/create_ros_fixture.py b/tests/create_ros_fixture.py index 5e24e66b..f3e06ceb 100644 --- a/tests/create_ros_fixture.py +++ b/tests/create_ros_fixture.py @@ -244,6 +244,12 @@ def realsense_depth_frame(i: int) -> np.ndarray: return (np.arange(RS_H * RS_W, dtype=np.uint16).reshape(RS_H, RS_W) * 7 + 100 * i) +def depth_pixels(i: int, encoding: str) -> np.ndarray: + """Pixels of depth frame ``i`` as recorded with depth encoding ``encoding``.""" + frame = realsense_depth_frame(i) + return frame.astype(np.float32) / 1000 if encoding == "32FC1" else frame + + def realsense_color_frame(i: int) -> np.ndarray: """Expected pixels of color frame ``i`` in a bag from :func:`make_realsense_bag`.""" return (np.arange(RS_H * RS_W * 3, dtype=np.uint8).reshape(RS_H, RS_W, 3) + 10 * i) @@ -256,6 +262,7 @@ def make_realsense_bag( wire: str = "ros1", streaminfo: bool = True, n: int = 3, + depth_encoding: str = "mono16", ) -> None: """Write a small bag like those recorded by the Intel RealSense SDK @@ -268,6 +275,10 @@ def make_realsense_bag( :param streaminfo: include a ``realsense_msgs/msg/StreamInfo`` connection, which is how the reader recognises a RealSense bag :param n: number of frames in each of the depth and color streams + :param depth_encoding: encoding of the depth stream: ``"mono16"`` as the SDK + writes it, ``"16UC1"`` as a ROS driver does (the pixels are the same), or + ``"32FC1"`` for depth in metres (the pixels are those of + :func:`realsense_depth_frame` divided by 1000) The topics, the ``mono16`` depth encoding and the layout match a real SDK recording (`bags/20260430_125558.bag`); the images are tiny. The expected pixels @@ -316,7 +327,7 @@ def footer_bytes(value: float) -> bytes: else Header(stamp=stamp, frame_id="") ) for conn, enc, pixels, scale in [ - (conn_depth, "mono16", realsense_depth_frame(i), RS_DEPTH_UNITS), + (conn_depth, depth_encoding, depth_pixels(i, depth_encoding), RS_DEPTH_UNITS), (conn_color, "rgb8", realsense_color_frame(i), 0.0), ]: msg = Image( diff --git a/tests/test_ros_depth_units.py b/tests/test_ros_depth_units.py new file mode 100644 index 00000000..9a8b0e2f --- /dev/null +++ b/tests/test_ros_depth_units.py @@ -0,0 +1,172 @@ +"""ROSBag(depth_units=...) returns depth images as float32 in metres. + +The bags are synthetic, written into a temporary directory by +``tests/create_ros_fixture.py``. Pixels that are zero (no measurement) stay zero. +""" + +from __future__ import annotations + +import warnings + +import numpy as np +import pytest + +pytest.importorskip("rosbags") + +from create_ros_fixture import ( + RS_COLOR_TOPIC, + RS_DEPTH_TOPIC, + RS_DEPTH_UNITS, + depth_pixels, + make_realsense_bag, + realsense_color_frame, + realsense_depth_frame, +) +from machinevisiontoolbox import ROSBag + + +def read(path, **kwargs): + """Depth and color images of a bag, and the RuntimeWarnings issued.""" + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + images = list(ROSBag(path, **kwargs)) + depth = [i for i in images if i.topic == RS_DEPTH_TOPIC] + color = [i for i in images if i.topic == RS_COLOR_TOPIC] + return depth, color, [w for w in caught if issubclass(w.category, RuntimeWarning)] + + +@pytest.mark.parametrize("value", [0, -1, 0.0, float("nan"), float("inf"), "metres", True]) +def test_depth_units_must_be_none_auto_or_a_positive_number(tmp_path, value): + with pytest.raises(ValueError, match="depth_units"): + ROSBag(tmp_path / "x.bag", depth_units=value) + + +def test_by_default_depth_is_returned_as_recorded(tmp_path): + make_realsense_bag(tmp_path / "rs.bag") + depth, _, caught = read(tmp_path / "rs.bag") + for k, im in enumerate(depth): + assert np.issubdtype(im.array.dtype, np.integer) # not converted + np.testing.assert_array_equal(im.array, realsense_depth_frame(k)) + assert im.depth_units == pytest.approx(RS_DEPTH_UNITS, rel=1e-6) + assert not caught + + +def test_auto_uses_the_footer_of_a_realsense_bag(tmp_path): + make_realsense_bag(tmp_path / "rs.bag") + depth, color, caught = read(tmp_path / "rs.bag", depth_units="auto") + assert len(depth) == 3 + for k, im in enumerate(depth): + assert im.array.dtype == np.float32 + np.testing.assert_allclose( + im.array, realsense_depth_frame(k) * RS_DEPTH_UNITS, rtol=1e-6 + ) + assert im.depth_units == 1.0 # now in metres + # the color stream is never touched + for k, im in enumerate(color): + assert im.array.dtype == np.uint8 + np.testing.assert_array_equal(im.array, realsense_color_frame(k)) + assert not hasattr(im, "depth_units") + assert not caught + + +def test_zero_stays_zero(tmp_path): + make_realsense_bag(tmp_path / "rs.bag") + depth, _, _ = read(tmp_path / "rs.bag", depth_units="auto") + assert depth[0].array[0, 0] == 0.0 # frame 0 starts at 0 + + +def test_a_number_overrides_the_footer(tmp_path): + make_realsense_bag(tmp_path / "rs.bag") + depth, _, _ = read(tmp_path / "rs.bag", depth_units=0.01) + np.testing.assert_allclose( + depth[1].array, realsense_depth_frame(1) * 0.01, rtol=1e-6 + ) + + +def test_mono16_without_a_footer_has_no_known_scale(tmp_path): + """Intel's sample bags have no footer, and mono16 has no unit in REP 118.""" + make_realsense_bag(tmp_path / "rs.bag", footer=False) + depth, _, caught = read(tmp_path / "rs.bag", depth_units="auto") + for k, im in enumerate(depth): # returned as recorded, not guessed + assert np.issubdtype(im.array.dtype, np.integer) + np.testing.assert_array_equal(im.array, realsense_depth_frame(k)) + assert len(caught) == 1 # once for the topic, not once for each frame + assert "not recorded" in str(caught[0].message) + assert "depth_units=" in str(caught[0].message) + + +def test_a_number_converts_mono16_without_a_footer(tmp_path): + make_realsense_bag(tmp_path / "rs.bag", footer=False) + depth, _, caught = read(tmp_path / "rs.bag", depth_units=0.001) + np.testing.assert_allclose(depth[2].array, realsense_depth_frame(2) * 0.001, rtol=1e-6) + assert not caught + + +def test_16uc1_is_in_millimetres_by_rep_118(tmp_path): + make_realsense_bag(tmp_path / "rs.bag", footer=False, depth_encoding="16UC1") + depth, _, caught = read(tmp_path / "rs.bag", depth_units="auto") + for k, im in enumerate(depth): + assert im.array.dtype == np.float32 + np.testing.assert_allclose(im.array, realsense_depth_frame(k) * 0.001, rtol=1e-6) + assert not caught + + +def test_32fc1_is_already_metres_and_a_number_is_a_gain(tmp_path): + make_realsense_bag(tmp_path / "rs.bag", footer=False, depth_encoding="32FC1") + auto, _, _ = read(tmp_path / "rs.bag", depth_units="auto") + gain, _, _ = read(tmp_path / "rs.bag", depth_units=2.0) + for k in range(3): + np.testing.assert_allclose(auto[k].array, depth_pixels(k, "32FC1"), rtol=1e-6) + np.testing.assert_allclose(gain[k].array, 2 * depth_pixels(k, "32FC1"), rtol=1e-6) + assert auto[k].array.dtype == np.float32 + + +# --------------------------------------------------------------------------- +# real bags, only in a development checkout (bags/ is not in the repository) +# --------------------------------------------------------------------------- + +import itertools +from pathlib import Path + +REAL_BAGS = Path(__file__).resolve().parent.parent / "bags" + + +def _first_depth(name: str, **kwargs): + path = REAL_BAGS / name + if not path.exists(): + pytest.skip(f"real bag {name} not available") + with warnings.catch_warnings(): + warnings.simplefilter("ignore") + for im in itertools.islice(ROSBag(str(path), **kwargs), 100): + if "depth" in im.topic.lower(): + return im + raise AssertionError(f"no depth image in {name}") + + +def test_real_realsense_bag_in_metres(): + raw = _first_depth("20260430_125558.bag") + metres = _first_depth("20260430_125558.bag", depth_units="auto") + assert metres.array.dtype == np.float32 + np.testing.assert_allclose(metres.array, raw.array * np.float32(0.001), rtol=1e-6) + + +def test_real_ros_driver_bag_is_millimetres_by_rep_118(): + raw = _first_depth("2024-12-03-05-40-15.bag") # 16UC1, no footer + metres = _first_depth("2024-12-03-05-40-15.bag", depth_units="auto") + np.testing.assert_allclose(metres.array, raw.array * np.float32(0.001), rtol=1e-6) + + +def test_real_tum_bag_is_already_metres(): + raw = _first_depth("rgbd_dataset_freiburg1_desk2.bag") # 32FC1 + metres = _first_depth("rgbd_dataset_freiburg1_desk2.bag", depth_units="auto") + np.testing.assert_array_equal(metres.array, raw.array) # NaN where equal + + +def test_real_intel_sample_bag_scale_is_not_guessed(): + path = REAL_BAGS / "single_depth_color_640x480.bag" + if not path.exists(): + pytest.skip("real bag single_depth_color_640x480.bag not available") + with pytest.warns(RuntimeWarning, match="not recorded"): + images = list(ROSBag(str(path), depth_units="auto")) + depth = [i for i in images if "Depth" in i.topic] + assert depth and all(np.issubdtype(i.array.dtype, np.integer) for i in depth) From bd1ffab4998a23c7a1f1429ca7c978c70a04d547 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 20 Nov 2026 13:50:33 +1000 Subject: [PATCH 2/2] docs: say what REP 118 is in the ROSBag depth_units docstring Co-Authored-By: Claude Sonnet 5.5 --- src/machinevisiontoolbox/Sources.py | 32 +++++++++++++++++------------ 1 file changed, 19 insertions(+), 13 deletions(-) diff --git a/src/machinevisiontoolbox/Sources.py b/src/machinevisiontoolbox/Sources.py index d8be661c..405c526e 100644 --- a/src/machinevisiontoolbox/Sources.py +++ b/src/machinevisiontoolbox/Sources.py @@ -3054,23 +3054,29 @@ class ROSBag(ImageSource): **Depth in metres** - A depth image is recorded as ``uint16`` (``16UC1``) in some unit, usually - millimetres, or as ``float32`` (``32FC1``) in metres. By default it is returned - as recorded. With ``depth_units`` it is returned as ``float32`` in metres, so - that the value of a pixel does not depend on the camera or driver that recorded it. - ``depth_units`` is the scale in metres per unit, as in librealsense, for example - 0.001 for millimetres. Note that this is the reciprocal of the ``depth_scale`` of - Open3D and TUM RGB-D, which are units per metre: 1000 and 5000 respectively. + ROS has a standard for depth images, `REP 118 `_ + (a ROS Enhancement Proposal). A depth image is either ``float32`` in metres + (``32FC1``, where NaN means no measurement) or, as a camera driver publishes it + before conversion, ``uint16`` in millimetres (``16UC1``, where 0 means no + measurement). With ``depth_units="auto"`` an image in either form is returned as + ``float32`` metres, since the standard says what the unit is. + + By default a depth image is returned as recorded. With ``depth_units`` it is + returned as ``float32`` in metres, so that the value of a pixel does not depend on + the camera or driver that recorded it. ``depth_units`` is the scale in metres per + unit, as in librealsense, for example 0.001 for millimetres. Note that this is the + reciprocal of the ``depth_scale`` of Open3D and TUM RGB-D, which are units per + metre: 1000 and 5000 respectively. - a number is used as the scale for every depth image. A ``32FC1`` image is already in metres, so the number is then a gain - ``"auto"`` finds the scale for each image: the footer of a RealSense bag if it - has one, otherwise the unit that `REP 118 `_ - gives for the encoding, which is 0.001 for ``16UC1`` and 1.0 for ``32FC1``. The - RealSense SDK writes depth as ``mono16``, which REP 118 does not cover, so - without a footer (Intel's sample bags have none) the scale is not known. The - image is then returned as recorded, with a ``RuntimeWarning`` once for the - topic, rather than guess: give the number instead + has one, otherwise the unit that REP 118 gives for the encoding, which is 0.001 + for ``16UC1`` and 1.0 for ``32FC1``. The RealSense SDK writes depth as + ``mono16``, which REP 118 does not cover, so without a footer (Intel's sample + bags have none) the scale is not known. The image is then returned as recorded, + with a ``RuntimeWarning`` once for the topic, rather than guess: give the number + instead An image is taken to be a depth image if it has a footer scale, or if it is ``16UC1``, ``mono16`` or ``32FC1`` and its topic name contains ``depth``, in any