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,15 +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
-# Apply smoothing for inpainted layer
+    input_layer_name: "despiked"
+    max_hole_size_m: 0.5                      # only fill holes up to ~0.5m x 0.5m (5x5 cells at 0.1m)
+    max_hole_area_m2: 0.25                    # explicit 0.5m x 0.5m cap so large dust/rain gaps stay invalid
 erosion:
   enable: True
   fill_nan: False
@@ -33,4 +81,4 @@ erosion:
     input_layer_name: "traversability"
     dilation_size: 3
     iteration_n: 20
-    reverse: True
+    reverse: True

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
@@ -414,6 +415,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()
 
         # Log final state
         self.update_normal(self.traversability_input)
@@ -916,6 +918,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

@@ -24,8 +24,21 @@ class Inpainting(PluginBase):
         **kwargs (): Additional keyword arguments.
     """
 
-    def __init__(self, cell_n: int = 100, method: str = "telea", **kwargs):
+    def __init__(
+        self,
+        cell_n: int = 100,
+        method: str = "telea",
+        input_layer_name: str = "elevation",
+        resolution: float = 0.1,
+        max_hole_size_m: float = -1.0,
+        max_hole_area_m2: float = -1.0,
+        **kwargs,
+    ):
         super().__init__()
+        self.input_layer_name = input_layer_name
+        self.resolution = float(resolution)
+        self.max_hole_size_m = float(max_hole_size_m)
+        self.max_hole_area_m2 = float(max_hole_area_m2)
         if method == "telea":
             self.method = cv.INPAINT_TELEA
         elif method == "ns":  # Navier-Stokes
@@ -54,12 +67,41 @@ def __call__(
             cupy._core.core.ndarray:
         """
         valid_layer = elevation_map[2]
-        mask_np = cp.asnumpy((valid_layer < 0.5).astype("uint8"))
-        elevation = elevation_map[0]
+        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)
+        invalid_mask = cp.logical_not(valid_mask)
+        mask_np = cp.asnumpy(invalid_mask.astype("uint8"))
+
+        max_hole_size_cells = np.inf
+        if self.max_hole_size_m > 0.0:
+            max_hole_size_cells = self.max_hole_size_m / max(self.resolution, 1e-6)
+
+        if self.max_hole_area_m2 > 0.0:
+            max_hole_area_cells = self.max_hole_area_m2 / max(self.resolution * self.resolution, 1e-6)
+        elif np.isfinite(max_hole_size_cells):
+            max_hole_area_cells = max_hole_size_cells * max_hole_size_cells
+        else:
+            max_hole_area_cells = np.inf
+
+        if np.isfinite(max_hole_size_cells) or np.isfinite(max_hole_area_cells):
+            num_labels, labels, stats, _ = cv.connectedComponentsWithStats(mask_np, connectivity=8)
+            inpaint_mask_np = np.zeros_like(mask_np, dtype="uint8")
+            for label in range(1, num_labels):
+                width = float(stats[label, cv.CC_STAT_WIDTH])
+                height = float(stats[label, cv.CC_STAT_HEIGHT])
+                area = float(stats[label, cv.CC_STAT_AREA])
+                if width <= max_hole_size_cells and height <= max_hole_size_cells and area <= max_hole_area_cells:
+                    inpaint_mask_np[labels == label] = 1
+        else:
+            inpaint_mask_np = mask_np
 
-        if (mask_np < 1).any():
+        if (inpaint_mask_np > 0).any():
             if not cp.any(valid_mask):
                 return elevation
 
@@ -78,12 +120,16 @@ def __call__(
                 # Replace NaNs with the minimum elevation value.
                 safe_elevation = cp.where(finite_elevation, elevation, h_min)
                 scaled = cp.asnumpy((safe_elevation - h_min) * 255.0 / denom).astype("uint8")
-                dst = cv.inpaint(scaled, mask_np, 1, self.method)
+                dst = cv.inpaint(scaled, inpaint_mask_np, 1, self.method)
                 h_inpainted = dst.astype(np.float32) * denom / 255.0 + h_min
                 filled = cp.asarray(h_inpainted, dtype=cp.float32)
 
-            # Ensure already-valid cells mirror the authoritative elevation layer.
-            filled = cp.where(valid_mask, elevation, filled)
-            return filled.astype(cp.float64)
+            # Already-valid cells stay authoritative, supported small holes get filled,
+            # and deeper unsupported gaps remain invalid (NaN) instead of being invented away.
+            inpaint_mask = cp.asarray(inpaint_mask_np.astype(bool))
+            output = cp.full(elevation.shape, cp.nan, dtype=cp.float32)
+            output = cp.where(inpaint_mask, filled, output)
+            output = cp.where(valid_mask, elevation, output)
+            return output.astype(cp.float64)
         else:
-            return elevation_map[0]
+            return cp.where(valid_mask, elevation, cp.nan).astype(cp.float64)