add fillter test cam intel

include/robot_depth_image_proc/depth_conversions.h

@@ -39,7 +39,10 @@
 #include <robot_sensor_msgs/point_cloud2_iterator.h>
 #include <robot_image_geometry/pinhole_camera_model.h>
 #include <robot_depth_image_proc/depth_traits.h>
+#include <robot_depth_image_proc/point_cloud_xyz.h>
 
+#include <algorithm>
+#include <cmath>
 #include <limits>
 
 namespace depth_image_proc {
@@ -97,6 +100,139 @@ void convert(
   }
 }
 
+// True when at least `min_neighbors` of the 8-connected neighbors (full
+// resolution) have a depth within `max_delta` meters of `depth_m`. Isolated
+// "flying pixels" at object edges fail this test.
+template<typename T>
+inline bool hasConsistentNeighbors(
+    const T* depth_data,
+    int row_step,
+    int width,
+    int height,
+    int u,
+    int v,
+    float depth_m,
+    float max_delta,
+    int min_neighbors)
+{
+  int consistent = 0;
+  for (int dv = -1; dv <= 1; ++dv)
+  {
+    const int nv = v + dv;
+    if (nv < 0 || nv >= height)
+    {
+      continue;
+    }
+    const T* neighbor_row = depth_data + static_cast<size_t>(nv) * row_step;
+    for (int du = -1; du <= 1; ++du)
+    {
+      if (du == 0 && dv == 0)
+      {
+        continue;
+      }
+      const int nu = u + du;
+      if (nu < 0 || nu >= width)
+      {
+        continue;
+      }
+      const T neighbor = neighbor_row[nu];
+      if (!DepthTraits<T>::valid(neighbor))
+      {
+        continue;
+      }
+      if (std::abs(DepthTraits<T>::toMeters(neighbor) - depth_m) <= max_delta)
+      {
+        if (++consistent >= min_neighbors)
+        {
+          return true;
+        }
+      }
+    }
+  }
+  return false;
+}
+
+// Converts with range clipping, NxN decimation and speckle removal.
+// Produces an unorganized dense cloud (height = 1, no NaN points).
+// cloud_msg must already have its xyz fields set by the caller.
+template<typename T>
+void convertFiltered(
+    const robot_sensor_msgs::Image& depth_msg,
+    PointCloud& cloud_msg,
+    const image_geometry::PinholeCameraModel& model,
+    const DepthFilterConfig& config)
+{
+  const float center_x = model.cx();
+  const float center_y = model.cy();
+
+  const double unit_scaling = DepthTraits<T>::toMeters( T(1) );
+  const float constant_x = unit_scaling / model.fx();
+  const float constant_y = unit_scaling / model.fy();
+
+  const int width = static_cast<int>(depth_msg.width);
+  const int height = static_cast<int>(depth_msg.height);
+  const int decimation = std::max(1, config.decimation);
+  const int row_step = depth_msg.step / sizeof(T);
+  const T* depth_data = reinterpret_cast<const T*>(&depth_msg.data[0]);
+
+  const float range_min = static_cast<float>(config.range_min);
+  const float range_max = static_cast<float>(config.range_max);
+  const float speckle_delta = static_cast<float>(config.speckle_max_delta);
+  const bool use_speckle = config.speckle_min_neighbors > 0;
+
+  const size_t max_points =
+      static_cast<size_t>((height + decimation - 1) / decimation) *
+      static_cast<size_t>((width + decimation - 1) / decimation);
+
+  cloud_msg.height = 1;
+  cloud_msg.is_dense = true;
+
+  robot_sensor_msgs::PointCloud2Modifier pcd_modifier(cloud_msg);
+  pcd_modifier.resize(max_points);
+
+  robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
+  robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
+  robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");
+
+  size_t valid_points = 0;
+  for (int v = 0; v < height; v += decimation)
+  {
+    const T* depth_row = depth_data + static_cast<size_t>(v) * row_step;
+    for (int u = 0; u < width; u += decimation)
+    {
+      const T depth = depth_row[u];
+      if (!DepthTraits<T>::valid(depth))
+      {
+        continue;
+      }
+
+      const float z = DepthTraits<T>::toMeters(depth);
+      if (z < range_min || z > range_max)
+      {
+        continue;
+      }
+
+      if (use_speckle &&
+          !hasConsistentNeighbors<T>(
+              depth_data, row_step, width, height, u, v, z, speckle_delta,
+              config.speckle_min_neighbors))
+      {
+        continue;
+      }
+
+      *iter_x = (u - center_x) * depth * constant_x;
+      *iter_y = (v - center_y) * depth * constant_y;
+      *iter_z = z;
+      ++iter_x;
+      ++iter_y;
+      ++iter_z;
+      ++valid_points;
+    }
+  }
+
+  pcd_modifier.resize(valid_points);
+}
+
 } // namespace depth_image_proc
 
 #endif