add fillter test cam intel

test/test_point_cloud_xyz.cpp

@@ -107,6 +107,88 @@ TEST(PointCloudXyz, RejectsUnsupportedEncoding)
   EXPECT_EQ(cloud.height, 0u);
 }
 
+TEST(PointCloudXyzFiltered, DecimatesAndClipsRange)
+{
+  const uint32_t width = 8;
+  const uint32_t height = 8;
+  const uint16_t depth_mm = 2000;
+
+  const robot_sensor_msgs::Image depth = makeFlatDepthImage(width, height, depth_mm);
+  const robot_sensor_msgs::CameraInfo info = makeCameraInfo(width, height);
+
+  depth_image_proc::DepthFilterConfig config;
+  config.decimation = 2;
+  config.range_min = 0.3;
+  config.range_max = 4.0;
+  config.speckle_min_neighbors = 0;
+
+  const robot_sensor_msgs::PointCloud2 cloud =
+      depth_image_proc::convertDepthToPointCloudFiltered(depth, info, config);
+
+  EXPECT_EQ(cloud.height, 1u);
+  EXPECT_EQ(cloud.width, (width / 2) * (height / 2));
+  EXPECT_TRUE(cloud.is_dense);
+
+  robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
+  for (size_t i = 0; i < cloud.width; ++i, ++iter_z)
+  {
+    EXPECT_NEAR(*iter_z, 2.0f, 1e-3f);
+  }
+
+  // Every point out of range -> empty but well-formed cloud.
+  config.range_min = 3.0;
+  config.range_max = 4.0;
+  const robot_sensor_msgs::PointCloud2 empty_cloud =
+      depth_image_proc::convertDepthToPointCloudFiltered(depth, info, config);
+  EXPECT_EQ(empty_cloud.width, 0u);
+  EXPECT_FALSE(empty_cloud.fields.empty());
+}
+
+TEST(PointCloudXyzFiltered, RemovesSpecklePoint)
+{
+  const uint32_t width = 9;
+  const uint32_t height = 9;
+  const uint16_t depth_mm = 2000;
+
+  robot_sensor_msgs::Image depth = makeFlatDepthImage(width, height, depth_mm);
+  // One isolated pixel jumps 0.5 m out of the surface: a flying pixel.
+  auto* data = reinterpret_cast<uint16_t*>(depth.data.data());
+  data[4 * width + 4] = 2500;
+
+  const robot_sensor_msgs::CameraInfo info = makeCameraInfo(width, height);
+
+  depth_image_proc::DepthFilterConfig config;
+  config.decimation = 1;
+  config.range_min = 0.3;
+  config.range_max = 4.0;
+  config.speckle_max_delta = 0.08;
+  config.speckle_min_neighbors = 3;
+
+  const robot_sensor_msgs::PointCloud2 cloud =
+      depth_image_proc::convertDepthToPointCloudFiltered(depth, info, config);
+
+  EXPECT_EQ(cloud.width, width * height - 1);
+
+  robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
+  for (size_t i = 0; i < cloud.width; ++i, ++iter_z)
+  {
+    EXPECT_NEAR(*iter_z, 2.0f, 1e-3f);
+  }
+}
+
+TEST(PointCloudXyzFiltered, RejectsInvalidConfig)
+{
+  const robot_sensor_msgs::Image depth = makeFlatDepthImage(4, 4, 1500);
+  const robot_sensor_msgs::CameraInfo info = makeCameraInfo(4, 4);
+
+  depth_image_proc::DepthFilterConfig config;
+  config.decimation = 0;
+
+  const robot_sensor_msgs::PointCloud2 cloud =
+      depth_image_proc::convertDepthToPointCloudFiltered(depth, info, config);
+  EXPECT_TRUE(cloud.fields.empty());
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);