diff --git a/README.md b/README.md index a3119af..9f0f1d5 100644 --- a/README.md +++ b/README.md @@ -18,4 +18,5 @@ Parameters * `range_min` (float) - The minimum distance in meters a projected point should be. Points closer than this are discarded. Defaults to 0.45 meters. * `range_max` (float) - The maximum distance in meters a projected point should be. Points further than this are discarded. Defaults to 10.0 meters. * `scan_height` (int) - The row from the depth image to use for the laser projection. Defaults to 1. +* `quantile_value` (float) - The quantile value to use when computing the distance for each column. Defaults to 0.0 (use minimum value in each column). * `output_frame` (string) - The frame id to publish in the LaserScan message. Defaults to "camera_depth_frame". diff --git a/cfg/param.yaml b/cfg/param.yaml index 8aa0aaf..98bad17 100644 --- a/cfg/param.yaml +++ b/cfg/param.yaml @@ -4,4 +4,5 @@ depthimage_to_laserscan: range_min: 0.45 range_max: 10.0 scan_height: 1 + quantile_value: 0.0 output_frame: "camera_depth_frame" diff --git a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp index 4d4b827..cf02834 100644 --- a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp +++ b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp @@ -35,6 +35,8 @@ #include #include +#include +#include #include "depthimage_to_laserscan/DepthImageToLaserScan_export.h" #include "depthimage_to_laserscan/depth_traits.hpp" @@ -69,13 +71,19 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final * radii for each angular increment. The output scan will output the closest radius that is * still not smaller than range_min. This can be used to vertically compress obstacles into * a single LaserScan. + * @param quantile_value The quantile value to use for calculating the distance for each column. + * This value determines which distance measurement to use from the multiple + * rows of depth data. For example, a quantile value of 0.1 will use the 10th + * percentile distance, providing a balance between ignoring outliers and + * maintaining accurate distance measurements. This helps in reducing noise + * and improving the stability of the laser scan data. * @param frame_id The output frame_id for the LaserScan. This will probably NOT be the same frame_id as the * depth image. Example: For OpenNI cameras, this should be set to 'camera_depth_frame' while * the camera uses 'camera_depth_optical_frame'. * */ explicit DepthImageToLaserScan( - float scan_time, float range_min, float range_max, int scan_height, + float scan_time, float range_min, float range_max, int scan_height, float quantile_value, const std::string & frame_id); ~DepthImageToLaserScan(); @@ -149,13 +157,16 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final * @param cam_model The image_geometry camera model for this image. * @param scan_msg The output LaserScan. * @param scan_height The number of vertical pixels to feed into each angular_measurement. + * @param quantile_value The quantile value to use for calculating the distance for each column. * */ template void convert( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, const image_geometry::PinholeCameraModel & cam_model, - const sensor_msgs::msg::LaserScan::UniquePtr & scan_msg, const int & scan_height) const + const sensor_msgs::msg::LaserScan::UniquePtr & scan_msg, + const int & scan_height, + const float & quantile_value) const { // Use correct principal point from calibration float center_x = cam_model.cx(); @@ -164,37 +175,55 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final double unit_scaling = depthimage_to_laserscan::DepthTraits::toMeters(T(1)); float constant_x = unit_scaling / cam_model.fx(); - const T * depth_row = reinterpret_cast(&depth_msg->data[0]); int row_step = depth_msg->step / sizeof(T); - int offset = static_cast(cam_model.cy() - static_cast(scan_height) / 2.0); - depth_row += offset * row_step; // Offset to center of image - for (int v = offset; v < offset + scan_height_; v++, depth_row += row_step) { + + std::vector> column_distances(depth_msg->width); + + // Collect distances for each column + for (int v = offset; v < offset + scan_height; v++) { + const T * depth_row = reinterpret_cast(&depth_msg->data[0]) + v * row_step; for (uint32_t u = 0; u < depth_msg->width; u++) { // Loop over each pixel in row T depth = depth_row[u]; double r = depth; // Assign to pass through NaNs and Infs - // Atan2(x, z), but depth divides out - double th = -std::atan2(static_cast(u - center_x) * constant_x, unit_scaling); - int index = (th - scan_msg->angle_min) / scan_msg->angle_increment; - if (depthimage_to_laserscan::DepthTraits::valid(depth)) { // Not NaN or Inf - // Calculate in XYZ double x = (u - center_x) * depth * constant_x; double z = depthimage_to_laserscan::DepthTraits::toMeters(depth); - - // Calculate actual distance r = std::sqrt(std::pow(x, 2.0) + std::pow(z, 2.0)); } - // Determine if this point should be used. + double th = -std::atan2(static_cast(u - center_x) * constant_x, unit_scaling); + int index = (th - scan_msg->angle_min) / scan_msg->angle_increment; + if (use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)) { - scan_msg->ranges[index] = r; + column_distances[u].push_back(r); + } + } + } + + // Compute quantile for each column and update scan ranges + for (uint32_t u = 0; u < depth_msg->width; u++) { + if (!column_distances[u].empty()) { + std::sort(column_distances[u].begin(), column_distances[u].end()); + size_t index = static_cast(quantile_value * column_distances[u].size()); + double quantile_distance = column_distances[u][index]; + + double th = -std::atan2(static_cast(u - center_x) * constant_x, unit_scaling); + int scan_index = (th - scan_msg->angle_min) / scan_msg->angle_increment; + + // Update scan range + if (use_point( + quantile_distance, scan_msg->ranges[scan_index], scan_msg->range_min, + scan_msg->range_max)) + { + scan_msg->ranges[scan_index] = quantile_distance; } } } } + ///< image_geometry helper class for managing sensor_msgs/CameraInfo messages. image_geometry::PinholeCameraModel cam_model_; @@ -202,6 +231,7 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final float range_min_; ///< Stores the current minimum range to use. float range_max_; ///< Stores the current maximum range to use. int scan_height_; ///< Number of pixel rows to use when producing a laserscan from an area. + float quantile_value_; ///< Quantile value to use for calculating the distance for each column. ///< Output frame_id for each laserscan. This is likely NOT the camera's frame_id. std::string output_frame_id_; }; diff --git a/src/DepthImageToLaserScan.cpp b/src/DepthImageToLaserScan.cpp index 4c589cd..568f70d 100644 --- a/src/DepthImageToLaserScan.cpp +++ b/src/DepthImageToLaserScan.cpp @@ -49,9 +49,9 @@ namespace depthimage_to_laserscan DepthImageToLaserScan::DepthImageToLaserScan( float scan_time, float range_min, float range_max, - int scan_height, const std::string & frame_id) + int scan_height, float quantile_value, const std::string & frame_id) : scan_time_(scan_time), range_min_(range_min), range_max_(range_max), scan_height_(scan_height), - output_frame_id_(frame_id) + quantile_value_(quantile_value), output_frame_id_(frame_id) { } @@ -156,9 +156,9 @@ sensor_msgs::msg::LaserScan::UniquePtr DepthImageToLaserScan::convert_msg( scan_msg->ranges.assign(ranges_size, std::numeric_limits::quiet_NaN()); if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) { - convert(depth_msg, cam_model_, scan_msg, scan_height_); + convert(depth_msg, cam_model_, scan_msg, scan_height_, quantile_value_); } else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) { - convert(depth_msg, cam_model_, scan_msg, scan_height_); + convert(depth_msg, cam_model_, scan_msg, scan_height_, quantile_value_); } else { std::stringstream ss; ss << "Depth image has unsupported encoding: " << depth_msg->encoding; diff --git a/src/DepthImageToLaserScanROS.cpp b/src/DepthImageToLaserScanROS.cpp index 2c4c0fe..fb015f8 100644 --- a/src/DepthImageToLaserScanROS.cpp +++ b/src/DepthImageToLaserScanROS.cpp @@ -69,10 +69,12 @@ DepthImageToLaserScanROS::DepthImageToLaserScanROS(const rclcpp::NodeOptions & o int scan_height = this->declare_parameter("scan_height", 1); + float quantile_value = this->declare_parameter("quantile_value", 0.0); + std::string output_frame = this->declare_parameter("output_frame", "camera_depth_frame"); dtl_ = std::make_unique( - scan_time, range_min, range_max, scan_height, output_frame); + scan_time, range_min, range_max, scan_height, quantile_value, output_frame); } DepthImageToLaserScanROS::~DepthImageToLaserScanROS() diff --git a/test/DepthImageToLaserScanTest.cpp b/test/DepthImageToLaserScanTest.cpp index 9547b84..6d1ad7a 100644 --- a/test/DepthImageToLaserScanTest.cpp +++ b/test/DepthImageToLaserScanTest.cpp @@ -36,6 +36,16 @@ #include #include +#include "depthimage_to_laserscan/depth_traits.hpp" +#if __has_include("image_geometry/pinhole_camera_model.hpp") +#include "image_geometry/pinhole_camera_model.hpp" +#else +// This header was deprecated as of https://github.com/ros-perception/vision_opencv/pull/448 +// (for Iron), and will be completely removed for J-Turtle. However, we still need it in +// Humble, since the .hpp doesn't exist there. +#include "image_geometry/pinhole_camera_model.h" +#endif + // Bring in my package's API, which is what I'm testing #include @@ -45,6 +55,7 @@ const float g_scan_time = 1.0 / 30.0; const float g_range_min = 0.45; const float g_range_max = 10.0; const int g_scan_height = 1; +const float g_quantile_value = 0.5; const char g_output_frame[] = "camera_depth_frame"; // Inputs @@ -55,7 +66,7 @@ sensor_msgs::msg::CameraInfo::SharedPtr info_msg_; TEST(ConvertTest, setupLibrary) { depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min, - g_range_max, g_scan_height, g_output_frame); + g_range_max, g_scan_height, g_quantile_value, g_output_frame); depth_msg_.reset(new sensor_msgs::msg::Image); depth_msg_->header.stamp.sec = 0; @@ -128,7 +139,7 @@ TEST(ConvertTest, setupLibrary) TEST(ConvertTest, testExceptions) { depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min, - g_range_max, g_scan_height, g_output_frame); + g_range_max, g_scan_height, g_quantile_value, g_output_frame); // Test supported image encodings for exceptions // Does not segfault as long as scan_height = 1 @@ -145,7 +156,7 @@ TEST(ConvertTest, testScanHeight) { for (int scan_height = 1; scan_height <= 100; scan_height++) { depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min, - g_range_max, scan_height, g_output_frame); + g_range_max, scan_height, g_quantile_value, g_output_frame); uint16_t low_value = 500; uint16_t high_value = 3000; @@ -169,16 +180,41 @@ TEST(ConvertTest, testScanHeight) // Convert sensor_msgs::msg::LaserScan::SharedPtr scan_msg = dtl.convert_msg(depth_msg_, info_msg_); - // Test for minimum - // 0.9f represents 10 percent margin on range - float high_float_thresh = static_cast(high_value) * 1.0f / 1000.0f * 0.9f; - for (size_t i = 0; i < scan_msg->ranges.size(); i++) { - // If this is a valid point - if (scan_msg->range_min <= scan_msg->ranges[i] && - scan_msg->ranges[i] <= scan_msg->range_max) - { - // Make sure it's not set to the high_value - ASSERT_LT(scan_msg->ranges[i], high_float_thresh); + image_geometry::PinholeCameraModel cam_model; + cam_model.fromCameraInfo(info_msg_); + // Use correct principal point from calibration + float center_x = cam_model.cx(); + float unit_scaling = 0.001f; + float constant_x = unit_scaling / cam_model.fx(); + + // Calculate quantile values for each column in the depth image + size_t quantile_index = static_cast(g_quantile_value * scan_height); + + // Collect distances for each column + + for (int u = 0; u < data_len; u++) { // Loop over each pixel in row + std::vector column_values; + for (int v = 0; v < scan_height; v++) { + uint16_t * data_row = reinterpret_cast(&depth_msg_->data[0]) + (offset + v) * + row_step; + column_values.push_back(data_row[u]); + } + std::sort(column_values.begin(), column_values.end()); + double depth_data_column_quantile = column_values[quantile_index]; + + double x = (u - center_x) * depth_data_column_quantile * constant_x; + double z = depth_data_column_quantile * 0.001f; + double r = std::sqrt(std::pow(x, 2.0) + std::pow(z, 2.0)); + + double th = std::atan2(static_cast(u - center_x) * constant_x, unit_scaling); + uint16_t index = (th - scan_msg->angle_min) / scan_msg->angle_increment; + + // Now we can compare the quantile of the column in the depthimage with the + // equivalent depth calculated back from the result of convert_msg + if (index < depth_msg_->width && std::isfinite(scan_msg->ranges[index])) { + ASSERT_NEAR( + r, scan_msg->ranges[index], + 0.01f * (scan_msg->ranges[index])); } } } @@ -197,7 +233,7 @@ TEST(ConvertTest, testRandom) } depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min, - g_range_max, g_scan_height, g_output_frame); + g_range_max, g_scan_height, g_quantile_value, g_output_frame); // Convert sensor_msgs::msg::LaserScan::SharedPtr scan_msg = dtl.convert_msg(depth_msg_, info_msg_); @@ -226,7 +262,7 @@ TEST(ConvertTest, testNaN) } depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min, - g_range_max, g_scan_height, g_output_frame); + g_range_max, g_scan_height, g_quantile_value, g_output_frame); // Convert sensor_msgs::msg::LaserScan::SharedPtr scan_msg = dtl.convert_msg(float_msg, info_msg_); @@ -254,7 +290,7 @@ TEST(ConvertTest, testPositiveInf) } depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min, - g_range_max, g_scan_height, g_output_frame); + g_range_max, g_scan_height, g_quantile_value, g_output_frame); // Convert sensor_msgs::msg::LaserScan::SharedPtr scan_msg = dtl.convert_msg(float_msg, info_msg_); @@ -289,7 +325,7 @@ TEST(ConvertTest, testNegativeInf) } depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min, - g_range_max, g_scan_height, g_output_frame); + g_range_max, g_scan_height, g_quantile_value, g_output_frame); // Convert sensor_msgs::msg::LaserScan::SharedPtr scan_msg = dtl.convert_msg(float_msg, info_msg_);