Add digging despike filters and bounded inpainting

Author: Idate96

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:
+
+- `<name>.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.

elevation_mapping_cupy/config/core/plugin_config.yaml

@@ -15,17 +15,63 @@ 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
   is_height_layer: True
   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

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

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] = []

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)

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)