From 40c11a5521c716bc50569670221d0c9ebcc6c274 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 20 Nov 2026 14:28:58 +1000 Subject: [PATCH] feat: depth_units turns 0 into NaN in an integer depth image In an integer depth image 0 means no measurement, and in a float one (REP 118) NaN does. A converted integer image now has NaN where it was 0, so that no measurement means the same in both and a minimum, mean or point cloud does not take it for a depth of 0 m. A 32FC1 image already uses NaN and is left alone. Checked on the real bags. Refs #151 Co-Authored-By: Claude Sonnet 5.5 --- src/machinevisiontoolbox/Sources.py | 14 ++++++--- tests/test_ros_depth_units.py | 45 +++++++++++++++++++---------- 2 files changed, 40 insertions(+), 19 deletions(-) diff --git a/src/machinevisiontoolbox/Sources.py b/src/machinevisiontoolbox/Sources.py index 405c526e..f427258f 100644 --- a/src/machinevisiontoolbox/Sources.py +++ b/src/machinevisiontoolbox/Sources.py @@ -3059,7 +3059,8 @@ class ROSBag(ImageSource): (``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. + ``float32`` metres, since the standard says what the unit is, and an integer image + has its zeros replaced by NaN, so that "no measurement" means the same in both. 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 @@ -3081,8 +3082,8 @@ class ROSBag(ImageSource): 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. + set to 1.0, since it is now in metres. In a converted integer image, pixels that + are 0 become NaN. A ``32FC1`` image already uses NaN, so only its scale changes. 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 @@ -3901,7 +3902,12 @@ def __iter__(self) -> Iterator[Any]: ) if scale is not None: # a depth image, returned in metres - data = data.astype(np.float32) * np.float32(scale) + metres = data.astype(np.float32) * np.float32(scale) + if np.issubdtype(data.dtype, np.integer): + # 0 means no measurement in an integer depth image, and NaN + # does in a float one (REP 118), so convert that too + metres[data == 0] = np.nan + data = metres depth_units = 1.0 # Reshape to (H, W, C) or (H, W) diff --git a/tests/test_ros_depth_units.py b/tests/test_ros_depth_units.py index 9a8b0e2f..4f1f2796 100644 --- a/tests/test_ros_depth_units.py +++ b/tests/test_ros_depth_units.py @@ -25,6 +25,13 @@ from machinevisiontoolbox import ROSBag +def metres(units: np.ndarray, scale: float) -> np.ndarray: + """Expected metres from integer pixels: scaled, with 0 (no measurement) as NaN.""" + out = units.astype(np.float32) * np.float32(scale) + out[units == 0] = np.nan + return out + + def read(path, **kwargs): """Depth and color images of a bag, and the RuntimeWarnings issued.""" with warnings.catch_warnings(record=True) as caught: @@ -58,7 +65,7 @@ def test_auto_uses_the_footer_of_a_realsense_bag(tmp_path): 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 + im.array, metres(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 @@ -69,17 +76,20 @@ def test_auto_uses_the_footer_of_a_realsense_bag(tmp_path): assert not caught -def test_zero_stays_zero(tmp_path): +def test_zero_is_no_measurement_and_becomes_nan(tmp_path): make_realsense_bag(tmp_path / "rs.bag") + raw, _, _ = read(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 + assert raw[0].array[0, 0] == 0 # frame 0 starts with a pixel with no measurement + assert np.isnan(depth[0].array[0, 0]) + assert np.isnan(depth[0].array).sum() == 1 # and only that one 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 + depth[1].array, metres(realsense_depth_frame(1), 0.01), rtol=1e-6 ) @@ -98,7 +108,9 @@ def test_mono16_without_a_footer_has_no_known_scale(tmp_path): 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) + np.testing.assert_allclose( + depth[2].array, metres(realsense_depth_frame(2), 0.001), rtol=1e-6 + ) assert not caught @@ -107,11 +119,13 @@ def test_16uc1_is_in_millimetres_by_rep_118(tmp_path): 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) + np.testing.assert_allclose( + im.array, metres(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): +def test_32fc1_is_left_alone_apart_from_its_scale(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) @@ -145,21 +159,22 @@ def _first_depth(name: str, **kwargs): 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) + converted = _first_depth("20260430_125558.bag", depth_units="auto") + assert converted.array.dtype == np.float32 + np.testing.assert_allclose(converted.array, metres(raw.array, 0.001), rtol=1e-6) + assert np.isnan(converted.array).sum() == (raw.array == 0).sum() # zero is NaN 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) + converted = _first_depth("2024-12-03-05-40-15.bag", depth_units="auto") + np.testing.assert_allclose(converted.array, metres(raw.array, 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 + raw = _first_depth("rgbd_dataset_freiburg1_desk2.bag") # 32FC1, NaN already + converted = _first_depth("rgbd_dataset_freiburg1_desk2.bag", depth_units="auto") + np.testing.assert_array_equal(converted.array, raw.array) # NaN where equal def test_real_intel_sample_bag_scale_is_not_guessed():