From 03e450436c9ea1c2916817b10873e0eea739ca5f Mon Sep 17 00:00:00 2001 From: Deepana Ishtaweera Date: Thu, 6 Aug 2026 15:23:24 +0200 Subject: [PATCH 1/2] Run the dilation filter regardless of the traversability filter update_map_with_kernel ends with update_normal(self.traversability_input), but the dilation that fills that buffer sat inside the `if self.traversability_filter is not None:` guard added when the filter was made optional. The buffer is not the filter's private input: it holds the dilated upper-bound surface, which update_normal consumes too (hence its `dilated_map` parameter name). When the filter fails to load -- no weights.dat, or torch missing or not CUDA-capable -- traversability_input therefore kept the all-zero contents from compile_kernels(). update_normal still ran, computing normals over a flat zero plane, so normal_x/normal_y/normal_z silently became (0, 0, 1) everywhere regardless of terrain. Nothing surfaced it: the only log line mentions the traversability filter, and (0, 0, 1) is a plausible normal -- on flat ground it is even correct. The dilation does not depend on the learned weights, and upstream runs it unconditionally, so move it back out of the guard and keep only the traversability_filter() call and its elevation_map[3] write behind it. Measured on a 15 degree ramp with the filter disabled: normal tilt was -0.0 deg before, ~15 deg after. --- elevation_mapping_cupy/elevation_mapping.py | 24 ++++++++++++--------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/elevation_mapping_cupy/elevation_mapping.py b/elevation_mapping_cupy/elevation_mapping.py index 5402fe0..3acfe0c 100644 --- a/elevation_mapping_cupy/elevation_mapping.py +++ b/elevation_mapping_cupy/elevation_mapping.py @@ -461,17 +461,21 @@ def update_map_with_kernel(self, points_all, channels, R, t, position_noise, ori if self.param.enable_overlap_clearance: self.clear_overlap_map(t) - # Only update traversability if filter is available (weights loaded) - if self.traversability_filter is not None: - self.traversability_input *= 0.0 - self.dilation_filter_kernel( - self.elevation_map[5], - self.elevation_map[2] + self.elevation_map[6], - self.traversability_input, - self.traversability_mask_dummy, - size=(self.cell_n * self.cell_n), - ) + # Dilate the upper-bound surface. This does not depend on the learned + # filter: it also feeds update_normal below, so it must run even when + # the traversability filter is unavailable, or the normal layers are + # computed over an all-zero surface. + self.traversability_input *= 0.0 + self.dilation_filter_kernel( + self.elevation_map[5], + self.elevation_map[2] + self.elevation_map[6], + self.traversability_input, + self.traversability_mask_dummy, + size=(self.cell_n * self.cell_n), + ) + # Only compute traversability if the filter is available (weights loaded) + if self.traversability_filter is not None: traversability = self.traversability_filter(self.traversability_input) self.elevation_map[3][3:-3, 3:-3] = traversability.reshape( (traversability.shape[2], traversability.shape[3]) From 3745c70d16a13aa12404a809b7807cfa3b1e3380 Mon Sep 17 00:00:00 2001 From: Deepana Ishtaweera Date: Thu, 6 Aug 2026 15:52:23 +0200 Subject: [PATCH 2/2] Add a regression test for normals without the traversability filter The harness in #3 covers this, but sim/ is not on dev yet, so nothing here would catch the dilation moving back inside the filter's guard. Drive the real input path with weight_file="" -- the supported way to run without the learned filter, and what a host with no CUDA-capable torch falls back to -- over a synthetic 15 degree plane, and assert the normal layers recover the slope. Against the bug the dilated surface stays all zeros and the recovered tilt reads -0.0 deg instead of ~15 deg, so the slope test fails; with the fix it passes. The flat-ground case cannot discriminate on its own, since (0, 0, 1) is the right answer there, but it pins the sign convention that makes the slope number meaningful. No new dependencies: the plane is generated directly, so this runs anywhere the existing cupy tests do. Deliberately not asserting that the enabled and disabled configurations agree cell-for-cell. add_points_kernel accumulates with atomics, so upper_bound depends on the order points land and two identical runs already differ by up to 0.29 in normal_x -- such a test would measure determinism, not this fix. --- .../tests/test_normal_layers.py | 116 ++++++++++++++++++ 1 file changed, 116 insertions(+) create mode 100644 elevation_mapping_cupy/tests/test_normal_layers.py diff --git a/elevation_mapping_cupy/tests/test_normal_layers.py b/elevation_mapping_cupy/tests/test_normal_layers.py new file mode 100644 index 0000000..d9d5ede --- /dev/null +++ b/elevation_mapping_cupy/tests/test_normal_layers.py @@ -0,0 +1,116 @@ +"""The normal layers must not depend on the traversability filter. + +``update_map_with_kernel`` feeds ``update_normal`` the ``traversability_input`` +buffer, which holds the dilated upper-bound surface. The learned traversability +filter reads that same buffer, so it is easy to guard the dilation that fills it +behind ``traversability_filter is not None`` -- which leaves the buffer all zeros +whenever the filter is unavailable, and every normal silently becomes (0, 0, 1) +regardless of terrain. + +These tests drive the real pipeline with the filter deliberately disabled, which +is the configuration that regressed. +""" + +import math +from pathlib import Path + +import cupy as cp +import numpy as np +import pytest + +from elevation_mapping_cupy import elevation_mapping, parameter + +CONFIGS = Path(__file__).parent.parent / "configs" + +RESOLUTION = 0.05 +MAP_LENGTH = 4.0 +#: Sample the plane finer than the grid so every interior cell gets several hits. +POINT_SPACING = 0.02 +POINT_HALF_EXTENT = 1.5 + + +def _make_map(weight_file: str): + """An ElevationMap over a plain geometric configuration. + + ``weight_file=""`` is the supported way to run without the learned filter + (see ``ElevationMap.__init__``), and is what a host with no CUDA-capable + torch effectively falls back to. + """ + param = parameter.Parameter( + use_chainer=False, + weight_file=weight_file, + plugin_config_file=str(CONFIGS / "plugin_config.yaml"), + resolution=RESOLUTION, + map_length=MAP_LENGTH, + enable_visibility_cleanup=False, + enable_drift_compensation=False, + ) + # A purely geometric run never writes the default semantic layers. + param.subscriber_cfg = {} + param.update() + return elevation_mapping.ElevationMap(param) + + +def _plane_points(slope_deg: float) -> cp.ndarray: + """A dense point cloud on the plane ``z = x * tan(slope)``, in world frame.""" + axis = np.arange(-POINT_HALF_EXTENT, POINT_HALF_EXTENT, POINT_SPACING, dtype=np.float32) + x, y = np.meshgrid(axis, axis, indexing="ij") + z = x * math.tan(math.radians(slope_deg)) + points = np.stack([x.ravel(), y.ravel(), z.ravel()], axis=1) + return cp.asarray(points, dtype=cp.float32) + + +def _feed(em, slope_deg: float): + """Push one sweep of the plane through the full input path.""" + points = _plane_points(slope_deg) + R = cp.eye(3, dtype=em.param.data_type) + t = cp.zeros(3, dtype=em.param.data_type) + em.input_pointcloud(points, ["x", "y", "z"], R, t, 0.0, 0.0) + + +def _interior(em, layer: str) -> np.ndarray: + """A layer cropped to the well-covered middle, where normals have neighbours.""" + data = cp.asnumpy(em.get_map_with_name_ref(layer, return_cupy=True)) + margin = data.shape[0] // 4 + return data[margin:-margin, margin:-margin] + + +def _recovered_tilt_deg(em) -> float: + """Slope angle implied by the normal layers, in degrees.""" + nx, nz = _interior(em, "normal_x"), _interior(em, "normal_z") + valid = np.abs(nz) > 1e-6 + assert valid.sum() > 100, "too few cells carry a normal to measure a tilt" + return float(np.rad2deg(np.arctan2(-np.median(nx[valid]), np.median(nz[valid])))) + + +def test_traversability_filter_is_actually_disabled(): + """Guards the premise of the tests below.""" + assert _make_map("").traversability_filter is None + + +def test_normals_tilt_on_a_slope_without_the_traversability_filter(): + """The regression test: normals must track terrain with the filter absent. + + Against the bug the dilated surface stays all zeros, so every normal is + (0, 0, 1) and this reads back 0 degrees rather than the true slope. + """ + em = _make_map("") + _feed(em, slope_deg=15.0) + assert _recovered_tilt_deg(em) == pytest.approx(15.0, abs=5.0) + + +def test_normals_point_up_on_flat_ground_without_the_traversability_filter(): + """The flat case cannot catch the bug on its own, but pins the sign convention. + + (0, 0, 1) is the correct answer here, so this passes either way -- it is + what makes the slope result above meaningful rather than a scaling artefact. + """ + em = _make_map("") + _feed(em, slope_deg=0.0) + nx, ny, nz = (_interior(em, layer) for layer in ("normal_x", "normal_y", "normal_z")) + valid = np.abs(nz) > 1e-6 + assert valid.sum() > 100 + + norm = np.sqrt(nx[valid] ** 2 + ny[valid] ** 2 + nz[valid] ** 2) + np.testing.assert_allclose(norm, 1.0, atol=1e-3) + assert np.median(nz[valid]) > 0.99