From 39e9acc2fa06ef211b2e17a5381b026a03102997 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Wed, 10 Jan 2024 01:50:08 +0900 Subject: [PATCH] feat(image_projection_based_fusion): add image segmentation_pointcloud_fusion for pointcloud filter (#5562) * fix: roiCallback for Image Signed-off-by: badai-nguyen * fix: pointcloud filter Signed-off-by: badai-nguyen * fix: pointcloud filter bugfix Signed-off-by: badai-nguyen * fix: segmentation poincloud filter Signed-off-by: badai-nguyen * fix: add option in launch file Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen * docs: update docs Signed-off-by: badai-nguyen * fix: roi_pointcloud_fusion with new template Signed-off-by: badai-nguyen * fix: add param of selectable semantic id Signed-off-by: badai-nguyen * chore: typo Signed-off-by: badai-nguyen * fix: launch file Signed-off-by: badai-nguyen * Revert "fix: launch file" This reverts commit 5dfa160964c6db2bcdb402bf47a2ebe30f177010. * fix: reverse perception launcher Signed-off-by: badai-nguyen * style(pre-commit): autofix * Update perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp Co-authored-by: Yoshi Ri * style(pre-commit): autofix --------- Signed-off-by: badai-nguyen Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yoshi Ri --- .../CMakeLists.txt | 6 + .../segmentation_pointcloud_fusion.param.yaml | 30 ++++ .../docs/segmentation-pointcloud-fusion.md | 87 +++++++++++ .../fusion_node.hpp | 16 +- .../pointpainting_fusion/node.hpp | 3 +- .../roi_cluster_fusion/node.hpp | 3 +- .../roi_detected_object_fusion/node.hpp | 3 +- .../roi_pointcloud_fusion/node.hpp | 2 +- .../segmentation_pointcloud_fusion/node.hpp | 54 +++++++ .../segmentation_pointcloud_fusion.launch.xml | 87 +++++++++++ .../src/fusion_node.cpp | 54 +++---- .../src/pointpainting_fusion/node.cpp | 3 +- .../src/roi_cluster_fusion/node.cpp | 3 +- .../src/roi_detected_object_fusion/node.cpp | 3 +- .../src/roi_pointcloud_fusion/node.cpp | 3 +- .../segmentation_pointcloud_fusion/node.cpp | 139 ++++++++++++++++++ 16 files changed, 455 insertions(+), 41 deletions(-) create mode 100644 perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml create mode 100644 perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md create mode 100644 perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp create mode 100644 perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml create mode 100644 perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index c021dd92dae64..b13f68b07181e 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -22,6 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/utils.cpp src/roi_cluster_fusion/node.cpp src/roi_detected_object_fusion/node.cpp + src/segmentation_pointcloud_fusion/node.cpp src/roi_pointcloud_fusion/node.cpp ) @@ -40,6 +41,11 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE roi_cluster_fusion_node ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "image_projection_based_fusion::SegmentPointCloudFusionNode" + EXECUTABLE segmentation_pointcloud_fusion_node +) + rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "image_projection_based_fusion::RoiPointCloudFusionNode" EXECUTABLE roi_pointcloud_fusion_node diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml new file mode 100644 index 0000000000000..914ad13519807 --- /dev/null +++ b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -0,0 +1,30 @@ +/**: + ros__parameters: + # if the semantic label is applied for pointcloud filtering + filter_semantic_label_target: + [ + true, # road + true, # sidewalk + true, # building + true, # wall + true, # fence + true, # pole + true, # traffic_light + true, # traffic_sign + true, # vegetation + true, # terrain + true, # sky + false, # person + false, # ride + false, # car + false, # truck + false, # bus + false, # train + false, # motorcycle + false, # bicycle + false, # others + ] + # the maximum distance of pointcloud to be applied filter, + # this is selected based on semantic segmentation model accuracy, + # calibration accuracy and unknown reaction distance + filter_distance_threshold: 60.0 diff --git a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md new file mode 100644 index 0000000000000..77188aafb3b22 --- /dev/null +++ b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md @@ -0,0 +1,87 @@ +# segmentation_pointcloud_fusion + +## Purpose + +The node `segmentation_pointcloud_fusion` is a package for filtering pointcloud that are belong to less interesting region which is defined by semantic or instance segmentation by 2D image segmentation model. + +## Inner-workings / Algorithms + +- The pointclouds are projected onto the semantic/instance segmentation mask image which is the output from 2D segmentation neural network. +- The pointclouds are belong to segment such as road, sidewalk, building, vegetation, sky ... are considered as less important points for autonomous driving and could be removed. + +![segmentation_pointcloud_fusion_image](./images/segmentation_pointcloud_fusion.png) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------ | ----------------------------------- | --------------------------------------------------------- | +| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud | +| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes | +| `input/rois[0-7]` | `tier4_perception_msgs::msg::Image` | semantic segmentation mask image | +| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | + +### Output + +| Name | Type | Description | +| -------- | ------------------------------- | -------------------------- | +| `output` | `sensor_msgs::msg::PointCloud2` | output filtered pointcloud | + +## Parameters + +### Core Parameters + +| Name | Type | Description | +| ------------- | ---- | ------------------------ | +| `rois_number` | int | the number of input rois | + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index 9572e62e0e0aa..fe8acb6746dba 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -49,13 +50,14 @@ namespace image_projection_based_fusion { using autoware_auto_perception_msgs::msg::DetectedObject; using autoware_auto_perception_msgs::msg::DetectedObjects; +using sensor_msgs::msg::CameraInfo; +using sensor_msgs::msg::Image; using sensor_msgs::msg::PointCloud2; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; using PointCloud = pcl::PointCloud; using autoware_auto_perception_msgs::msg::ObjectClassification; - -template +template class FusionNode : public rclcpp::Node { public: @@ -78,12 +80,12 @@ class FusionNode : public rclcpp::Node virtual void subCallback(const typename Msg::ConstSharedPtr input_msg); // callback for roi subscription + virtual void roiCallback( - const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i); + const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); virtual void fuseOnSingleImage( - const Msg & input_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, + const Msg & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg, const sensor_msgs::msg::CameraInfo & camera_info, Msg & output_msg) = 0; // set args if you need @@ -111,7 +113,7 @@ class FusionNode : public rclcpp::Node /** \brief A vector of subscriber. */ typename rclcpp::Subscription::SharedPtr sub_; - std::vector::SharedPtr> rois_subs_; + std::vector::SharedPtr> rois_subs_; // offsets between cameras and the lidars std::vector input_offset_ms_; @@ -120,7 +122,7 @@ class FusionNode : public rclcpp::Node std::vector is_fused_; std::pair cached_msg_; // first element is the timestamp in nanoseconds, second element is the message - std::vector> cached_roi_msgs_; + std::vector> cached_roi_msgs_; std::mutex mutex_cached_msgs_; // output publisher diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp index d7de10fed068f..78ae152141a00 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -33,7 +33,8 @@ namespace image_projection_based_fusion { using Label = autoware_auto_perception_msgs::msg::ObjectClassification; -class PointPaintingFusionNode : public FusionNode +class PointPaintingFusionNode +: public FusionNode { public: explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp index e54710ad477da..b9471f2f3b78e 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -25,7 +25,8 @@ namespace image_projection_based_fusion const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; class RoiClusterFusionNode -: public FusionNode +: public FusionNode< + DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature> { public: explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index e11a62c060480..7d7132309fb91 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -29,7 +29,8 @@ namespace image_projection_based_fusion using sensor_msgs::msg::RegionOfInterest; -class RoiDetectedObjectFusionNode : public FusionNode +class RoiDetectedObjectFusionNode +: public FusionNode { public: explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index 2b4eb822a9edb..fe685baa0b6e2 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -22,7 +22,7 @@ namespace image_projection_based_fusion { class RoiPointCloudFusionNode -: public FusionNode +: public FusionNode { private: int min_cluster_size_{1}; diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp new file mode 100644 index 0000000000000..4168c483469ab --- /dev/null +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ +#define IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ + +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include + +#if __has_include() +#include +#else +#include +#endif + +namespace image_projection_based_fusion +{ +class SegmentPointCloudFusionNode : public FusionNode +{ +private: + rclcpp::Publisher::SharedPtr pub_pointcloud_ptr_; + std::vector filter_semantic_label_target_; + float filter_distance_threshold_; + /* data */ +public: + explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); + +protected: + void preprocess(PointCloud2 & pointcloud_msg) override; + + void postprocess(PointCloud2 & pointcloud_msg) override; + + void fuseOnSingleImage( + const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const Image & input_mask, + const CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; + + bool out_of_scope(const PointCloud2 & filtered_cloud); +}; + +} // namespace image_projection_based_fusion +#endif // IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ diff --git a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml new file mode 100644 index 0000000000000..96bf47594bfe8 --- /dev/null +++ b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml @@ -0,0 +1,87 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 4abd92d7fb063..6bced39bc61ef 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -40,8 +40,8 @@ static double processing_time_ms = 0; namespace image_projection_based_fusion { -template -FusionNode::FusionNode( +template +FusionNode::FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { @@ -81,7 +81,7 @@ FusionNode::FusionNode( } input_offset_ms_ = declare_parameter>("input_offset_ms"); - if (!input_offset_ms_.empty() && rois_number_ != input_offset_ms_.size()) { + if (!input_offset_ms_.empty() && rois_number_ > input_offset_ms_.size()) { throw std::runtime_error("The number of offsets does not match the number of topics."); } @@ -99,9 +99,9 @@ FusionNode::FusionNode( cached_roi_msgs_.resize(rois_number_); is_fused_.resize(rois_number_, false); for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - std::function roi_callback = + std::function roi_callback = std::bind(&FusionNode::roiCallback, this, std::placeholders::_1, roi_i); - rois_subs_.at(roi_i) = this->create_subscription( + rois_subs_.at(roi_i) = this->create_subscription( input_rois_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); } @@ -147,22 +147,22 @@ FusionNode::FusionNode( filter_scope_maxz_ = declare_parameter("filter_scope_max_z"); } -template -void FusionNode::cameraInfoCallback( +template +void FusionNode::cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id) { camera_info_map_[camera_id] = *input_camera_info_msg; } -template -void FusionNode::preprocess(Msg & ouput_msg __attribute__((unused))) +template +void FusionNode::preprocess(Msg & ouput_msg __attribute__((unused))) { // do nothing by default } -template -void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_msg) +template +void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_msg) { if (cached_msg_.second != nullptr) { stop_watch_ptr_->toc("processing_time", true); @@ -285,9 +285,9 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ } } -template -void FusionNode::roiCallback( - const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i) +template +void FusionNode::roiCallback( + const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i) { stop_watch_ptr_->toc("processing_time", true); @@ -350,14 +350,14 @@ void FusionNode::roiCallback( (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; } -template -void FusionNode::postprocess(Msg & output_msg __attribute__((unused))) +template +void FusionNode::postprocess(Msg & output_msg __attribute__((unused))) { // do nothing by default } -template -void FusionNode::timer_callback() +template +void FusionNode::timer_callback() { using std::chrono_literals::operator""ms; timer_->cancel(); @@ -395,8 +395,8 @@ void FusionNode::timer_callback() } } -template -void FusionNode::setPeriod(const int64_t new_period) +template +void FusionNode::setPeriod(const int64_t new_period) { if (!timer_) { return; @@ -412,8 +412,8 @@ void FusionNode::setPeriod(const int64_t new_period) } } -template -void FusionNode::publish(const Msg & output_msg) +template +void FusionNode::publish(const Msg & output_msg) { if (pub_ptr_->get_subscription_count() < 1) { return; @@ -421,8 +421,10 @@ void FusionNode::publish(const Msg & output_msg) pub_ptr_->publish(output_msg); } -template class FusionNode; -template class FusionNode; -template class FusionNode; -template class FusionNode; +template class FusionNode; +template class FusionNode< + DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>; +template class FusionNode; +template class FusionNode; +template class FusionNode; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 76b561f677c0f..14783a46ba8b4 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -93,7 +93,8 @@ inline bool isUnknown(int label2d) } PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("pointpainting_fusion", options) +: FusionNode( + "pointpainting_fusion", options) { omp_num_threads_ = this->declare_parameter("omp_params.num_threads"); const float score_threshold = diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 492f832e30e8b..1c064e4d0f060 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -34,7 +34,8 @@ namespace image_projection_based_fusion { RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_cluster_fusion", options) +: FusionNode( + "roi_cluster_fusion", options) { trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); non_trust_object_iou_mode_ = declare_parameter("non_trust_object_iou_mode"); diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 333ec7d7ed206..19defc0a1cab0 100644 --- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -25,7 +25,8 @@ namespace image_projection_based_fusion { RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_detected_object_fusion", options) +: FusionNode( + "roi_detected_object_fusion", options) { fusion_params_.passthrough_lower_bound_probability_thresholds = declare_parameter>("passthrough_lower_bound_probability_thresholds"); diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index 233e568ebee0b..421aa3a453451 100644 --- a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -30,7 +30,8 @@ namespace image_projection_based_fusion { RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_pointcloud_fusion", options) +: FusionNode( + "roi_pointcloud_fusion", options) { fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); min_cluster_size_ = declare_parameter("min_cluster_size"); diff --git a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp new file mode 100644 index 0000000000000..1c9c865b8d21e --- /dev/null +++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -0,0 +1,139 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp" + +#include "image_projection_based_fusion/utils/geometry.hpp" +#include "image_projection_based_fusion/utils/utils.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include +#include +#endif + +namespace image_projection_based_fusion +{ +SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options) +: FusionNode("segmentation_pointcloud_fusion", options) +{ + filter_distance_threshold_ = declare_parameter("filter_distance_threshold"); + filter_semantic_label_target_ = + declare_parameter>("filter_semantic_label_target"); +} + +void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) +{ + return; +} + +void SegmentPointCloudFusionNode::postprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) +{ + return; +} +void SegmentPointCloudFusionNode::fuseOnSingleImage( + const PointCloud2 & input_pointcloud_msg, __attribute__((unused)) const std::size_t image_id, + [[maybe_unused]] const Image & input_mask, __attribute__((unused)) const CameraInfo & camera_info, + __attribute__((unused)) PointCloud2 & output_pointcloud_msg) +{ + if (input_pointcloud_msg.data.empty()) { + return; + } + cv_bridge::CvImagePtr in_image_ptr; + try { + in_image_ptr = cv_bridge::toCvCopy( + std::make_shared(input_mask), input_mask.encoding); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception:%s", e.what()); + return; + } + + cv::Mat mask = in_image_ptr->image; + if (mask.cols == 0 || mask.rows == 0) { + return; + } + Eigen::Matrix4d projection; + projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), + camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), + camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11), 0.0, 0.0, + 0.0, 1.0; + geometry_msgs::msg::TransformStamped transform_stamped; + // transform pointcloud from frame id to camera optical frame id + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer_, input_mask.header.frame_id, input_pointcloud_msg.header.frame_id, + input_pointcloud_msg.header.stamp); + if (!transform_stamped_optional) { + return; + } + transform_stamped = transform_stamped_optional.value(); + } + + PointCloud2 transformed_cloud; + tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); + + PointCloud output_cloud; + + for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cloud, "x"), + iter_y(transformed_cloud, "y"), iter_z(transformed_cloud, "z"), + iter_orig_x(input_pointcloud_msg, "x"), iter_orig_y(input_pointcloud_msg, "y"), + iter_orig_z(input_pointcloud_msg, "z"); + iter_x != iter_x.end(); + ++iter_x, ++iter_y, ++iter_z, ++iter_orig_x, ++iter_orig_y, ++iter_orig_z) { + // skip filtering pointcloud behind the camera or too far from camera + if (*iter_z <= 0.0 || *iter_z > filter_distance_threshold_) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + continue; + } + + Eigen::Vector4d projected_point = projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); + Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( + projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + + bool is_inside_image = + normalized_projected_point.x() > 0 && normalized_projected_point.x() < camera_info.width && + normalized_projected_point.y() > 0 && normalized_projected_point.y() < camera_info.height; + if (!is_inside_image) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + continue; + } + + // skip filtering pointcloud where semantic id out of the defined list + uint8_t semantic_id = mask.at( + static_cast(normalized_projected_point.y()), + static_cast(normalized_projected_point.x())); + if (static_cast(semantic_id) >= filter_semantic_label_target_.size()) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + continue; + } + if (!filter_semantic_label_target_.at(semantic_id)) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + } + } + + pcl::toROSMsg(output_cloud, output_pointcloud_msg); + output_pointcloud_msg.header = input_pointcloud_msg.header; +} + +bool SegmentPointCloudFusionNode::out_of_scope(__attribute__((unused)) + const PointCloud2 & filtered_cloud) +{ + return false; +} +} // namespace image_projection_based_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::SegmentPointCloudFusionNode)