From 75c97050f113f27d3ba491064f10c8c586644da7 Mon Sep 17 00:00:00 2001 From: duongtd Date: Wed, 22 Jul 2026 10:36:52 +0700 Subject: [PATCH] add fillter test cam intel --- .../depth_conversions.h | 136 ++++++++++++++++++ .../robot_depth_image_proc/point_cloud_xyz.h | 36 +++++ launch/depth_image_proc_gazebo.launch | 16 ++- launch/depth_image_proc_realsense.launch | 12 ++ launch/tf_cam.launch | 8 ++ src/depth_image_proc_node.cpp | 45 +++++- src/point_cloud_xyz.cpp | 48 +++++++ test/test_point_cloud_xyz.cpp | 82 +++++++++++ 8 files changed, 377 insertions(+), 6 deletions(-) create mode 100644 launch/tf_cam.launch diff --git a/include/robot_depth_image_proc/depth_conversions.h b/include/robot_depth_image_proc/depth_conversions.h index 88c9ac0..9c200de 100644 --- a/include/robot_depth_image_proc/depth_conversions.h +++ b/include/robot_depth_image_proc/depth_conversions.h @@ -39,7 +39,10 @@ #include #include #include +#include +#include +#include #include 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 +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(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::valid(neighbor)) + { + continue; + } + if (std::abs(DepthTraits::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 +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::toMeters( T(1) ); + const float constant_x = unit_scaling / model.fx(); + const float constant_y = unit_scaling / model.fy(); + + const int width = static_cast(depth_msg.width); + const int height = static_cast(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(&depth_msg.data[0]); + + const float range_min = static_cast(config.range_min); + const float range_max = static_cast(config.range_max); + const float speckle_delta = static_cast(config.speckle_max_delta); + const bool use_speckle = config.speckle_min_neighbors > 0; + + const size_t max_points = + static_cast((height + decimation - 1) / decimation) * + static_cast((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 iter_x(cloud_msg, "x"); + robot_sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y"); + robot_sensor_msgs::PointCloud2Iterator 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(v) * row_step; + for (int u = 0; u < width; u += decimation) + { + const T depth = depth_row[u]; + if (!DepthTraits::valid(depth)) + { + continue; + } + + const float z = DepthTraits::toMeters(depth); + if (z < range_min || z > range_max) + { + continue; + } + + if (use_speckle && + !hasConsistentNeighbors( + 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 diff --git a/include/robot_depth_image_proc/point_cloud_xyz.h b/include/robot_depth_image_proc/point_cloud_xyz.h index a1b227d..3a257b1 100644 --- a/include/robot_depth_image_proc/point_cloud_xyz.h +++ b/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 diff --git a/launch/depth_image_proc_gazebo.launch b/launch/depth_image_proc_gazebo.launch index 53c62c6..f079872 100644 --- a/launch/depth_image_proc_gazebo.launch +++ b/launch/depth_image_proc_gazebo.launch @@ -6,6 +6,13 @@ + + + + + + + + + + + + - + args="-d $(find robot_depth_image_proc)/rviz/depth_image_proc_gazebo.rviz"/> diff --git a/launch/depth_image_proc_realsense.launch b/launch/depth_image_proc_realsense.launch index f819eaa..df47cfb 100644 --- a/launch/depth_image_proc_realsense.launch +++ b/launch/depth_image_proc_realsense.launch @@ -8,6 +8,13 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/depth_image_proc_node.cpp b/src/depth_image_proc_node.cpp index 1077db8..b080608 100644 --- a/src/depth_image_proc_node.cpp +++ b/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 configs = loadCameraConfigs(pnh); if (configs.empty()) { @@ -170,13 +178,42 @@ public: for (const CameraConfig& config : configs) { pipelines_.push_back(std::make_unique( - 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, diff --git a/src/point_cloud_xyz.cpp b/src/point_cloud_xyz.cpp index 420d0e2..53305d6 100644 --- a/src/point_cloud_xyz.cpp +++ b/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(depth_msg, cloud_msg, model, config); + } + else if (depth_msg.encoding == enc::TYPE_32FC1) + { + convertFiltered(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 diff --git a/test/test_point_cloud_xyz.cpp b/test/test_point_cloud_xyz.cpp index 30afe19..ed9c34e 100644 --- a/test/test_point_cloud_xyz.cpp +++ b/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 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(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 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);