From c5b78f0273f9b3f33d57f8d7a58f418deeb51b6c Mon Sep 17 00:00:00 2001 From: Philipp Polterauer Date: Sat, 17 Feb 2024 11:28:44 +0100 Subject: [PATCH 1/6] added invalid_depth_ parameter --- .gitignore | 2 +- .../include/depth_image_proc/conversions.hpp | 22 +++++++++++++------ .../depth_image_proc/point_cloud_xyz.hpp | 3 +++ depth_image_proc/src/point_cloud_xyz.cpp | 7 ++++-- 4 files changed, 24 insertions(+), 10 deletions(-) diff --git a/.gitignore b/.gitignore index 9d93f2973..189be055b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,3 @@ *pyc -.vscode/settings.json +.vscode/ */doc/generated diff --git a/depth_image_proc/include/depth_image_proc/conversions.hpp b/depth_image_proc/include/depth_image_proc/conversions.hpp index 11496320f..55f1a7d57 100644 --- a/depth_image_proc/include/depth_image_proc/conversions.hpp +++ b/depth_image_proc/include/depth_image_proc/conversions.hpp @@ -53,7 +53,7 @@ void convertDepth( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, const image_geometry::PinholeCameraModel & model, - double range_max = 0.0) + double default_depth = 0.0) { // Use correct principal point from calibration float center_x = model.cx(); @@ -64,20 +64,28 @@ void convertDepth( float constant_x = unit_scaling / model.fx(); float constant_y = unit_scaling / model.fy(); float bad_point = std::numeric_limits::quiet_NaN(); - + + // the following lines ensure that the computation only happens in case we have a default depth + const T default_depth_cvt; + bool use_default_depth = default_depth != 0.0; + if (use_default_depth) { + default_depth_cvt=DepthTraits::fromMeters(default_depth); + } sensor_msgs::PointCloud2Iterator iter_x(*cloud_msg, "x"); sensor_msgs::PointCloud2Iterator iter_y(*cloud_msg, "y"); sensor_msgs::PointCloud2Iterator iter_z(*cloud_msg, "z"); + + // TODO: i think this is undefined behaviour we should favor memcpy? https://stackoverflow.com/questions/55150001/vector-with-reinterpret-cast const T * depth_row = reinterpret_cast(&depth_msg->data[0]); - int row_step = depth_msg->step / sizeof(T); - for (int v = 0; v < static_cast(cloud_msg->height); ++v, depth_row += row_step) { - for (int u = 0; u < static_cast(cloud_msg->width); ++u, ++iter_x, ++iter_y, ++iter_z) { + uint32_t row_step = depth_msg->step / sizeof(T); + for (uint32_t v = 0; v < cloud_msg->height; ++v, depth_row += row_step) { + for (uint32_t u = 0; u < cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z) { T depth = depth_row[u]; // Missing points denoted by NaNs if (!DepthTraits::valid(depth)) { - if (range_max != 0.0) { - depth = DepthTraits::fromMeters(range_max); + if (use_default_depth) { + depth = default_depth_cvt; } else { *iter_x = *iter_y = *iter_z = bad_point; continue; diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp index c5fc4bfd2..6818e8cd8 100644 --- a/depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp +++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp @@ -67,6 +67,9 @@ class PointCloudXyzNode : public rclcpp::Node image_transport::CameraSubscriber sub_depth_; int queue_size_; + // Parameters + double invalid_depth_; + // Publications std::mutex connect_mutex_; rclcpp::Publisher::SharedPtr pub_point_cloud_; diff --git a/depth_image_proc/src/point_cloud_xyz.cpp b/depth_image_proc/src/point_cloud_xyz.cpp index 0175d929a..94f318713 100644 --- a/depth_image_proc/src/point_cloud_xyz.cpp +++ b/depth_image_proc/src/point_cloud_xyz.cpp @@ -58,6 +58,9 @@ PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options) // Read parameters queue_size_ = this->declare_parameter("queue_size", 5); + // values used for invalid points for pcd conversion + invalid_depth_ = this->declare_parameter("invalid_depth_", 0.0); + // Create publisher with connect callback rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = @@ -109,9 +112,9 @@ void PointCloudXyzNode::depthCb( // Convert Depth Image to Pointcloud if (depth_msg->encoding == enc::TYPE_16UC1 || depth_msg->encoding == enc::MONO16) { - convertDepth(depth_msg, cloud_msg, model_); + convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); } else if (depth_msg->encoding == enc::TYPE_32FC1) { - convertDepth(depth_msg, cloud_msg, model_); + convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); } else { RCLCPP_ERROR( get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); From 079454e5003a8626a3bd5c18658283aebb28fdfc Mon Sep 17 00:00:00 2001 From: Philipp Polterauer Date: Sat, 17 Feb 2024 12:14:49 +0100 Subject: [PATCH 2/6] renamed parameter to invalid_depth from default_depth --- .gitignore | 3 ++ .../include/depth_image_proc/conversions.hpp | 30 +++++++++---------- depth_image_proc/src/conversions.cpp | 4 +-- depth_image_proc/src/point_cloud_xyz.cpp | 6 ++-- 4 files changed, 23 insertions(+), 20 deletions(-) diff --git a/.gitignore b/.gitignore index 189be055b..a3df3ca4e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,6 @@ *pyc .vscode/ */doc/generated +/build/ +/log/ +/install/ \ No newline at end of file diff --git a/depth_image_proc/include/depth_image_proc/conversions.hpp b/depth_image_proc/include/depth_image_proc/conversions.hpp index 55f1a7d57..27dbf7b18 100644 --- a/depth_image_proc/include/depth_image_proc/conversions.hpp +++ b/depth_image_proc/include/depth_image_proc/conversions.hpp @@ -51,9 +51,9 @@ namespace depth_image_proc template void convertDepth( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, - sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, const image_geometry::PinholeCameraModel & model, - double default_depth = 0.0) + double invalid_depth = 0.0) { // Use correct principal point from calibration float center_x = model.cx(); @@ -64,18 +64,18 @@ void convertDepth( float constant_x = unit_scaling / model.fx(); float constant_y = unit_scaling / model.fy(); float bad_point = std::numeric_limits::quiet_NaN(); - - // the following lines ensure that the computation only happens in case we have a default depth - const T default_depth_cvt; - bool use_default_depth = default_depth != 0.0; - if (use_default_depth) { - default_depth_cvt=DepthTraits::fromMeters(default_depth); + + // ensure that the computation only happens in case we have a default depth + T invalid_depth_cvt = T(0); + bool use_invalid_depth = invalid_depth != 0.0; + if (use_invalid_depth) { + invalid_depth_cvt = DepthTraits::fromMeters(invalid_depth); } sensor_msgs::PointCloud2Iterator iter_x(*cloud_msg, "x"); sensor_msgs::PointCloud2Iterator iter_y(*cloud_msg, "y"); sensor_msgs::PointCloud2Iterator iter_z(*cloud_msg, "z"); - // TODO: i think this is undefined behaviour we should favor memcpy? https://stackoverflow.com/questions/55150001/vector-with-reinterpret-cast + // TODO(philipppolterauer): i think this is undefined behaviour we should favor memcpy? https://stackoverflow.com/questions/55150001/vector-with-reinterpret-cast const T * depth_row = reinterpret_cast(&depth_msg->data[0]); uint32_t row_step = depth_msg->step / sizeof(T); for (uint32_t v = 0; v < cloud_msg->height; ++v, depth_row += row_step) { @@ -84,8 +84,8 @@ void convertDepth( // Missing points denoted by NaNs if (!DepthTraits::valid(depth)) { - if (use_default_depth) { - depth = default_depth_cvt; + if (use_invalid_depth) { + depth = invalid_depth_cvt; } else { *iter_x = *iter_y = *iter_z = bad_point; continue; @@ -104,8 +104,8 @@ void convertDepth( template void convertDepthRadial( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, - sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, - cv::Mat & transform) + const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + const cv::Mat & transform) { // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) float bad_point = std::numeric_limits::quiet_NaN(); @@ -137,7 +137,7 @@ void convertDepthRadial( template void convertIntensity( const sensor_msgs::msg::Image::ConstSharedPtr & intensity_msg, - sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg) + const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg) { sensor_msgs::PointCloud2Iterator iter_i(*cloud_msg, "intensity"); const T * inten_row = reinterpret_cast(&intensity_msg->data[0]); @@ -153,7 +153,7 @@ void convertIntensity( // Handles RGB8, BGR8, and MONO8 void convertRgb( const sensor_msgs::msg::Image::ConstSharedPtr & rgb_msg, - sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg, int red_offset, int green_offset, int blue_offset, int color_step); cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial); diff --git a/depth_image_proc/src/conversions.cpp b/depth_image_proc/src/conversions.cpp index ba462323f..3fd19f871 100644 --- a/depth_image_proc/src/conversions.cpp +++ b/depth_image_proc/src/conversions.cpp @@ -79,7 +79,7 @@ cv::Mat initMatrix( void convertRgb( const sensor_msgs::msg::Image::ConstSharedPtr & rgb_msg, - sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, int red_offset, int green_offset, int blue_offset, int color_step) { sensor_msgs::PointCloud2Iterator iter_r(*cloud_msg, "r"); @@ -101,7 +101,7 @@ void convertRgb( // force template instantiation template void convertDepth( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, - sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, const image_geometry::PinholeCameraModel & model, double range_max); diff --git a/depth_image_proc/src/point_cloud_xyz.cpp b/depth_image_proc/src/point_cloud_xyz.cpp index 94f318713..227a4100b 100644 --- a/depth_image_proc/src/point_cloud_xyz.cpp +++ b/depth_image_proc/src/point_cloud_xyz.cpp @@ -59,8 +59,8 @@ PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options) queue_size_ = this->declare_parameter("queue_size", 5); // values used for invalid points for pcd conversion - invalid_depth_ = this->declare_parameter("invalid_depth_", 0.0); - + invalid_depth_ = this->declare_parameter("invalid_depth", 0.0); + // Create publisher with connect callback rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = @@ -97,7 +97,7 @@ void PointCloudXyzNode::depthCb( const Image::ConstSharedPtr & depth_msg, const CameraInfo::ConstSharedPtr & info_msg) { - auto cloud_msg = std::make_shared(); + const PointCloud2::SharedPtr cloud_msg = std::make_shared(); cloud_msg->header = depth_msg->header; cloud_msg->height = depth_msg->height; cloud_msg->width = depth_msg->width; From 5513acc8367aa1693b5f5cc56ad98cad5bd79734 Mon Sep 17 00:00:00 2001 From: Philipp Polterauer Date: Sat, 17 Feb 2024 17:03:11 +0100 Subject: [PATCH 3/6] removed folders from gitignore --- .gitignore | 3 --- 1 file changed, 3 deletions(-) diff --git a/.gitignore b/.gitignore index a3df3ca4e..189be055b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,3 @@ *pyc .vscode/ */doc/generated -/build/ -/log/ -/install/ \ No newline at end of file From afab6c0940d0dc553a2a8e6e36a91a35787aec84 Mon Sep 17 00:00:00 2001 From: "philipp.polterauer" <45892981+PhilippPolterauer@users.noreply.github.com> Date: Tue, 20 Feb 2024 17:55:38 +0100 Subject: [PATCH 4/6] Update depth_image_proc/include/depth_image_proc/conversions.hpp --- depth_image_proc/include/depth_image_proc/conversions.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/depth_image_proc/include/depth_image_proc/conversions.hpp b/depth_image_proc/include/depth_image_proc/conversions.hpp index 27dbf7b18..8af177bd0 100644 --- a/depth_image_proc/include/depth_image_proc/conversions.hpp +++ b/depth_image_proc/include/depth_image_proc/conversions.hpp @@ -75,7 +75,6 @@ void convertDepth( sensor_msgs::PointCloud2Iterator iter_y(*cloud_msg, "y"); sensor_msgs::PointCloud2Iterator iter_z(*cloud_msg, "z"); - // TODO(philipppolterauer): i think this is undefined behaviour we should favor memcpy? https://stackoverflow.com/questions/55150001/vector-with-reinterpret-cast const T * depth_row = reinterpret_cast(&depth_msg->data[0]); uint32_t row_step = depth_msg->step / sizeof(T); for (uint32_t v = 0; v < cloud_msg->height; ++v, depth_row += row_step) { From cea2d65e78b30849e03328d9a4d6cb30aa503112 Mon Sep 17 00:00:00 2001 From: Philipp Polterauer Date: Tue, 20 Feb 2024 18:16:45 +0100 Subject: [PATCH 5/6] added invalid_depth parameter to all other nodes --- .../include/depth_image_proc/point_cloud_xyzi.hpp | 3 +++ .../include/depth_image_proc/point_cloud_xyzrgb.hpp | 3 +++ depth_image_proc/src/conversions.cpp | 2 +- depth_image_proc/src/point_cloud_xyzi.cpp | 7 +++++-- depth_image_proc/src/point_cloud_xyzrgb.cpp | 7 +++++-- 5 files changed, 17 insertions(+), 5 deletions(-) diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp index 1a9e754a2..cb079cc1d 100644 --- a/depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp +++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp @@ -71,6 +71,9 @@ class PointCloudXyziNode : public rclcpp::Node using Synchronizer = message_filters::Synchronizer; std::shared_ptr sync_; + // parameters + float invalid_depth_; + // Publications std::mutex connect_mutex_; rclcpp::Publisher::SharedPtr pub_point_cloud_; diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp index f2b99c165..47c7e0cba 100644 --- a/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp +++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp @@ -75,6 +75,9 @@ class PointCloudXyzrgbNode : public rclcpp::Node std::shared_ptr sync_; std::shared_ptr exact_sync_; + // parameters + float invalid_depth_; + // Publications std::mutex connect_mutex_; rclcpp::Publisher::SharedPtr pub_point_cloud_; diff --git a/depth_image_proc/src/conversions.cpp b/depth_image_proc/src/conversions.cpp index 3fd19f871..697734414 100644 --- a/depth_image_proc/src/conversions.cpp +++ b/depth_image_proc/src/conversions.cpp @@ -103,6 +103,6 @@ template void convertDepth( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, const image_geometry::PinholeCameraModel & model, - double range_max); + double invalid_depth); } // namespace depth_image_proc diff --git a/depth_image_proc/src/point_cloud_xyzi.cpp b/depth_image_proc/src/point_cloud_xyzi.cpp index 9b373d86d..0abc0ff8c 100644 --- a/depth_image_proc/src/point_cloud_xyzi.cpp +++ b/depth_image_proc/src/point_cloud_xyzi.cpp @@ -58,6 +58,9 @@ PointCloudXyziNode::PointCloudXyziNode(const rclcpp::NodeOptions & options) this->declare_parameter("image_transport", "raw"); this->declare_parameter("depth_image_transport", "raw"); + // value used for invalid points for pcd conversion + invalid_depth_ = this->declare_parameter("invalid_depth", 0.0); + // Read parameters int queue_size = this->declare_parameter("queue_size", 5); @@ -202,9 +205,9 @@ void PointCloudXyziNode::imageCb( // Convert Depth Image to Pointcloud if (depth_msg->encoding == enc::TYPE_16UC1) { - convertDepth(depth_msg, cloud_msg, model_); + convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); } else if (depth_msg->encoding == enc::TYPE_32FC1) { - convertDepth(depth_msg, cloud_msg, model_); + convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); } else { RCLCPP_ERROR( get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); diff --git a/depth_image_proc/src/point_cloud_xyzrgb.cpp b/depth_image_proc/src/point_cloud_xyzrgb.cpp index 3347b11e1..5ef736d96 100644 --- a/depth_image_proc/src/point_cloud_xyzrgb.cpp +++ b/depth_image_proc/src/point_cloud_xyzrgb.cpp @@ -55,6 +55,9 @@ PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions & options) this->declare_parameter("image_transport", "raw"); this->declare_parameter("depth_image_transport", "raw"); + // value used for invalid points for pcd conversion + invalid_depth_ = this->declare_parameter("invalid_depth", 0.0); + // Read parameters int queue_size = this->declare_parameter("queue_size", 5); bool use_exact_sync = this->declare_parameter("exact_sync", false); @@ -256,9 +259,9 @@ void PointCloudXyzrgbNode::imageCb( // Convert Depth Image to Pointcloud if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) { - convertDepth(depth_msg, cloud_msg, model_); + convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); } else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) { - convertDepth(depth_msg, cloud_msg, model_); + convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); } else { RCLCPP_ERROR( get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); From 5b8122f13f4d8e89cbcf7b4db2fc32a8846bed73 Mon Sep 17 00:00:00 2001 From: Philipp Polterauer Date: Tue, 20 Feb 2024 19:49:55 +0100 Subject: [PATCH 6/6] added inavlid_depth parameter in doc --- depth_image_proc/doc/components.rst | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/depth_image_proc/doc/components.rst b/depth_image_proc/doc/components.rst index c0a783c60..19d870a73 100644 --- a/depth_image_proc/doc/components.rst +++ b/depth_image_proc/doc/components.rst @@ -116,6 +116,8 @@ Parameters for the depth topic subscriber. * **queue_size** (int, default: 5): Size of message queue for synchronizing subscribed topics. + * **invalid_depth** (double, default: 0.0): Value used for replacing invalid depth + values (if 0.0 the parameter has no effect). depth_image_proc::PointCloudXyzRadialNode ----------------------------------------- @@ -165,6 +167,8 @@ Parameters the intensity image subscriber. * **queue_size** (int, default: 5): Size of message queue for synchronizing subscribed topics. + * **invalid_depth** (double, default: 0.0): Value used for replacing invalid depth + values (if 0.0 the parameter has no effect). depth_image_proc::PointCloudXyziRadialNode ------------------------------------------ @@ -235,6 +239,8 @@ Parameters * **exact_sync** (bool, default: False): Whether to use exact synchronizer. * **queue_size** (int, default: 5): Size of message queue for synchronizing subscribed topics. + * **invalid_depth** (double, default: 0.0): Value used for replacing invalid depth + values (if 0.0 the parameter has no effect). depth_image_proc::PointCloudXyzrgbRadialNode --------------------------------------------