add fillter test cam intel 14:33

CMakeLists.txt

@@ -124,6 +124,7 @@ endif()
 # ========================================================
 add_library(${PROJECT_NAME} SHARED
   src/point_cloud_xyz.cpp
+  src/depth_frame_filter.cpp
 )
 
 # ========================================================

include/robot_depth_image_proc/depth_frame_filter.h

@@ -0,0 +1,62 @@
+#ifndef ROBOT_DEPTH_IMAGE_PROC_DEPTH_FRAME_FILTER_H
+#define ROBOT_DEPTH_IMAGE_PROC_DEPTH_FRAME_FILTER_H
+
+#include <cstdint>
+#include <vector>
+
+#include <robot_sensor_msgs/Image.h>
+
+#include <robot_depth_image_proc/point_cloud_xyz.h>
+
+namespace depth_image_proc
+{
+
+/**
+ * Stateful per-camera depth image pre-filter, applied before the depth ->
+ * point cloud conversion:
+ *
+ *  - Edge filter: invalidates pixels sitting on depth discontinuities plus a
+ *    dilated halo around them. Border pixels mix foreground and background
+ *    light, producing "flying pixels" floating between the object and the
+ *    background. Besides adjacent-pixel jumps it also catches the smooth
+ *    ramps flying pixels smear into (edge_window baseline) and pixels
+ *    hugging no-data holes at occlusion boundaries (edge_invalid_border).
+ *  - Temporal filter: keeps a pixel only after its depth stayed within
+ *    temporal_max_delta for temporal_min_frames consecutive frames. Rejects
+ *    the transient speckle left by objects moving through the field of view.
+ *
+ * Rejected pixels are invalidated in place (0 for 16UC1/mono16, NaN for
+ * 32FC1). History is never used to fill pixels back in, so the filter cannot
+ * create ghost obstacles of objects that already left the scene.
+ *
+ * One instance per camera stream. Not thread-safe: call apply() from a
+ * single thread (e.g. a single-threaded ros::spin()).
+ */
+class DepthFrameFilter
+{
+public:
+  explicit DepthFrameFilter(const DepthFilterConfig& config);
+
+  /// Invalidates edge-halo and temporally unstable pixels in place.
+  /// Unsupported encodings are left untouched.
+  void apply(robot_sensor_msgs::Image& depth_msg);
+
+private:
+  template<typename T>
+  void applyImpl(robot_sensor_msgs::Image& depth_msg);
+
+  void resetHistory(uint32_t width, uint32_t height);
+
+  const DepthFilterConfig config_;
+
+  uint32_t width_{0};
+  uint32_t height_{0};
+  std::vector<float> prev_depth_m_;     ///< <= 0 means no valid history
+  std::vector<uint16_t> stable_frames_; ///< consecutive stable frame count
+  std::vector<uint8_t> edge_mask_;
+  std::vector<uint8_t> dilate_scratch_;
+};
+
+}  // namespace depth_image_proc
+
+#endif

include/robot_depth_image_proc/point_cloud_xyz.h

@@ -27,11 +27,40 @@ struct DepthFilterConfig
   /// the speckle ("flying pixel") filter.
   int speckle_min_neighbors = 3;
 
+  /// Depth jump [m] between adjacent pixels marking an object edge. Pixels on
+  /// and around edges carry mixed foreground/background depth (flying
+  /// pixels). 0 disables the edge filter.
+  double edge_max_delta = 0.1;
+  /// Halo radius [px] removed around detected edges (Chebyshev distance).
+  int edge_dilation = 2;
+  /// Edge detection also compares pixels this many px apart and marks the
+  /// whole span in between. Flying pixels smear into smooth foreground-to-
+  /// background ramps whose per-pixel step stays below edge_max_delta; the
+  /// wider baseline still sees the full jump. 1 keeps only the
+  /// adjacent-pixel test.
+  int edge_window = 4;
+  /// Treat valid pixels bordering invalid (no-data) pixels as edge pixels.
+  /// Mixed pixels hug the no-data band stereo matching leaves at occlusion
+  /// boundaries, where no valid-to-valid depth jump exists to detect.
+  bool edge_invalid_border = true;
+
+  /// Per-pixel frame-to-frame depth tolerance [m] to count as stable.
+  double temporal_max_delta = 0.06;
+  /// Keep a pixel only after it has been stable for this many consecutive
+  /// frames. Rejects transient speckle from objects moving through the view
+  /// at the cost of (frames - 1) camera periods of detection latency.
+  /// 0 or 1 disables the temporal filter.
+  int temporal_min_frames = 2;
+
   bool valid() const
   {
     return decimation >= 1 && range_min >= 0.0 && range_max > range_min &&
            speckle_max_delta > 0.0 && speckle_min_neighbors >= 0 &&
-           speckle_min_neighbors <= 8;
+           speckle_min_neighbors <= 8 && edge_max_delta >= 0.0 &&
+           edge_dilation >= 0 && edge_dilation <= 10 &&
+           edge_window >= 1 && edge_window <= 32 &&
+           temporal_max_delta > 0.0 && temporal_min_frames >= 0 &&
+           temporal_min_frames <= 100;
   }
 };
 

launch/depth_image_proc_gazebo.launch

@@ -13,7 +13,13 @@
   <arg name="filter_range_max" default="4.0"/>         <!-- [m] -->
   <arg name="filter_speckle_max_delta" default="0.08"/> <!-- [m] neighbor depth tolerance -->
   <arg name="filter_speckle_min_neighbors" default="3"/> <!-- 0 disables speckle filter -->
-  <!-- <arg name="rviz" default="true"/> -->
+  <arg name="filter_edge_max_delta" default="0.1"/>     <!-- [m] depth jump = object edge; 0 disables -->
+  <arg name="filter_edge_dilation" default="2"/>        <!-- [px] halo removed around edges -->
+  <arg name="filter_edge_window" default="4"/>          <!-- [px] wide baseline catching smooth flying-pixel ramps; 1 disables -->
+  <arg name="filter_edge_invalid_border" default="true"/> <!-- drop pixels hugging no-data holes at occlusion edges -->
+  <arg name="filter_temporal_max_delta" default="0.06"/> <!-- [m] per-frame stability tolerance -->
+  <arg name="filter_temporal_min_frames" default="2"/>  <!-- stable frames required; 0/1 disables -->
+  <arg name="rviz" default="true"/>
 
   <node pkg="robot_depth_image_proc"
         type="depth_image_proc_node"
@@ -30,6 +36,12 @@
     <param name="filter/range_max" value="$(arg filter_range_max)"/>
     <param name="filter/speckle_max_delta" value="$(arg filter_speckle_max_delta)"/>
     <param name="filter/speckle_min_neighbors" value="$(arg filter_speckle_min_neighbors)"/>
+    <param name="filter/edge_max_delta" value="$(arg filter_edge_max_delta)"/>
+    <param name="filter/edge_dilation" value="$(arg filter_edge_dilation)"/>
+    <param name="filter/edge_window" value="$(arg filter_edge_window)"/>
+    <param name="filter/edge_invalid_border" value="$(arg filter_edge_invalid_border)"/>
+    <param name="filter/temporal_max_delta" value="$(arg filter_temporal_max_delta)"/>
+    <param name="filter/temporal_min_frames" value="$(arg filter_temporal_min_frames)"/>
   </node>
 
   <node if="$(arg rviz)"

launch/depth_image_proc_realsense.launch

@@ -11,9 +11,15 @@
   <!-- Noise filter + downsampling (applied before publishing the cloud) -->
   <arg name="filter_decimation" default="4"/>          <!-- keep 1 of NxN pixels -->
   <arg name="filter_range_min" default="0.1"/>         <!-- [m] -->
-  <arg name="filter_range_max" default="4.0"/>         <!-- [m] -->
+  <arg name="filter_range_max" default="3.0"/>         <!-- [m] -->
   <arg name="filter_speckle_max_delta" default="0.08"/> <!-- [m] neighbor depth tolerance -->
-  <arg name="filter_speckle_min_neighbors" default="3"/> <!-- 0 disables speckle filter -->
+  <arg name="filter_speckle_min_neighbors" default="5"/> <!-- 0 disables speckle filter; 5 also kills 2x2 noise clusters -->
+  <arg name="filter_edge_max_delta" default="0.1"/>     <!-- [m] depth jump = object edge; 0 disables -->
+  <arg name="filter_edge_dilation" default="2"/>        <!-- [px] halo removed around edges -->
+  <arg name="filter_edge_window" default="4"/>          <!-- [px] wide baseline catching smooth flying-pixel ramps; 1 disables -->
+  <arg name="filter_edge_invalid_border" default="true"/> <!-- drop pixels hugging no-data holes at occlusion edges -->
+  <arg name="filter_temporal_max_delta" default="0.06"/> <!-- [m] per-frame stability tolerance -->
+  <arg name="filter_temporal_min_frames" default="3"/>  <!-- stable frames required (~100 ms latency at 30 fps); 0/1 disables -->
 
   <node pkg="robot_depth_image_proc"
         type="depth_image_proc_node"
@@ -29,6 +35,12 @@
     <param name="filter/range_max" value="$(arg filter_range_max)"/>
     <param name="filter/speckle_max_delta" value="$(arg filter_speckle_max_delta)"/>
     <param name="filter/speckle_min_neighbors" value="$(arg filter_speckle_min_neighbors)"/>
+    <param name="filter/edge_max_delta" value="$(arg filter_edge_max_delta)"/>
+    <param name="filter/edge_dilation" value="$(arg filter_edge_dilation)"/>
+    <param name="filter/edge_window" value="$(arg filter_edge_window)"/>
+    <param name="filter/edge_invalid_border" value="$(arg filter_edge_invalid_border)"/>
+    <param name="filter/temporal_max_delta" value="$(arg filter_temporal_max_delta)"/>
+    <param name="filter/temporal_min_frames" value="$(arg filter_temporal_min_frames)"/>
   </node>
 
   <node if="$(arg rviz)"

rviz/depth_image_proc_realsense.rviz

@@ -10,7 +10,7 @@ Visualization Manager:
       Class: rviz/Grid
       Enabled: true
       Name: Grid
-      Reference Frame: map
+      Reference Frame: base_link
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -29,7 +29,7 @@ Visualization Manager:
       Selectable: true
       Size (Pixels): 2
       Style: Points
-      Topic: /camera/depth/points
+      Topic: /camera/depth/points_proc
       Use Fixed Frame: true
     - Class: rviz/Image
       Enabled: true

src/depth_frame_filter.cpp

@@ -0,0 +1,271 @@
+#include <robot_depth_image_proc/depth_frame_filter.h>
+
+#include <algorithm>
+#include <cmath>
+#include <cstring>
+#include <limits>
+
+#include <robot/robot.h>
+#include <robot_depth_image_proc/depth_traits.h>
+#include <robot_sensor_msgs/image_encodings.h>
+
+namespace depth_image_proc
+{
+
+namespace enc = robot_sensor_msgs::image_encodings;
+
+namespace
+{
+
+template<typename T>
+T invalidDepth();
+
+template<>
+uint16_t invalidDepth<uint16_t>()
+{
+  return 0;
+}
+
+template<>
+float invalidDepth<float>()
+{
+  return std::numeric_limits<float>::quiet_NaN();
+}
+
+// Chebyshev dilation of a binary mask, separable in two passes.
+void dilateMask(
+    std::vector<uint8_t>& mask,
+    std::vector<uint8_t>& scratch,
+    int width,
+    int height,
+    int radius)
+{
+  if (radius <= 0)
+  {
+    return;
+  }
+
+  std::fill(scratch.begin(), scratch.end(), 0);
+  for (int v = 0; v < height; ++v)
+  {
+    const uint8_t* row = mask.data() + static_cast<size_t>(v) * width;
+    uint8_t* out = scratch.data() + static_cast<size_t>(v) * width;
+    for (int u = 0; u < width; ++u)
+    {
+      if (row[u] == 0)
+      {
+        continue;
+      }
+      const int lo = std::max(0, u - radius);
+      const int hi = std::min(width - 1, u + radius);
+      std::memset(out + lo, 1, static_cast<size_t>(hi - lo + 1));
+    }
+  }
+
+  std::fill(mask.begin(), mask.end(), 0);
+  for (int v = 0; v < height; ++v)
+  {
+    const uint8_t* row = scratch.data() + static_cast<size_t>(v) * width;
+    const int v_lo = std::max(0, v - radius);
+    const int v_hi = std::min(height - 1, v + radius);
+    for (int u = 0; u < width; ++u)
+    {
+      if (row[u] == 0)
+      {
+        continue;
+      }
+      for (int nv = v_lo; nv <= v_hi; ++nv)
+      {
+        mask[static_cast<size_t>(nv) * width + u] = 1;
+      }
+    }
+  }
+}
+
+}  // namespace
+
+DepthFrameFilter::DepthFrameFilter(const DepthFilterConfig& config)
+    : config_(config)
+{
+}
+
+void DepthFrameFilter::apply(robot_sensor_msgs::Image& depth_msg)
+{
+  if (depth_msg.encoding == enc::TYPE_16UC1 || depth_msg.encoding == enc::MONO16)
+  {
+    applyImpl<uint16_t>(depth_msg);
+  }
+  else if (depth_msg.encoding == enc::TYPE_32FC1)
+  {
+    applyImpl<float>(depth_msg);
+  }
+  else
+  {
+    robot::log_error_throttle(
+        5, "DepthFrameFilter: unsupported encoding [%s]",
+        depth_msg.encoding.c_str());
+  }
+}
+
+void DepthFrameFilter::resetHistory(uint32_t width, uint32_t height)
+{
+  width_ = width;
+  height_ = height;
+  const size_t size = static_cast<size_t>(width) * height;
+  prev_depth_m_.assign(size, -1.0f);
+  stable_frames_.assign(size, 0);
+  edge_mask_.assign(size, 0);
+  dilate_scratch_.assign(size, 0);
+}
+
+template<typename T>
+void DepthFrameFilter::applyImpl(robot_sensor_msgs::Image& depth_msg)
+{
+  const int width = static_cast<int>(depth_msg.width);
+  const int height = static_cast<int>(depth_msg.height);
+  if (width <= 0 || height <= 0)
+  {
+    return;
+  }
+
+  const bool use_edge = config_.edge_max_delta > 0.0;
+  const bool use_temporal = config_.temporal_min_frames > 1;
+  if (!use_edge && !use_temporal)
+  {
+    return;
+  }
+
+  if (depth_msg.width != width_ || depth_msg.height != height_)
+  {
+    resetHistory(depth_msg.width, depth_msg.height);
+  }
+
+  T* data = reinterpret_cast<T*>(depth_msg.data.data());
+  const int row_step = depth_msg.step / sizeof(T);
+
+  if (use_edge)
+  {
+    const float edge_delta = static_cast<float>(config_.edge_max_delta);
+    const int window = std::max(1, config_.edge_window);
+    const int strides[2] = {1, window};
+    const int num_strides = window > 1 ? 2 : 1;
+    std::fill(edge_mask_.begin(), edge_mask_.end(), 0);
+
+    for (int v = 0; v < height; ++v)
+    {
+      const T* row = data + static_cast<size_t>(v) * row_step;
+      uint8_t* mask_row = edge_mask_.data() + static_cast<size_t>(v) * width;
+      for (int u = 0; u < width; ++u)
+      {
+        const T depth = row[u];
+        if (!DepthTraits<T>::valid(depth))
+        {
+          continue;
+        }
+
+        // Mixed pixels hug the no-data band stereo matching leaves at
+        // occlusion boundaries, where no valid-to-valid jump exists.
+        if (config_.edge_invalid_border &&
+            ((u > 0 && !DepthTraits<T>::valid(row[u - 1])) ||
+             (u + 1 < width && !DepthTraits<T>::valid(row[u + 1])) ||
+             (v > 0 &&
+              !DepthTraits<T>::valid(data[static_cast<size_t>(v - 1) * row_step + u])) ||
+             (v + 1 < height && !DepthTraits<T>::valid(row[row_step + u]))))
+        {
+          mask_row[u] = 1;
+        }
+
+        const float z = DepthTraits<T>::toMeters(depth);
+
+        // Stride 1 marks both sides of sharp discontinuities. Stride
+        // `window` sees the full jump across the smooth foreground-to-
+        // background ramps flying pixels form (per-pixel step below
+        // edge_delta); the whole span is marked so no ramp interior
+        // survives.
+        for (int s = 0; s < num_strides; ++s)
+        {
+          const int k = strides[s];
+          if (u + k < width)
+          {
+            const T right = row[u + k];
+            if (DepthTraits<T>::valid(right) &&
+                std::abs(DepthTraits<T>::toMeters(right) - z) > edge_delta)
+            {
+              std::memset(mask_row + u, 1, static_cast<size_t>(k) + 1);
+            }
+          }
+          if (v + k < height)
+          {
+            const T down = data[static_cast<size_t>(v + k) * row_step + u];
+            if (DepthTraits<T>::valid(down) &&
+                std::abs(DepthTraits<T>::toMeters(down) - z) > edge_delta)
+            {
+              for (int nv = v; nv <= v + k; ++nv)
+              {
+                edge_mask_[static_cast<size_t>(nv) * width + u] = 1;
+              }
+            }
+          }
+        }
+      }
+    }
+
+    dilateMask(edge_mask_, dilate_scratch_, width, height, config_.edge_dilation);
+  }
+
+  const float temporal_delta = static_cast<float>(config_.temporal_max_delta);
+  const uint16_t min_frames = static_cast<uint16_t>(config_.temporal_min_frames);
+
+  for (int v = 0; v < height; ++v)
+  {
+    T* row = data + static_cast<size_t>(v) * row_step;
+    const size_t mask_offset = static_cast<size_t>(v) * width;
+    for (int u = 0; u < width; ++u)
+    {
+      T& depth = row[u];
+      bool valid = DepthTraits<T>::valid(depth);
+
+      // Edge pixels are unreliable: drop them and their temporal history.
+      if (valid && use_edge && edge_mask_[mask_offset + u] != 0)
+      {
+        depth = invalidDepth<T>();
+        valid = false;
+      }
+
+      if (!use_temporal)
+      {
+        continue;
+      }
+
+      const size_t idx = mask_offset + u;
+      if (!valid)
+      {
+        prev_depth_m_[idx] = -1.0f;
+        stable_frames_[idx] = 0;
+        continue;
+      }
+
+      const float z = DepthTraits<T>::toMeters(depth);
+      const float prev = prev_depth_m_[idx];
+      if (prev > 0.0f && std::abs(z - prev) <= temporal_delta)
+      {
+        if (stable_frames_[idx] < std::numeric_limits<uint16_t>::max())
+        {
+          ++stable_frames_[idx];
+        }
+      }
+      else
+      {
+        stable_frames_[idx] = 1;
+      }
+      prev_depth_m_[idx] = z;
+
+      if (stable_frames_[idx] < min_frames)
+      {
+        depth = invalidDepth<T>();
+      }
+    }
+  }
+}
+
+}  // namespace depth_image_proc

src/depth_image_proc_node.cpp

@@ -10,6 +10,7 @@
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <XmlRpcValue.h>
 
+#include <robot_depth_image_proc/depth_frame_filter.h>
 #include <robot_depth_image_proc/point_cloud_xyz.h>
 #include <robot_depth_image_proc/ros_message_conversions.h>
 
@@ -32,6 +33,7 @@ public:
       bool publish_tf)
       : config_(config),
         filter_config_(filter_config),
+        frame_filter_(filter_config),
         fixed_frame_(fixed_frame),
         publish_tf_(publish_tf)
   {
@@ -104,7 +106,8 @@ private:
       return;
     }
 
-    const robot_sensor_msgs::Image depth = depth_image_proc::toRobotImage(*msg);
+    robot_sensor_msgs::Image depth = depth_image_proc::toRobotImage(*msg);
+    frame_filter_.apply(depth);
     const robot_sensor_msgs::PointCloud2 cloud =
         depth_image_proc::convertDepthToPointCloudFiltered(
             depth, camera_info, filter_config_);
@@ -120,6 +123,8 @@ private:
       return;
     }
 
+    ROS_WARN("cloud: %d", (int)cloud.data.size());
+
     sensor_msgs::PointCloud2 ros_cloud = depth_image_proc::toRosPointCloud(cloud);
     ros_cloud.header.stamp = msg->header.stamp;
     ros_cloud.header.frame_id = msg->header.frame_id;
@@ -143,6 +148,7 @@ private:
 
   const CameraConfig config_;
   const depth_image_proc::DepthFilterConfig filter_config_;
+  depth_image_proc::DepthFrameFilter frame_filter_;
   const std::string fixed_frame_;
   const bool publish_tf_;
 
@@ -195,22 +201,45 @@ private:
               config.speckle_max_delta);
     pnh.param("filter/speckle_min_neighbors", config.speckle_min_neighbors,
               config.speckle_min_neighbors);
+    pnh.param("filter/edge_max_delta", config.edge_max_delta,
+              config.edge_max_delta);
+    pnh.param("filter/edge_dilation", config.edge_dilation,
+              config.edge_dilation);
+    pnh.param("filter/edge_window", config.edge_window, config.edge_window);
+    pnh.param("filter/edge_invalid_border", config.edge_invalid_border,
+              config.edge_invalid_border);
+    pnh.param("filter/temporal_max_delta", config.temporal_max_delta,
+              config.temporal_max_delta);
+    pnh.param("filter/temporal_min_frames", config.temporal_min_frames,
+              config.temporal_min_frames);
 
     if (!config.valid())
     {
       ROS_FATAL(
           "Invalid filter config: decimation=%d range=[%.2f, %.2f] m "
-          "speckle_max_delta=%.3f m speckle_min_neighbors=%d",
+          "speckle_max_delta=%.3f m speckle_min_neighbors=%d "
+          "edge_max_delta=%.3f m edge_dilation=%d edge_window=%d "
+          "edge_invalid_border=%d "
+          "temporal_max_delta=%.3f m temporal_min_frames=%d",
           config.decimation, config.range_min, config.range_max,
-          config.speckle_max_delta, config.speckle_min_neighbors);
+          config.speckle_max_delta, config.speckle_min_neighbors,
+          config.edge_max_delta, config.edge_dilation, config.edge_window,
+          config.edge_invalid_border ? 1 : 0,
+          config.temporal_max_delta, config.temporal_min_frames);
       throw std::runtime_error("invalid depth filter configuration");
     }
 
     ROS_INFO(
         "Depth filter: decimation=%d range=[%.2f, %.2f] m "
-        "speckle_max_delta=%.3f m speckle_min_neighbors=%d",
+        "speckle_max_delta=%.3f m speckle_min_neighbors=%d "
+        "edge_max_delta=%.3f m edge_dilation=%d edge_window=%d "
+        "edge_invalid_border=%d "
+        "temporal_max_delta=%.3f m temporal_min_frames=%d",
         config.decimation, config.range_min, config.range_max,
-        config.speckle_max_delta, config.speckle_min_neighbors);
+        config.speckle_max_delta, config.speckle_min_neighbors,
+        config.edge_max_delta, config.edge_dilation, config.edge_window,
+        config.edge_invalid_border ? 1 : 0,
+        config.temporal_max_delta, config.temporal_min_frames);
     return config;
   }
 

test/test_cam.py

@@ -29,8 +29,8 @@ def main():
     pipeline = rs.pipeline()
     config = rs.config()
 
-    width = 848
+    width = 424
-    height = 480
+    height = 240
     fps = 30
 
     config.enable_stream(

test/test_point_cloud_xyz.cpp

@@ -3,6 +3,7 @@
 #include <cmath>
 #include <cstdint>
 
+#include <robot_depth_image_proc/depth_frame_filter.h>
 #include <robot_depth_image_proc/point_cloud_xyz.h>
 #include <robot_sensor_msgs/image_encodings.h>
 #include <robot_sensor_msgs/point_cloud2_iterator.h>
@@ -189,6 +190,192 @@ TEST(PointCloudXyzFiltered, RejectsInvalidConfig)
   EXPECT_TRUE(cloud.fields.empty());
 }
 
+namespace
+{
+
+size_t countValidPixels(const robot_sensor_msgs::Image& image)
+{
+  const auto* depth = reinterpret_cast<const uint16_t*>(image.data.data());
+  size_t count = 0;
+  for (size_t i = 0; i < image.width * image.height; ++i)
+  {
+    if (depth[i] != 0)
+    {
+      ++count;
+    }
+  }
+  return count;
+}
+
+}  // namespace
+
+TEST(DepthFrameFilter, RemovesEdgeHalo)
+{
+  const uint32_t width = 10;
+  const uint32_t height = 10;
+
+  // Left half at 1.0 m, right half at 2.0 m: a vertical object edge.
+  robot_sensor_msgs::Image depth = makeFlatDepthImage(width, height, 1000);
+  auto* data = reinterpret_cast<uint16_t*>(depth.data.data());
+  for (uint32_t v = 0; v < height; ++v)
+  {
+    for (uint32_t u = 5; u < width; ++u)
+    {
+      data[v * width + u] = 2000;
+    }
+  }
+
+  depth_image_proc::DepthFilterConfig config;
+  config.edge_max_delta = 0.1;
+  config.edge_dilation = 1;
+  config.edge_window = 1;
+  config.edge_invalid_border = false;
+  config.temporal_min_frames = 0;
+
+  depth_image_proc::DepthFrameFilter filter(config);
+  filter.apply(depth);
+
+  // Edge pixels (columns 4 and 5) plus a 1 px halo (columns 3 and 6) removed.
+  EXPECT_EQ(countValidPixels(depth), (width - 4) * height);
+  for (uint32_t v = 0; v < height; ++v)
+  {
+    for (uint32_t u = 3; u <= 6; ++u)
+    {
+      EXPECT_EQ(data[v * width + u], 0u) << "u=" << u << " v=" << v;
+    }
+    EXPECT_NE(data[v * width + 2], 0u);
+    EXPECT_NE(data[v * width + 7], 0u);
+  }
+}
+
+TEST(DepthFrameFilter, RemovesSmoothFlyingPixelRamp)
+{
+  const uint32_t width = 24;
+  const uint32_t height = 6;
+
+  // Foreground at 1.0 m (u 0-7), a smooth 80 mm/px ramp (u 8-15), background
+  // at 1.64 m (u 16-23). Every adjacent step stays below edge_max_delta, so
+  // only the wide-baseline test can see the jump.
+  robot_sensor_msgs::Image depth = makeFlatDepthImage(width, height, 1000);
+  auto* data = reinterpret_cast<uint16_t*>(depth.data.data());
+  for (uint32_t v = 0; v < height; ++v)
+  {
+    for (uint32_t u = 8; u < 16; ++u)
+    {
+      data[v * width + u] = static_cast<uint16_t>(1000 + 80 * (u - 7));
+    }
+    for (uint32_t u = 16; u < width; ++u)
+    {
+      data[v * width + u] = 1640;
+    }
+  }
+
+  depth_image_proc::DepthFilterConfig config;
+  config.edge_max_delta = 0.1;
+  config.edge_dilation = 0;
+  config.edge_window = 4;
+  config.edge_invalid_border = false;
+  config.temporal_min_frames = 0;
+
+  depth_image_proc::DepthFrameFilter filter(config);
+  filter.apply(depth);
+
+  // Spans where |z(u+4) - z(u)| > 0.1 m cover columns 5..17: the whole ramp
+  // plus its shoulders goes, the flat surfaces on both sides stay.
+  for (uint32_t v = 0; v < height; ++v)
+  {
+    for (uint32_t u = 0; u < width; ++u)
+    {
+      if (u >= 5 && u <= 17)
+      {
+        EXPECT_EQ(data[v * width + u], 0u) << "u=" << u << " v=" << v;
+      }
+      else
+      {
+        EXPECT_NE(data[v * width + u], 0u) << "u=" << u << " v=" << v;
+      }
+    }
+  }
+}
+
+TEST(DepthFrameFilter, RemovesPixelsHuggingInvalidHoles)
+{
+  const uint32_t width = 10;
+  const uint32_t height = 10;
+
+  // Flat surface with a no-data band (column 5, rows 2-7), as stereo
+  // matching leaves at occlusion boundaries. No valid-to-valid jump exists.
+  robot_sensor_msgs::Image depth = makeFlatDepthImage(width, height, 2000);
+  auto* data = reinterpret_cast<uint16_t*>(depth.data.data());
+  for (uint32_t v = 2; v <= 7; ++v)
+  {
+    data[v * width + 5] = 0;
+  }
+
+  depth_image_proc::DepthFilterConfig config;
+  config.edge_max_delta = 0.1;
+  config.edge_dilation = 0;
+  config.edge_window = 1;
+  config.edge_invalid_border = true;
+  config.temporal_min_frames = 0;
+
+  depth_image_proc::DepthFrameFilter filter(config);
+  filter.apply(depth);
+
+  // Hole (6 px) plus its 4-connected valid border (14 px) are invalid.
+  EXPECT_EQ(countValidPixels(depth), width * height - 20);
+  for (uint32_t v = 2; v <= 7; ++v)
+  {
+    EXPECT_EQ(data[v * width + 4], 0u) << "v=" << v;
+    EXPECT_EQ(data[v * width + 6], 0u) << "v=" << v;
+    EXPECT_NE(data[v * width + 3], 0u) << "v=" << v;
+    EXPECT_NE(data[v * width + 7], 0u) << "v=" << v;
+  }
+  EXPECT_EQ(data[1 * width + 5], 0u);
+  EXPECT_EQ(data[8 * width + 5], 0u);
+  EXPECT_NE(data[0 * width + 5], 0u);
+  EXPECT_NE(data[9 * width + 5], 0u);
+}
+
+TEST(DepthFrameFilter, RejectsTransientPixels)
+{
+  const uint32_t width = 6;
+  const uint32_t height = 6;
+
+  depth_image_proc::DepthFilterConfig config;
+  config.edge_max_delta = 0.0;
+  config.temporal_max_delta = 0.06;
+  config.temporal_min_frames = 2;
+
+  depth_image_proc::DepthFrameFilter filter(config);
+
+  // Frame 1: nothing has history yet, everything suppressed.
+  robot_sensor_msgs::Image frame = makeFlatDepthImage(width, height, 2000);
+  filter.apply(frame);
+  EXPECT_EQ(countValidPixels(frame), 0u);
+
+  // Frame 2: static scene is now stable and passes through.
+  frame = makeFlatDepthImage(width, height, 2000);
+  filter.apply(frame);
+  EXPECT_EQ(countValidPixels(frame), width * height);
+
+  // Frame 3: one pixel jumps 0.5 m (object crossing the view) -> rejected,
+  // the static background stays.
+  frame = makeFlatDepthImage(width, height, 2000);
+  auto* data = reinterpret_cast<uint16_t*>(frame.data.data());
+  data[3 * width + 3] = 2500;
+  filter.apply(frame);
+  EXPECT_EQ(countValidPixels(frame), width * height - 1);
+  EXPECT_EQ(data[3 * width + 3], 0u);
+
+  // Frame 4: the pixel holds its new depth -> accepted again.
+  frame = makeFlatDepthImage(width, height, 2000);
+  data = reinterpret_cast<uint16_t*>(frame.data.data());
+  data[3 * width + 3] = 2500;
+  filter.apply(frame);
+  EXPECT_EQ(countValidPixels(frame), width * height);
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);