add fillter test cam intel 14:33

test/test_cam.py

@@ -29,8 +29,8 @@ def main():
     pipeline = rs.pipeline()
     config = rs.config()
 
-    width = 848
-    height = 480
+    width = 424
+    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);