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

include/robot_depth_image_proc/point_cloud_xyz.h

@@ -8,11 +8,47 @@
 namespace depth_image_proc
 {
 
+/**
+ * Filtering applied while converting a depth image to a point cloud.
+ * Filtering at the depth-image level is much cheaper than filtering the
+ * generated cloud, and the output stays small enough for costmap_2d.
+ */
+struct DepthFilterConfig
+{
+  /// Keep 1 pixel out of every (decimation x decimation) block. >= 1.
+  int decimation = 4;
+  /// Drop points closer than this depth [m] (sensor near-range noise).
+  double range_min = 0.3;
+  /// Drop points farther than this depth [m].
+  double range_max = 4.0;
+  /// Neighbor depth difference [m] below which a neighbor counts as consistent.
+  double speckle_max_delta = 0.08;
+  /// Minimum consistent 8-connected neighbors to keep a point. 0 disables
+  /// the speckle ("flying pixel") filter.
+  int speckle_min_neighbors = 3;
+
+  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;
+  }
+};
+
+/// Dense organized cloud, one point per pixel (invalid pixels become NaN).
 robot_sensor_msgs::PointCloud2 convertDepthToPointCloud(
     const robot_sensor_msgs::Image& depth_msg,
     const robot_sensor_msgs::CameraInfo& info_msg,
     double range_max = 4.0);
 
+/// Filtered unorganized cloud (height = 1, is_dense = true): range clip,
+/// NxN decimation and speckle removal. Suitable as costmap_2d observation
+/// source input.
+robot_sensor_msgs::PointCloud2 convertDepthToPointCloudFiltered(
+    const robot_sensor_msgs::Image& depth_msg,
+    const robot_sensor_msgs::CameraInfo& info_msg,
+    const DepthFilterConfig& config);
+
 }  // namespace depth_image_proc
 
 #endif

launch/depth_image_proc_gazebo.launch

@@ -6,6 +6,13 @@
   <arg name="camera_info_topic" default="/camera/depth/camera_info"/>
   <arg name="cloud_topic" default="/camera/depth/points_proc"/>
   <arg name="fixed_frame" default="odom"/>
+
+  <!-- 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.3"/>         <!-- [m] -->
+  <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"/> -->
 
   <node pkg="robot_depth_image_proc"
@@ -18,11 +25,16 @@
     <param name="cloud_topic" value="$(arg cloud_topic)"/>
     <param name="fixed_frame" value="$(arg fixed_frame)"/>
     <param name="publish_tf" value="false"/>
+    <param name="filter/decimation" value="$(arg filter_decimation)"/>
+    <param name="filter/range_min" value="$(arg filter_range_min)"/>
+    <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)"/>
   </node>
 
-  <!-- <node if="$(arg rviz)"
+  <node if="$(arg rviz)"
         pkg="rviz"
         type="rviz"
         name="rviz"
-        args="-d $(find robot_depth_image_proc)/rviz/depth_image_proc_gazebo.rviz"/> -->
+        args="-d $(find robot_depth_image_proc)/rviz/depth_image_proc_gazebo.rviz"/>
 </launch>

launch/depth_image_proc_realsense.launch

@@ -8,6 +8,13 @@
   <arg name="publish_tf" default="true"/>
   <arg name="rviz" default="true"/>
 
+  <!-- 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_speckle_max_delta" default="0.08"/> <!-- [m] neighbor depth tolerance -->
+  <arg name="filter_speckle_min_neighbors" default="3"/> <!-- 0 disables speckle filter -->
+
   <node pkg="robot_depth_image_proc"
         type="depth_image_proc_node"
         name="depth_image_proc"
@@ -17,6 +24,11 @@
     <param name="cloud_topic" value="$(arg cloud_topic)"/>
     <param name="fixed_frame" value="$(arg fixed_frame)"/>
     <param name="publish_tf" value="$(arg publish_tf)"/>
+    <param name="filter/decimation" value="$(arg filter_decimation)"/>
+    <param name="filter/range_min" value="$(arg filter_range_min)"/>
+    <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)"/>
   </node>
 
   <node if="$(arg rviz)"

launch/tf_cam.launch

@@ -0,0 +1,8 @@
+<launch>
+  <!-- Vị trí camera so với baselink -->
+  <node pkg="tf2_ros"
+        type="static_transform_publisher"
+        name="baselink_to_camera_depth_optical"
+        args="0.2575 0.0 0.23 -1.5708 0.0 -1.5708 base_link camera_depth_optical_frame"
+        output="screen"/>
+</launch>

src/depth_image_proc_node.cpp

@@ -27,9 +27,11 @@ public:
   DepthCameraPipeline(
       ros::NodeHandle& nh,
       const CameraConfig& config,
+      const depth_image_proc::DepthFilterConfig& filter_config,
       const std::string& fixed_frame,
       bool publish_tf)
       : config_(config),
+        filter_config_(filter_config),
         fixed_frame_(fixed_frame),
         publish_tf_(publish_tf)
   {
@@ -104,13 +106,16 @@ private:
 
     const robot_sensor_msgs::Image depth = depth_image_proc::toRobotImage(*msg);
     const robot_sensor_msgs::PointCloud2 cloud =
-        depth_image_proc::convertDepthToPointCloud(depth, camera_info);
+        depth_image_proc::convertDepthToPointCloudFiltered(
+            depth, camera_info, filter_config_);
 
-    if (cloud.width == 0 || cloud.height == 0)
+    // A fully filtered-out frame (nothing in range) is valid: publish the
+    // empty cloud so costmap_2d observation buffers do not go stale.
+    if (cloud.fields.empty())
     {
       ROS_ERROR_THROTTLE(
           5.0,
-          "[%s] depth_image_proc conversion returned an empty point cloud",
+          "[%s] depth_image_proc conversion failed (bad encoding or filter config)",
           config_.name.c_str());
       return;
     }
@@ -137,6 +142,7 @@ private:
   }
 
   const CameraConfig config_;
+  const depth_image_proc::DepthFilterConfig filter_config_;
   const std::string fixed_frame_;
   const bool publish_tf_;
 
@@ -159,6 +165,8 @@ public:
     pnh.param("fixed_frame", fixed_frame_, std::string("map"));
     pnh.param("publish_tf", publish_tf_, true);
 
+    const depth_image_proc::DepthFilterConfig filter_config = loadFilterConfig(pnh);
+
     const std::vector<CameraConfig> configs = loadCameraConfigs(pnh);
     if (configs.empty())
     {
@@ -170,13 +178,42 @@ public:
     for (const CameraConfig& config : configs)
     {
       pipelines_.push_back(std::make_unique<DepthCameraPipeline>(
-          nh, config, fixed_frame_, publish_tf_));
+          nh, config, filter_config, fixed_frame_, publish_tf_));
     }
 
     ROS_INFO("depth_image_proc_node started with %zu camera(s)", pipelines_.size());
   }
 
 private:
+  static depth_image_proc::DepthFilterConfig loadFilterConfig(ros::NodeHandle& pnh)
+  {
+    depth_image_proc::DepthFilterConfig config;
+    pnh.param("filter/decimation", config.decimation, config.decimation);
+    pnh.param("filter/range_min", config.range_min, config.range_min);
+    pnh.param("filter/range_max", config.range_max, config.range_max);
+    pnh.param("filter/speckle_max_delta", config.speckle_max_delta,
+              config.speckle_max_delta);
+    pnh.param("filter/speckle_min_neighbors", config.speckle_min_neighbors,
+              config.speckle_min_neighbors);
+
+    if (!config.valid())
+    {
+      ROS_FATAL(
+          "Invalid filter config: decimation=%d range=[%.2f, %.2f] m "
+          "speckle_max_delta=%.3f m speckle_min_neighbors=%d",
+          config.decimation, config.range_min, config.range_max,
+          config.speckle_max_delta, config.speckle_min_neighbors);
+      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",
+        config.decimation, config.range_min, config.range_max,
+        config.speckle_max_delta, config.speckle_min_neighbors);
+    return config;
+  }
+
   static bool readCameraConfig(
       const XmlRpc::XmlRpcValue& entry,
       CameraConfig& config,

src/point_cloud_xyz.cpp

@@ -46,4 +46,52 @@ robot_sensor_msgs::PointCloud2 convertDepthToPointCloud(
   return cloud_msg;
 }
 
+robot_sensor_msgs::PointCloud2 convertDepthToPointCloudFiltered(
+    const robot_sensor_msgs::Image& depth_msg,
+    const robot_sensor_msgs::CameraInfo& info_msg,
+    const DepthFilterConfig& config)
+{
+  robot_sensor_msgs::PointCloud2 cloud_msg;
+
+  if (!config.valid())
+  {
+    robot::log_error_throttle(
+        5,
+        "Invalid depth filter config: decimation=%d range=[%.2f, %.2f] m "
+        "speckle_max_delta=%.3f m speckle_min_neighbors=%d",
+        config.decimation, config.range_min, config.range_max,
+        config.speckle_max_delta, config.speckle_min_neighbors);
+    return cloud_msg;
+  }
+
+  cloud_msg.header = depth_msg.header;
+  cloud_msg.height = 1;
+  cloud_msg.width = 0;
+  cloud_msg.is_dense = true;
+  cloud_msg.is_bigendian = false;
+
+  robot_sensor_msgs::PointCloud2Modifier pcd_modifier(cloud_msg);
+  pcd_modifier.setPointCloud2FieldsByString(1, "xyz");
+
+  image_geometry::PinholeCameraModel model;
+  model.fromCameraInfo(info_msg);
+
+  if (depth_msg.encoding == enc::TYPE_16UC1 || depth_msg.encoding == enc::MONO16)
+  {
+    convertFiltered<uint16_t>(depth_msg, cloud_msg, model, config);
+  }
+  else if (depth_msg.encoding == enc::TYPE_32FC1)
+  {
+    convertFiltered<float>(depth_msg, cloud_msg, model, config);
+  }
+  else
+  {
+    robot::log_error_throttle(
+        5, "Depth image has unsupported encoding [%s]", depth_msg.encoding.c_str());
+    return robot_sensor_msgs::PointCloud2();
+  }
+
+  return cloud_msg;
+}
+
 }  // namespace depth_image_proc

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);