Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
105 changes: 104 additions & 1 deletion src/machinevisiontoolbox/Sources.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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:

Expand All @@ -3033,7 +3049,40 @@ 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**

ROS has a standard for depth images, `REP 118 <https://www.ros.org/reps/rep-0118.html>`_
(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

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
Expand Down Expand Up @@ -3118,6 +3167,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

Expand All @@ -3130,8 +3180,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. "
Expand All @@ -3148,12 +3209,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 (
Expand Down Expand Up @@ -3600,6 +3664,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:
Expand Down Expand Up @@ -3800,6 +3896,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:
Expand Down
13 changes: 12 additions & 1 deletion tests/create_ros_fixture.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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

Expand All @@ -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
Expand Down Expand Up @@ -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(
Expand Down
172 changes: 172 additions & 0 deletions tests/test_ros_depth_units.py
Original file line number Diff line number Diff line change
@@ -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)
Loading