diff --git a/docs/TEST_DATA_DIG3D_2026-03-30.md b/docs/TEST_DATA_DIG3D_2026-03-30.md new file mode 100644 index 00000000..f7a99edd --- /dev/null +++ b/docs/TEST_DATA_DIG3D_2026-03-30.md @@ -0,0 +1,151 @@ +# DIG3D Test Data For `elevation_mapping_cupy` + +This note points to the current March 26 DIG3D real bags that are useful when +changing `elevation_mapping_cupy`. + +## Current local digging chain + +When replaying the split DIG bags with the current local digging profile, the +effective height-processing chain is: + +- `elevation -> near_base_filtered -> despiked -> inpaint -> excavation_mapping` + +Important detail: + +- excavation mapping currently patches from `inpaint`, not directly from + `despiked` +- so a despike change should usually be checked in both + `/mole/elevation_map_filter` and `/mole/excavation_mapping/grid_map` + +## Existing dataset docs + +- Main bag manifest: + - `/home/lorenzo/mcap/dig3d_2026-03-26/bag_manifest_initial_2026-03-28.md` +- Single-scoop playback priority: + - `/home/lorenzo/mcap/dig3d_2026-03-26/split_single_scoops_obs_2026-03-28/PLAYBACK_PRIORITY_2026-03-28.md` +- Single-scoop manifest: + - `/home/lorenzo/mcap/dig3d_2026-03-26/split_single_scoops_obs_2026-03-28/single_scoop_manifest_2026-03-28.csv` +- Current despike validation summary: + - `/home/lorenzo/mcap/dig3d_2026-03-26/analysis/elevation_despike_2026-03-29/README.md` + +## Bag layouts + +There are two layouts in this dataset. + +### 1. Split full-run layout + +Use this when you want to replay raw lidar and run the perception stack live. + +Example: + +- `/home/lorenzo/mcap/dig3d_2026-03-26/dig3d_real_run_2026-03-26_21-09-18/` + +Expected structure: + +- `sensors/` +- `state/` +- `commands/` +- `lidar/` +- `elevation_map/` +- optional `camera/` + +This is the cleanest layout for: + +- `robot_self_filter` +- live `elevation_mapping_cupy` +- live excavation mapping on top of the `inpaint` layer built from the despiked + elevation-map chain + +### 2. Monolithic bag layout + +Use this when you want a short repro bag or a single whole-run bag in one +folder. + +Examples: + +- `/home/lorenzo/mcap/dig3d_2026-03-26/trenching_single_2026-03-26_21-23-04/` +- `/home/lorenzo/mcap/dig3d_2026-03-26/analysis/single_scoop_splits_2026-03-28/trenching_single_2026-03-26_21-39-24/trenching_single_2026-03-26_21-39-24__scoop_03/` + +Expected structure: + +- `.mcap` +- `metadata.yaml` + +This is the easiest layout for: + +- short Foxglove review +- targeted repro of one failure case +- offline analysis scripts + +## Recommended test data + +### Clean baseline full replay + +Use: + +- `/home/lorenzo/mcap/dig3d_2026-03-26/dig3d_real_run_2026-03-26_21-09-18/` + +Why: + +- split layout is already clean +- short controlled run +- good reference when checking that a filter does not damage reasonable terrain + +### Spike-heavy full-run repro + +Use: + +- `/home/lorenzo/mcap/dig3d_2026-03-26/trenching_single_2026-03-26_21-23-04/` + +Why: + +- strongest near-machine positive spike behavior +- primary bag for validating spike rejection + +Related metrics: + +- `/home/lorenzo/mcap/dig3d_2026-03-26/analysis/elevation_despike_2026-03-29/despike_summary_212304.json` + +### Short focused spike repro + +Use: + +- `/home/lorenzo/mcap/dig3d_2026-03-26/analysis/single_scoop_splits_2026-03-28/trenching_single_2026-03-26_21-23-04/trenching_single_2026-03-26_21-23-04__scoop_05/` + +Why: + +- short repro bag +- good when iterating quickly in Foxglove + +### Short mixed-quality comparison case + +Use: + +- `/home/lorenzo/mcap/dig3d_2026-03-26/analysis/single_scoop_splits_2026-03-28/trenching_single_2026-03-26_21-39-24/trenching_single_2026-03-26_21-39-24__scoop_03/` + +Why: + +- useful comparison case after the spike-heavy repro +- also contains the pitch-jerk behavior investigated elsewhere + +## Fast guidance for future agents + +If the task is "change `elevation_mapping_cupy` and validate on real DIG data", +point the agent to: + +- clean reference: + - `/home/lorenzo/mcap/dig3d_2026-03-26/dig3d_real_run_2026-03-26_21-09-18/` +- spike repro: + - `/home/lorenzo/mcap/dig3d_2026-03-26/trenching_single_2026-03-26_21-23-04/` +- short repro: + - `/home/lorenzo/mcap/dig3d_2026-03-26/analysis/single_scoop_splits_2026-03-28/trenching_single_2026-03-26_21-23-04/trenching_single_2026-03-26_21-23-04__scoop_05/` + +The full-run split layout is the best target when the agent needs live replay +with: + +- bag TF +- raw lidar replay +- `robot_self_filter` +- local `elevation_mapping_cupy` + +The monolithic bags are better when the agent only needs one concise repro case. diff --git a/elevation_mapping_cupy/config/core/plugin_config.yaml b/elevation_mapping_cupy/config/core/plugin_config.yaml index 8ec7b210..2d43fef1 100644 --- a/elevation_mapping_cupy/config/core/plugin_config.yaml +++ b/elevation_mapping_cupy/config/core/plugin_config.yaml @@ -15,7 +15,53 @@ smooth_filter: layer_name: "smooth" extra_params: input_layer_name: "min_filter" -# Apply inpainting using opencv +# Reject close-range above-base returns before despiking. This is meant for +# BASE-centered digging maps where weather/dust can create floating points that +# block later ground observations. +near_base_height_filter: + enable: True + fill_nan: False + is_height_layer: True + layer_name: "near_base_filtered" + extra_params: + input_layer_name: "elevation" + radius_m: 4.0 + max_allowed_height_m: 0.5 +# Remove isolated positive spikes before inpainting. This is intentionally +# asymmetric: upward one/few-cell towers are clipped, trench drops are kept. +positive_spike_filter: + enable: True + fill_nan: False + is_height_layer: True + layer_name: "despiked_coarse" + extra_params: + input_layer_name: "near_base_filtered" + median_filter_size: 5 + spike_height_diff_m: 0.35 + support_height_tolerance_m: 0.12 + max_support_neighbor_count: 1 + max_component_cells: 4 +# A second narrower cleanup pass catches the smaller shovel-local residual bumps +# that survive the coarse tower filter while still preserving supported walls. +positive_spike_filter_cleanup: + type: "positive_spike_filter" + enable: True + fill_nan: False + is_height_layer: True + layer_name: "despiked" + extra_params: + input_layer_name: "despiked_coarse" + median_filter_size: 3 + spike_height_diff_m: 0.08 + support_height_tolerance_m: 0.02 + max_support_neighbor_count: 1 + max_component_cells: 2 + edge_invalid_neighbor_count_min: 3 + edge_peak_diff_m: 0.12 + edge_similarity_tolerance_m: 0.05 + edge_max_similar_neighbor_count: 1 +# Inpaint the cleaned surface so downstream digging keeps hole filling without +# reintroducing the rejected spikes. inpainting: enable: True fill_nan: False @@ -23,9 +69,9 @@ inpainting: layer_name: "inpaint" extra_params: method: "telea" # telea or ns - max_hole_area: 64 # fill only small invalid components; <=0 disables the size bound + input_layer_name: "despiked" + max_hole_area: 25 # 5x5 cells at 0.1m resolution fill_border_holes: False # boundary-connected invalid regions stay unknown -# Apply smoothing for inpainted layer erosion: enable: True fill_nan: False diff --git a/elevation_mapping_cupy/config/setups/menzi/base.yaml b/elevation_mapping_cupy/config/setups/menzi/base.yaml index f2665688..e46faaef 100644 --- a/elevation_mapping_cupy/config/setups/menzi/base.yaml +++ b/elevation_mapping_cupy/config/setups/menzi/base.yaml @@ -11,6 +11,6 @@ basic_layers: ['elevation'] fps: 5.0 elevation_map_filter: - layers: ['min_filter', 'smooth', 'inpaint', 'elevation'] - basic_layers: ['min_filter'] + layers: ['min_filter', 'smooth', 'near_base_filtered', 'despiked', 'inpaint', 'elevation'] + basic_layers: ['min_filter', 'smooth', 'near_base_filtered', 'despiked', 'inpaint'] fps: 3.0 diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py b/elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py index 72cd7046..f44a4594 100644 --- a/elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py +++ b/elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py @@ -148,7 +148,7 @@ def __init__(self, param: Parameter): self.untraversable_polygon = xp.zeros((1, 2)) # Plugins - self.plugin_manager = PluginManager(cell_n=self.cell_n) + self.plugin_manager = PluginManager(cell_n=self.cell_n, resolution=self.resolution) self.plugin_manager.load_plugin_settings(param.plugin_config_file) self.map_initializer = MapInitializer(self.initial_variance, param.initialized_variance, xp=cp, method="points") @@ -159,6 +159,7 @@ def clear(self): self.elevation_map *= 0.0 # Initial variance self.elevation_map[1] += self.initial_variance + self.plugin_manager.reset_layers() self.mean_error = 0.0 self.additive_mean_error = 0.0 @@ -407,6 +408,7 @@ def update_map_with_kernel(self, points_all, channels, R, t, position_noise, ori self.elevation_map[3][3:-3, 3:-3] = traversability.reshape( (traversability.shape[2], traversability.shape[3]) ) + self.plugin_manager.reset_layers() self.update_normal(self.traversability_input) @@ -977,6 +979,7 @@ def initialize_map(self, points, method="cubic"): size=(self.cell_n * self.cell_n), ) self.update_upper_bound_with_valid_elevation() + self.plugin_manager.reset_layers() def list_layers(self) -> List[str]: ordered: List[str] = [] diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/plugins/inpainting.py b/elevation_mapping_cupy/elevation_mapping_cupy/plugins/inpainting.py index 9b23b6eb..2fc9a2c2 100644 --- a/elevation_mapping_cupy/elevation_mapping_cupy/plugins/inpainting.py +++ b/elevation_mapping_cupy/elevation_mapping_cupy/plugins/inpainting.py @@ -9,10 +9,10 @@ import cv2 as cv import numpy as np -_LOGGER = logging.getLogger(__name__) - from .plugin_manager import PluginBase +_LOGGER = logging.getLogger(__name__) + class Inpainting(PluginBase): """ @@ -28,12 +28,14 @@ def __init__( self, cell_n: int = 100, method: str = "telea", + input_layer_name: str = "elevation", max_hole_area: int = 64, fill_border_holes: bool = False, inpaint_radius: float = 1.0, **kwargs, ): super().__init__() + self.input_layer_name = input_layer_name if method == "telea": self.method = cv.INPAINT_TELEA elif method == "ns": # Navier-Stokes @@ -91,8 +93,14 @@ def __call__( Returns: cupy._core.core.ndarray: """ - elevation = elevation_map[0] valid_layer = elevation_map[2] + if self.input_layer_name in layer_names: + elevation = elevation_map[layer_names.index(self.input_layer_name)] + elif self.input_layer_name in plugin_layer_names: + elevation = plugin_layers[plugin_layer_names.index(self.input_layer_name)] + else: + raise ValueError(f"Inpainting could not find layer '{self.input_layer_name}'") + finite_elevation = cp.isfinite(elevation) valid_mask = cp.logical_and(valid_layer > 0.5, finite_elevation) output = cp.full(elevation.shape, cp.nan, dtype=cp.float32) diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/plugins/near_base_height_filter.py b/elevation_mapping_cupy/elevation_mapping_cupy/plugins/near_base_height_filter.py new file mode 100644 index 00000000..eef5000c --- /dev/null +++ b/elevation_mapping_cupy/elevation_mapping_cupy/plugins/near_base_height_filter.py @@ -0,0 +1,62 @@ +import cupy as cp +from typing import List + +from .plugin_manager import PluginBase + + +class NearBaseHeightFilter(PluginBase): + """Invalidate close-range cells that sit above a fixed base-frame height threshold. + + This is intended for BASE-centered digging maps, where spurious close lidar returns + from dust, rain, or snow can float above the real ground and then persist because + later rays no longer see through them. Cells that violate the near-base gate are + marked invalid (NaN) so downstream despiking/inpainting can treat them as holes. + """ + + def __init__( + self, + cell_n: int = 100, + input_layer_name: str = "elevation", + resolution: float = 0.1, + radius_m: float = 3.5, + max_allowed_height_m: float = 0.0, + **kwargs, + ): + super().__init__() + self.input_layer_name = input_layer_name + self.radius_m = float(radius_m) + self.max_allowed_height_m = float(max_allowed_height_m) + + center = (float(cell_n) - 1.0) / 2.0 + coords = (cp.arange(cell_n, dtype=cp.float32) - center) * float(resolution) + yy, xx = cp.meshgrid(coords, coords, indexing="ij") + self.radial_mask = (xx * xx + yy * yy) < (self.radius_m * self.radius_m) + + def _get_input_layer( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + ) -> cp.ndarray: + if self.input_layer_name in layer_names: + layer = elevation_map[layer_names.index(self.input_layer_name)].copy() + if self.input_layer_name == "elevation": + valid_mask = elevation_map[2] > 0.5 + layer = cp.where(valid_mask & cp.isfinite(layer), layer, cp.nan) + return layer + if self.input_layer_name in plugin_layer_names: + return plugin_layers[plugin_layer_names.index(self.input_layer_name)].copy() + return elevation_map[0].copy() + + def __call__( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + *args, + ) -> cp.ndarray: + height_map = self._get_input_layer(elevation_map, layer_names, plugin_layers, plugin_layer_names) + reject_mask = cp.isfinite(height_map) & self.radial_mask & (height_map > self.max_allowed_height_m) + return cp.where(reject_mask, cp.nan, height_map) diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/plugins/plugin_manager.py b/elevation_mapping_cupy/elevation_mapping_cupy/plugins/plugin_manager.py index aa66299a..1ff7ad2d 100644 --- a/elevation_mapping_cupy/elevation_mapping_cupy/plugins/plugin_manager.py +++ b/elevation_mapping_cupy/elevation_mapping_cupy/plugins/plugin_manager.py @@ -113,8 +113,9 @@ class PluginManager(object): This manages the plugins. """ - def __init__(self, cell_n: int): + def __init__(self, cell_n: int, resolution: Optional[float] = None): self.cell_n = cell_n + self.resolution = resolution def init(self, plugin_params: List[PluginParams], extra_params: List[Dict]): self.plugin_params = plugin_params @@ -124,10 +125,12 @@ def init(self, plugin_params: List[PluginParams], extra_params: List[Dict]): m = importlib.import_module("." + param.name, package="elevation_mapping_cupy.plugins") # -> 'module' for name, obj in inspect.getmembers(m): if inspect.isclass(obj) and issubclass(obj, PluginBase) and name != "PluginBase": - # Add cell_n to params + # Add shared grid geometry so plugins can reason in meters. extra_param["cell_n"] = self.cell_n + if self.resolution is not None: + extra_param["resolution"] = self.resolution self.plugins.append(obj(**extra_param)) - self.layers = cp.zeros((len(self.plugins), self.cell_n, self.cell_n), dtype=cp.float32) + self.layers = cp.full((len(self.plugins), self.cell_n, self.cell_n), cp.nan, dtype=cp.float32) self.layer_names = self.get_layer_names() self.plugin_names = self.get_plugin_names() @@ -191,6 +194,7 @@ def update_with_name( semantic_params=None, rotation=None, elements_to_shift=None, + _active_stack=None, ): # Semantic layers are optional. In this repo's supported surface we don't use them, so # default to empty containers to keep plugins robust. @@ -200,42 +204,64 @@ def update_with_name( semantic_params = [] if elements_to_shift is None: elements_to_shift = {} + if _active_stack is None: + _active_stack = set() idx = self.get_layer_index_with_name(name) if idx is not None and idx < len(self.plugins): - n_param = len(signature(self.plugins[idx]).parameters) - if n_param == 5: - self.layers[idx] = self.plugins[idx](elevation_map, layer_names, self.layers, self.layer_names) - elif n_param == 7: - self.layers[idx] = self.plugins[idx]( - elevation_map, - layer_names, - self.layers, - self.layer_names, - semantic_map, - semantic_params, - ) - elif n_param == 8: - self.layers[idx] = self.plugins[idx]( - elevation_map, - layer_names, - self.layers, - self.layer_names, - semantic_map, - semantic_params, - rotation, - ) - else: - self.layers[idx] = self.plugins[idx]( - elevation_map, - layer_names, - self.layers, - self.layer_names, - semantic_map, - semantic_params, - rotation, - elements_to_shift, - ) + if name in _active_stack: + raise RuntimeError(f"Cyclic plugin dependency detected while computing '{name}'") + _active_stack.add(name) + try: + input_layer_name = getattr(self.plugins[idx], "input_layer_name", None) + if input_layer_name in self.layer_names: + dependency_idx = self.get_layer_index_with_name(input_layer_name) + if dependency_idx is not None and cp.isnan(self.layers[dependency_idx]).all(): + self.update_with_name( + input_layer_name, + elevation_map, + layer_names, + semantic_map=semantic_map, + semantic_params=semantic_params, + rotation=rotation, + elements_to_shift=elements_to_shift, + _active_stack=_active_stack, + ) + n_param = len(signature(self.plugins[idx]).parameters) + if n_param == 5: + self.layers[idx] = self.plugins[idx](elevation_map, layer_names, self.layers, self.layer_names) + elif n_param == 7: + self.layers[idx] = self.plugins[idx]( + elevation_map, + layer_names, + self.layers, + self.layer_names, + semantic_map, + semantic_params, + ) + elif n_param == 8: + self.layers[idx] = self.plugins[idx]( + elevation_map, + layer_names, + self.layers, + self.layer_names, + semantic_map, + semantic_params, + rotation, + ) + else: + self.layers[idx] = self.plugins[idx]( + elevation_map, + layer_names, + self.layers, + self.layer_names, + semantic_map, + semantic_params, + rotation, + elements_to_shift, + ) + finally: + _active_stack.remove(name) def get_map_with_name(self, name: str) -> cp.ndarray: idx = self.get_layer_index_with_name(name) diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/plugins/positive_spike_filter.py b/elevation_mapping_cupy/elevation_mapping_cupy/plugins/positive_spike_filter.py new file mode 100644 index 00000000..fa94bf2a --- /dev/null +++ b/elevation_mapping_cupy/elevation_mapping_cupy/plugins/positive_spike_filter.py @@ -0,0 +1,205 @@ +import cupy as cp +import cupyx.scipy.ndimage as ndimage +from typing import List + +from .plugin_manager import PluginBase + + +class PositiveSpikeFilter(PluginBase): + """Remove isolated positive height spikes while keeping extended structures. + + The filter uses two cues: + 1. finite-only local background prominence for isolated tower components + 2. exposed-tip prominence over the highest finite neighbor near NaN frontiers + + This keeps trench drops and supported walls while allowing spikes to be + rejected even when several neighbors are invalid. + """ + + def __init__( + self, + cell_n: int = 100, + input_layer_name: str = "elevation", + median_filter_size: int = 5, + spike_height_diff_m: float = 0.35, + support_height_tolerance_m: float = 0.12, + max_support_neighbor_count: int = 1, + max_component_cells: int = 2, + edge_invalid_neighbor_count_min: int = 9, + edge_peak_diff_m: float = 0.0, + edge_similarity_tolerance_m: float = 0.05, + edge_max_similar_neighbor_count: int = 1, + **kwargs, + ): + super().__init__() + self.input_layer_name = input_layer_name + self.median_filter_size = max(3, int(median_filter_size)) + if self.median_filter_size % 2 == 0: + self.median_filter_size += 1 + self.spike_height_diff_m = float(spike_height_diff_m) + self.support_height_tolerance_m = float(support_height_tolerance_m) + self.max_support_neighbor_count = int(max_support_neighbor_count) + self.max_component_cells = int(max_component_cells) + self.edge_invalid_neighbor_count_min = int(edge_invalid_neighbor_count_min) + self.edge_peak_diff_m = float(edge_peak_diff_m) + self.edge_similarity_tolerance_m = float(edge_similarity_tolerance_m) + self.edge_max_similar_neighbor_count = int(edge_max_similar_neighbor_count) + + def _get_input_layer( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + ) -> cp.ndarray: + if self.input_layer_name in layer_names: + layer = elevation_map[layer_names.index(self.input_layer_name)].copy() + if self.input_layer_name == "elevation": + valid_mask = elevation_map[2] > 0.5 + layer = cp.where(valid_mask & cp.isfinite(layer), layer, cp.nan) + return layer + if self.input_layer_name in plugin_layer_names: + return plugin_layers[plugin_layer_names.index(self.input_layer_name)].copy() + valid_mask = elevation_map[2] > 0.5 + layer = elevation_map[0].copy() + return cp.where(valid_mask & cp.isfinite(layer), layer, cp.nan) + + def _count_supporting_neighbors( + self, + height_map: cp.ndarray, + finite_mask: cp.ndarray, + candidate_mask: cp.ndarray, + ) -> cp.ndarray: + support = cp.zeros(height_map.shape, dtype=cp.int32) + for dy in (-1, 0, 1): + for dx in (-1, 0, 1): + if dx == 0 and dy == 0: + continue + shifted = cp.roll(height_map, shift=(dy, dx), axis=(0, 1)) + shifted_finite = cp.roll(finite_mask, shift=(dy, dx), axis=(0, 1)) + shifted_candidate = cp.roll(candidate_mask, shift=(dy, dx), axis=(0, 1)) + if dy > 0: + shifted_finite[:dy, :] = False + shifted_candidate[:dy, :] = False + elif dy < 0: + shifted_finite[dy:, :] = False + shifted_candidate[dy:, :] = False + if dx > 0: + shifted_finite[:, :dx] = False + shifted_candidate[:, :dx] = False + elif dx < 0: + shifted_finite[:, dx:] = False + shifted_candidate[:, dx:] = False + support += ( + shifted_finite + & (~shifted_candidate) + & (cp.abs(shifted - height_map) <= self.support_height_tolerance_m) + ) + return support + + def _get_local_background( + self, + height_map: cp.ndarray, + ) -> tuple[cp.ndarray, cp.ndarray]: + pad = self.median_filter_size // 2 + padded = cp.pad(height_map, pad_width=pad, mode="constant", constant_values=cp.nan) + window_views = [] + for dy in range(self.median_filter_size): + for dx in range(self.median_filter_size): + if dy == pad and dx == pad: + continue + window_views.append( + padded[dy : dy + height_map.shape[0], dx : dx + height_map.shape[1]] + ) + window_stack = cp.stack(window_views, axis=0) + finite_neighbor_count = cp.sum(cp.isfinite(window_stack), axis=0, dtype=cp.int32) + local_background = cp.nanmedian(window_stack, axis=0) + # If a cell has no finite neighbors in the full window, leave the background + # equal to the cell itself so it won't become a false candidate. + local_background = cp.where(cp.isfinite(local_background), local_background, height_map) + return local_background, finite_neighbor_count + + def _get_edge_peak_reject_mask( + self, + height_map: cp.ndarray, + finite_mask: cp.ndarray, + ) -> tuple[cp.ndarray, cp.ndarray]: + reject = cp.zeros(height_map.shape, dtype=cp.bool_) + replacement = cp.full(height_map.shape, cp.nan, dtype=height_map.dtype) + if self.edge_invalid_neighbor_count_min > 8 or self.edge_peak_diff_m <= 0.0: + return reject, replacement + + finite_neighbor_count = cp.zeros(height_map.shape, dtype=cp.int32) + similar_neighbor_count = cp.zeros(height_map.shape, dtype=cp.int32) + max_finite_neighbor_height = cp.full(height_map.shape, -cp.inf, dtype=height_map.dtype) + + for dy in (-1, 0, 1): + for dx in (-1, 0, 1): + if dx == 0 and dy == 0: + continue + shifted = cp.roll(height_map, shift=(dy, dx), axis=(0, 1)) + shifted_finite = cp.roll(finite_mask, shift=(dy, dx), axis=(0, 1)) + if dy > 0: + shifted_finite[:dy, :] = False + elif dy < 0: + shifted_finite[dy:, :] = False + if dx > 0: + shifted_finite[:, :dx] = False + elif dx < 0: + shifted_finite[:, dx:] = False + finite_neighbor_count += shifted_finite + similar_neighbor_count += shifted_finite & ( + cp.abs(shifted - height_map) <= self.edge_similarity_tolerance_m + ) + max_finite_neighbor_height = cp.where( + shifted_finite, + cp.maximum(max_finite_neighbor_height, shifted), + max_finite_neighbor_height, + ) + + invalid_neighbor_count = 8 - finite_neighbor_count + has_finite_neighbor = finite_neighbor_count > 0 + reject = ( + finite_mask + & has_finite_neighbor + & (invalid_neighbor_count >= self.edge_invalid_neighbor_count_min) + & (similar_neighbor_count <= self.edge_max_similar_neighbor_count) + & ((height_map - max_finite_neighbor_height) > self.edge_peak_diff_m) + ) + replacement = cp.where(has_finite_neighbor, max_finite_neighbor_height, replacement) + return reject, replacement + + def __call__( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + *args, + ) -> cp.ndarray: + height_map = self._get_input_layer(elevation_map, layer_names, plugin_layers, plugin_layer_names) + finite_mask = cp.isfinite(height_map) + if not cp.any(finite_mask): + return height_map + + local_background, _ = self._get_local_background(height_map) + candidate = finite_mask & ((height_map - local_background) > self.spike_height_diff_m) + reject = cp.zeros(height_map.shape, dtype=cp.bool_) + if cp.any(candidate): + labels, _ = ndimage.label(candidate, structure=ndimage.generate_binary_structure(2, 2)) + component_sizes = cp.bincount(labels.ravel()) + small_component = labels > 0 + small_component &= component_sizes[labels] <= self.max_component_cells + + support = self._count_supporting_neighbors(height_map, finite_mask, candidate) + reject = small_component & (support <= self.max_support_neighbor_count) + + edge_peak_reject, edge_peak_replacement = self._get_edge_peak_reject_mask( + height_map, + finite_mask, + ) + if not cp.any(reject) and not cp.any(edge_peak_reject): + return height_map + filtered = cp.where(reject, local_background, height_map) + filtered = cp.where(edge_peak_reject, edge_peak_replacement, filtered) + return cp.where(finite_mask, filtered, height_map) diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/tests/test_near_base_height_filter.py b/elevation_mapping_cupy/elevation_mapping_cupy/tests/test_near_base_height_filter.py new file mode 100644 index 00000000..71500d9f --- /dev/null +++ b/elevation_mapping_cupy/elevation_mapping_cupy/tests/test_near_base_height_filter.py @@ -0,0 +1,43 @@ +import cupy as cp + +from elevation_mapping_cupy.plugins.near_base_height_filter import NearBaseHeightFilter + + +def test_near_base_height_filter_removes_above_threshold_cells_near_base(): + filt = NearBaseHeightFilter( + cell_n=21, + input_layer_name="elevation", + resolution=0.5, + radius_m=4.0, + max_allowed_height_m=0.5, + ) + + elevation_map = cp.zeros((7, 21, 21), dtype=cp.float32) + elevation_map[2, ...] = 1.0 + elevation_map[0, 10, 10] = 0.6 + elevation_map[0, 10, 14] = 0.6 + + filtered = filt(elevation_map, ["elevation"], cp.zeros((0, 21, 21), dtype=cp.float32), []) + + assert cp.isnan(filtered[10, 10]) + assert cp.isnan(filtered[10, 14]) + + +def test_near_base_height_filter_keeps_low_and_far_cells(): + filt = NearBaseHeightFilter( + cell_n=21, + input_layer_name="elevation", + resolution=0.5, + radius_m=4.0, + max_allowed_height_m=0.5, + ) + + elevation_map = cp.zeros((7, 21, 21), dtype=cp.float32) + elevation_map[2, ...] = 1.0 + elevation_map[0, 10, 10] = 0.4 + elevation_map[0, 10, 19] = 0.8 + + filtered = filt(elevation_map, ["elevation"], cp.zeros((0, 21, 21), dtype=cp.float32), []) + + assert float(filtered[10, 10]) > 0.3 + assert float(filtered[10, 19]) > 0.7 diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/tests/test_positive_spike_filter.py b/elevation_mapping_cupy/elevation_mapping_cupy/tests/test_positive_spike_filter.py new file mode 100644 index 00000000..ba69802f --- /dev/null +++ b/elevation_mapping_cupy/elevation_mapping_cupy/tests/test_positive_spike_filter.py @@ -0,0 +1,220 @@ +import cupy as cp + +from elevation_mapping_cupy.plugins.inpainting import Inpainting +from elevation_mapping_cupy.plugins.positive_spike_filter import PositiveSpikeFilter + + +def test_positive_spike_filter_removes_small_spikes_and_preserves_wall(): + filt = PositiveSpikeFilter( + cell_n=21, + input_layer_name="inpaint", + median_filter_size=5, + spike_height_diff_m=0.35, + support_height_tolerance_m=0.12, + max_support_neighbor_count=1, + max_component_cells=2, + ) + + elevation_map = cp.zeros((7, 21, 21), dtype=cp.float32) + plugin_layers = cp.zeros((1, 21, 21), dtype=cp.float32) + plugin_layer_names = ["inpaint"] + + wall_height = 0.7 + plugin_layers[0, 6:15, 10:13] = wall_height + + plugin_layers[0, 4, 4] = 1.1 + plugin_layers[0, 16, 5] = 0.95 + plugin_layers[0, 16, 6] = 0.95 + + filtered = filt(elevation_map, ["elevation"], plugin_layers, plugin_layer_names) + + assert float(filtered[4, 4]) < 0.2 + assert float(filtered[16, 5]) < 0.2 + assert float(filtered[16, 6]) < 0.2 + assert float(filtered[10, 11]) > 0.6 + + +def test_positive_spike_filter_keeps_negative_trench_drop(): + filt = PositiveSpikeFilter( + cell_n=21, + input_layer_name="inpaint", + median_filter_size=5, + spike_height_diff_m=0.35, + support_height_tolerance_m=0.12, + max_support_neighbor_count=1, + max_component_cells=2, + ) + + elevation_map = cp.zeros((7, 21, 21), dtype=cp.float32) + plugin_layers = cp.zeros((1, 21, 21), dtype=cp.float32) + plugin_layer_names = ["inpaint"] + + plugin_layers[0, ...] = 0.0 + plugin_layers[0, 8:13, 8:13] = -0.8 + + filtered = filt(elevation_map, ["elevation"], plugin_layers, plugin_layer_names) + assert float(filtered[10, 10]) < -0.7 + + +def test_positive_spike_filter_respects_local_height_threshold(): + filt = PositiveSpikeFilter( + cell_n=21, + input_layer_name="inpaint", + median_filter_size=5, + spike_height_diff_m=0.35, + support_height_tolerance_m=0.12, + max_support_neighbor_count=1, + max_component_cells=2, + ) + + elevation_map = cp.zeros((7, 21, 21), dtype=cp.float32) + plugin_layers = cp.zeros((1, 21, 21), dtype=cp.float32) + plugin_layer_names = ["inpaint"] + + plugin_layers[0, 10, 10] = 0.2 + + filtered = filt(elevation_map, ["elevation"], plugin_layers, plugin_layer_names) + assert float(filtered[10, 10]) > 0.19 + + +def test_positive_spike_filter_rejects_isolated_2x2_patch_without_self_support(): + filt = PositiveSpikeFilter( + cell_n=21, + input_layer_name="inpaint", + median_filter_size=5, + spike_height_diff_m=0.35, + support_height_tolerance_m=0.12, + max_support_neighbor_count=1, + max_component_cells=4, + ) + + elevation_map = cp.zeros((7, 21, 21), dtype=cp.float32) + plugin_layers = cp.zeros((1, 21, 21), dtype=cp.float32) + plugin_layer_names = ["inpaint"] + + plugin_layers[0, 9:11, 9:11] = 1.0 + + filtered = filt(elevation_map, ["elevation"], plugin_layers, plugin_layer_names) + assert float(filtered[9, 9]) < 0.2 + assert float(filtered[9, 10]) < 0.2 + assert float(filtered[10, 9]) < 0.2 + assert float(filtered[10, 10]) < 0.2 + + +def test_positive_spike_filter_rejects_exposed_edge_tip_next_to_nans(): + filt = PositiveSpikeFilter( + cell_n=21, + input_layer_name="inpaint", + median_filter_size=3, + spike_height_diff_m=0.08, + support_height_tolerance_m=0.02, + max_support_neighbor_count=1, + max_component_cells=2, + edge_invalid_neighbor_count_min=3, + edge_peak_diff_m=0.12, + edge_similarity_tolerance_m=0.05, + edge_max_similar_neighbor_count=1, + ) + + elevation_map = cp.zeros((7, 21, 21), dtype=cp.float32) + plugin_layers = cp.full((1, 21, 21), cp.nan, dtype=cp.float32) + plugin_layer_names = ["inpaint"] + + plugin_layers[0, 8:12, 8] = 0.0 + plugin_layers[0, 9:12, 9:11] = 0.45 + plugin_layers[0, 10, 10] = 0.7 + + filtered = filt(elevation_map, ["elevation"], plugin_layers, plugin_layer_names) + + assert 0.4 < float(filtered[10, 10]) < 0.55 + assert float(filtered[9, 9]) > 0.4 + assert float(filtered[9, 10]) > 0.4 + assert cp.isnan(filtered[8, 11]) + + +def test_positive_spike_filter_keeps_supported_ridge_along_nan_boundary(): + filt = PositiveSpikeFilter( + cell_n=21, + input_layer_name="inpaint", + median_filter_size=3, + spike_height_diff_m=0.08, + support_height_tolerance_m=0.02, + max_support_neighbor_count=1, + max_component_cells=2, + edge_invalid_neighbor_count_min=3, + edge_peak_diff_m=0.12, + edge_similarity_tolerance_m=0.05, + edge_max_similar_neighbor_count=1, + ) + + elevation_map = cp.zeros((7, 21, 21), dtype=cp.float32) + plugin_layers = cp.full((1, 21, 21), cp.nan, dtype=cp.float32) + plugin_layer_names = ["inpaint"] + + plugin_layers[0, 9:12, 8] = 0.0 + plugin_layers[0, 9:11, 9:12] = 0.52 + + filtered = filt(elevation_map, ["elevation"], plugin_layers, plugin_layer_names) + + assert float(filtered[9, 10]) > 0.45 + assert float(filtered[10, 10]) > 0.45 + assert cp.isnan(filtered[8, 12]) + + +def test_positive_spike_filter_uses_finite_only_local_background_near_sparse_nan_frontier(): + filt = PositiveSpikeFilter( + cell_n=21, + input_layer_name="inpaint", + median_filter_size=3, + spike_height_diff_m=0.08, + support_height_tolerance_m=0.02, + max_support_neighbor_count=1, + max_component_cells=2, + edge_invalid_neighbor_count_min=3, + edge_peak_diff_m=0.12, + edge_similarity_tolerance_m=0.05, + edge_max_similar_neighbor_count=1, + ) + + elevation_map = cp.zeros((7, 21, 21), dtype=cp.float32) + plugin_layers = cp.full((1, 21, 21), 0.8, dtype=cp.float32) + plugin_layer_names = ["inpaint"] + + plugin_layers[0, 8:13, 8:13] = cp.nan + plugin_layers[0, 10, 9] = 0.0 + plugin_layers[0, 10, 10] = 0.0 + plugin_layers[0, 10, 11] = 0.25 + + filtered = filt(elevation_map, ["elevation"], plugin_layers, plugin_layer_names) + + assert float(filtered[10, 11]) < 0.1 + assert float(filtered[5, 5]) > 0.75 + + +def test_inpainting_can_consume_despiked_plugin_layer(): + elevation_map = cp.zeros((7, 5, 5), dtype=cp.float32) + elevation_map[2, ...] = 1.0 + elevation_map[2, 2, 2] = 0.0 + + plugin_layers = cp.zeros((1, 5, 5), dtype=cp.float32) + plugin_layers[0, ...] = 0.4 + plugin_layers[0, 2, 2] = cp.nan + + plugin = Inpainting(cell_n=5, input_layer_name="despiked") + filled = plugin(elevation_map, ["elevation"], plugin_layers, ["despiked"]) + assert float(filled[2, 2]) > 0.3 + + +def test_inpainting_skips_large_hole_when_hole_size_is_limited(): + elevation_map = cp.zeros((7, 7, 7), dtype=cp.float32) + elevation_map[2, ...] = 1.0 + elevation_map[2, 1:6, 1:6] = 0.0 + + plugin_layers = cp.full((1, 7, 7), 0.4, dtype=cp.float32) + plugin_layers[0, 1:6, 1:6] = cp.nan + + plugin = Inpainting(cell_n=7, input_layer_name="despiked", max_hole_area=4) + filled = plugin(elevation_map, ["elevation"], plugin_layers, ["despiked"]) + + assert cp.isnan(filled[1, 3]) + assert cp.isnan(filled[3, 3]) diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/tests/test_positive_spike_filter_shovel_snapshot.py b/elevation_mapping_cupy/elevation_mapping_cupy/tests/test_positive_spike_filter_shovel_snapshot.py new file mode 100644 index 00000000..9a1b4b6d --- /dev/null +++ b/elevation_mapping_cupy/elevation_mapping_cupy/tests/test_positive_spike_filter_shovel_snapshot.py @@ -0,0 +1,141 @@ +import cupy as cp +import cupyx.scipy.ndimage as ndimage + +from elevation_mapping_cupy.plugins.plugin_manager import PluginManager, PluginParams + + +# Captured from the paused DIG3D replay around SHOVEL_1300 in: +# /home/lorenzo/mcap/dig3d_2026-03-26/dig3d_real_run_2026-03-26_21-02-16 +# Input layer is the live `near_base_filtered` crop from the paused replay. +# The point of this fixture is to keep the real shovel-neighborhood residual bump +# case in-tree and verify the configured two-stage despike chain clears it. +_DIG3D_SHOVEL_NEAR_BASE_FILTERED = [ + [-0.106454, -0.105685, -0.079661, -0.212375, -0.218414, -0.086213, 0.010798, -0.060414, -0.059289, -0.103631, -0.112124, -0.05758, -0.083119, -0.090957, -0.112136, -0.24986, -0.306922], + [-0.082693, -0.083633, -0.072124, -0.160965, -0.146849, -0.015773, 0.015772, -0.03653, -0.05221, -0.080262, -0.087122, -0.063964, -0.077231, -0.127877, -0.164966, -0.239629, -0.382257], + [-0.052809, None, -0.021516, -0.109432, -0.056233, 0.123821, 0.079235, -0.022929, -0.091618, -0.128768, -0.125102, -0.13553, -0.122142, -0.122738, -0.151495, -0.194349, -0.357536], + [None, None, 0.061757, -0.03017, 0.081713, 0.176614, 0.072558, -0.037393, -0.206509, -0.210881, -0.145878, -0.118147, -0.109268, -0.111122, -0.144733, -0.177084, -0.203425], + [None, 0.151703, 0.050805, -0.022074, 0.066795, 0.173782, 0.06233, -0.057888, -0.259199, -0.288448, -0.188871, -0.147942, -0.127483, -0.125233, -0.150424, -0.179148, -0.203675], + [0.211117, 0.17846, 0.025005, -0.042287, 0.070251, 0.144075, 0.077089, -0.067781, -0.190521, -0.311407, -0.307716, -0.219126, -0.197178, -0.181394, -0.187897, -0.212334, -0.221471], + [0.182953, 0.102395, -0.017858, -0.005488, 0.081912, 0.124752, 0.062562, -0.055928, -0.111967, -0.294161, -0.323928, -0.313504, -0.294019, -0.263872, -0.265382, -0.264924, -0.259117], + [0.210857, 0.234403, 0.007223, 0.072316, 0.105333, 0.113418, 0.073348, -0.006843, -0.075788, -0.105427, -0.067347, -0.165784, -0.237731, -0.319602, -0.342158, -0.313188, -0.283981], + [0.274896, 0.262294, 0.103932, 0.117801, 0.102389, 0.095856, 0.101016, 0.066938, -0.044105, -0.063601, -0.045781, -0.138703, -0.214677, -0.300229, -0.282262, -0.374491, -0.317916], + [None, 0.296808, 0.240826, 0.152636, 0.230127, 0.132109, 0.144157, 0.107457, 0.016317, -0.019914, -0.008179, -0.085746, -0.160717, -0.212806, -0.237671, -0.315363, -0.434866], + [None, 0.333239, 0.337791, 0.189904, 0.222295, 0.214662, 0.142167, 0.096078, 0.100083, 0.125587, 0.032163, -0.080256, -0.085567, -0.126295, -0.211153, -0.346405, -0.364523], + [None, None, 0.361097, 0.308092, 0.315232, 0.278821, 0.174725, 0.088646, 0.12519, 0.153612, 0.103663, 0.027897, -0.005071, -0.108958, -0.151502, -0.471054, None], + [None, None, 0.473916, 0.40097, 0.302285, 0.212646, 0.189816, 0.065589, 0.117446, 0.146662, 0.126095, 0.087161, 0.140688, None, None, None, None], + [None, None, None, 0.410359, 0.409464, 0.268848, 0.167423, 0.135426, 0.136431, 0.182271, 0.222348, 0.265474, 0.162581, 0.077579, None, None, -0.280323], + [None, None, None, 0.493615, 0.446911, 0.406165, 0.160394, 0.173892, 0.275772, 0.274095, 0.260192, 0.272611, 0.186416, 0.152871, 0.11557, -0.008845, -0.121657], + [None, None, None, None, 0.470584, 0.467531, 0.430181, 0.25324, 0.35233, 0.324383, 0.285365, 0.273898, 0.292658, 0.205482, 0.121639, 0.01228, -0.094619], + [None, None, None, None, 0.519143, 0.471398, 0.396459, 0.262259, 0.337293, 0.333187, 0.300346, 0.277374, 0.289072, 0.239099, 0.101106, -0.082525, -0.179052], +] + + +def _as_cupy(snapshot): + return cp.asarray([[cp.nan if value is None else value for value in row] for row in snapshot], dtype=cp.float32) + + +def _run_configured_despike_chain(snapshot: cp.ndarray) -> cp.ndarray: + elevation_map = cp.zeros((7, snapshot.shape[0], snapshot.shape[1]), dtype=cp.float32) + elevation_map[0] = snapshot + elevation_map[2] = cp.isfinite(snapshot).astype(cp.float32) + layer_names = [ + "elevation", + "variance", + "is_valid", + "traversability", + "time", + "upper_bound", + "is_upper_bound", + ] + + manager = PluginManager(cell_n=snapshot.shape[0]) + manager.init( + [ + PluginParams(name="positive_spike_filter", layer_name="despiked_coarse"), + PluginParams(name="positive_spike_filter", layer_name="despiked"), + ], + [ + { + "input_layer_name": "elevation", + "median_filter_size": 5, + "spike_height_diff_m": 0.35, + "support_height_tolerance_m": 0.12, + "max_support_neighbor_count": 1, + "max_component_cells": 4, + }, + { + "input_layer_name": "despiked_coarse", + "median_filter_size": 3, + "spike_height_diff_m": 0.08, + "support_height_tolerance_m": 0.02, + "max_support_neighbor_count": 1, + "max_component_cells": 2, + "edge_invalid_neighbor_count_min": 3, + "edge_peak_diff_m": 0.12, + "edge_similarity_tolerance_m": 0.05, + "edge_max_similar_neighbor_count": 1, + }, + ], + ) + manager.update_with_name("despiked", elevation_map, layer_names) + return manager.get_map_with_name("despiked") + + +def _count_local_positive_bump_components( + height_map: cp.ndarray, + threshold_m: float, + radius_m: float | None = None, + resolution_m: float = 0.1, +) -> int: + finite_mask = cp.isfinite(height_map) + if not cp.any(finite_mask): + return 0 + padded = cp.pad( + cp.where(finite_mask, height_map, cp.nan), + pad_width=2, + mode="constant", + constant_values=cp.nan, + ) + window_stack = cp.stack( + [padded[dy : dy + height_map.shape[0], dx : dx + height_map.shape[1]] for dy in range(5) for dx in range(5)], + axis=0, + ) + local_median = cp.nanmedian(window_stack, axis=0) + bump_mask = finite_mask & ((height_map - local_median) > threshold_m) + if radius_m is not None: + center = (float(height_map.shape[0]) - 1.0) / 2.0 + coords = (cp.arange(height_map.shape[0], dtype=cp.float32) - center) * resolution_m + yy, xx = cp.meshgrid(coords, coords, indexing="ij") + bump_mask &= (xx * xx + yy * yy) <= (radius_m * radius_m) + _, component_count = ndimage.label(bump_mask, structure=ndimage.generate_binary_structure(2, 2)) + return int(component_count) + + +def test_positive_spike_filter_chain_preserves_nan_mask_in_dig3d_shovel_snapshot(): + snapshot = _as_cupy(_DIG3D_SHOVEL_NEAR_BASE_FILTERED) + filtered = _run_configured_despike_chain(snapshot) + + assert bool(cp.array_equal(cp.isfinite(filtered), cp.isfinite(snapshot))) + + +def test_positive_spike_filter_chain_clears_small_residual_shovel_bumps_in_snapshot(): + snapshot = _as_cupy(_DIG3D_SHOVEL_NEAR_BASE_FILTERED) + filtered = _run_configured_despike_chain(snapshot) + + assert _count_local_positive_bump_components(filtered, threshold_m=0.15, radius_m=0.8) == 0 + + +def test_positive_spike_filter_chain_preserves_supported_wall(): + snapshot = cp.zeros((21, 21), dtype=cp.float32) + + snapshot[6:15, 10:13] = 0.7 + snapshot[4, 4] = 1.1 + snapshot[16, 5] = 0.95 + snapshot[16, 6] = 0.95 + + filtered = _run_configured_despike_chain(snapshot) + + assert float(filtered[10, 11]) > 0.6 + assert float(filtered[4, 4]) < 0.2 + assert float(filtered[16, 5]) < 0.2 + assert float(filtered[16, 6]) < 0.2