From ef9359200a7abc95f9d5ea262d5f0797aa144e06 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Fri, 25 Aug 2023 11:22:38 +0900
Subject: [PATCH 01/16] fix: roiCallback for Image

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 .../CMakeLists.txt                            |   6 +
 .../fusion_node.hpp                           |  20 +--
 .../pointpainting_fusion/node.hpp             |   3 +-
 .../roi_cluster_fusion/node.hpp               |   3 +-
 .../roi_detected_object_fusion/node.hpp       |   3 +-
 .../segmentation_pointcloud_fusion/node.hpp   |  59 +++++++++
 .../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 +-
 .../segmentation_pointcloud_fusion/node.cpp   | 125 ++++++++++++++++++
 11 files changed, 242 insertions(+), 40 deletions(-)
 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/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 53aafa44b75d6..477698e0b1872 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/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp
index 5174aebe069a8..4db12ab345727 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 <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 #include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
 
@@ -32,6 +33,9 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
@@ -49,13 +53,13 @@ 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<pcl::PointXYZ>;
-using autoware_auto_perception_msgs::msg::ObjectClassification;
-
-template <class Msg, class ObjType>
+template <class Msg, class ObjType, class Msg2D>
 class FusionNode : public rclcpp::Node
 {
 public:
@@ -78,12 +82,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 +115,7 @@ class FusionNode : public rclcpp::Node
 
   /** \brief A vector of subscriber. */
   typename rclcpp::Subscription<Msg>::SharedPtr sub_;
-  std::vector<rclcpp::Subscription<DetectedObjectsWithFeature>::SharedPtr> rois_subs_;
+  std::vector<typename rclcpp::Subscription<Msg2D>::SharedPtr> rois_subs_;
 
   // offsets between cameras and the lidars
   std::vector<double> input_offset_ms_;
@@ -119,7 +123,7 @@ class FusionNode : public rclcpp::Node
   // cache for fusion
   std::vector<bool> is_fused_;
   std::pair<int64_t, typename Msg::SharedPtr> sub_std_pair_;
-  std::vector<std::map<int64_t, DetectedObjectsWithFeature::ConstSharedPtr>> roi_stdmap_;
+  std::vector<std::map<int64_t, typename Msg2D::ConstSharedPtr>> roi_stdmap_;
   std::mutex mutex_;
 
   // 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<sensor_msgs::msg::PointCloud2, DetectedObjects>
+class PointPaintingFusionNode
+: public FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjects, DetectedObjectsWithFeature>
 {
 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<std::string, uint8_t> IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}};
 
 class RoiClusterFusionNode
-: public FusionNode<DetectedObjectsWithFeature, DetectedObjectWithFeature>
+: 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<DetectedObjects, DetectedObject>
+class RoiDetectedObjectFusionNode
+: public FusionNode<DetectedObjects, DetectedObject, DetectedObjectsWithFeature>
 {
 public:
   explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options);
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..0b776a38c90b3
--- /dev/null
+++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp
@@ -0,0 +1,59 @@
+// 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 <string>
+#include <vector>
+
+#if __has_include(<cv_bridge/cv_bridge.hpp>)
+#include <cv_bridge/cv_bridge.hpp>
+#else
+#include <cv_bridge/cv_bridge.h>
+#endif
+
+
+namespace image_projection_based_fusion
+{
+class SegmentPointCloudFusionNode : public FusionNode<PointCloud2, PointCloud2, Image>
+{
+private:
+  int min_cluster_size_{1};
+  bool fuse_unknown_only_{true};
+  double cluster_2d_tolerance_;
+
+  rclcpp::Publisher<PointCloud2>::SharedPtr pub_pointcloud_ptr_;
+  /* 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/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp
index a540688f7e751..aa2a5890dc09c 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 <class Msg, class ObjType>
-FusionNode<Msg, ObjType>::FusionNode(
+template <class Msg, class ObjType, class Msg2D>
+FusionNode<Msg, ObjType, Msg2D>::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<Msg, ObjType>::FusionNode(
   }
 
   input_offset_ms_ = declare_parameter("input_offset_ms", std::vector<double>{});
-  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<Msg, ObjType>::FusionNode(
   roi_stdmap_.resize(rois_number_);
   is_fused_.resize(rois_number_, false);
   for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) {
-    std::function<void(const DetectedObjectsWithFeature::ConstSharedPtr msg)> roi_callback =
+    std::function<void(const typename Msg2D::ConstSharedPtr msg)> roi_callback =
       std::bind(&FusionNode::roiCallback, this, std::placeholders::_1, roi_i);
-    rois_subs_.at(roi_i) = this->create_subscription<DetectedObjectsWithFeature>(
+    rois_subs_.at(roi_i) = this->create_subscription<Msg2D>(
       input_rois_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback);
   }
 
@@ -146,22 +146,22 @@ FusionNode<Msg, ObjType>::FusionNode(
   filter_scope_maxz_ = declare_parameter("filter_scope_maxz", 100);
 }
 
-template <class Msg, class Obj>
-void FusionNode<Msg, Obj>::cameraInfoCallback(
+template <class Msg, class Obj, class Msg2D>
+void FusionNode<Msg, Obj, Msg2D>::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 <class Msg, class Obj>
-void FusionNode<Msg, Obj>::preprocess(Msg & ouput_msg __attribute__((unused)))
+template <class Msg, class Obj, class Msg2D>
+void FusionNode<Msg, Obj, Msg2D>::preprocess(Msg & ouput_msg __attribute__((unused)))
 {
   // do nothing by default
 }
 
-template <class Msg, class Obj>
-void FusionNode<Msg, Obj>::subCallback(const typename Msg::ConstSharedPtr input_msg)
+template <class Msg, class Obj, class Msg2D>
+void FusionNode<Msg, Obj, Msg2D>::subCallback(const typename Msg::ConstSharedPtr input_msg)
 {
   if (sub_std_pair_.second != nullptr) {
     stop_watch_ptr_->toc("processing_time", true);
@@ -283,9 +283,9 @@ void FusionNode<Msg, Obj>::subCallback(const typename Msg::ConstSharedPtr input_
   }
 }
 
-template <class Msg, class Obj>
-void FusionNode<Msg, Obj>::roiCallback(
-  const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i)
+template <class Msg, class Obj, class Msg2D>
+void FusionNode<Msg, Obj, Msg2D>::roiCallback(
+  const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i)
 {
   stop_watch_ptr_->toc("processing_time", true);
 
@@ -348,14 +348,14 @@ void FusionNode<Msg, Obj>::roiCallback(
   (roi_stdmap_.at(roi_i))[timestamp_nsec] = input_roi_msg;
 }
 
-template <class Msg, class Obj>
-void FusionNode<Msg, Obj>::postprocess(Msg & output_msg __attribute__((unused)))
+template <class Msg, class Obj, class Msg2D>
+void FusionNode<Msg, Obj, Msg2D>::postprocess(Msg & output_msg __attribute__((unused)))
 {
   // do nothing by default
 }
 
-template <class Msg, class Obj>
-void FusionNode<Msg, Obj>::timer_callback()
+template <class Msg, class Obj, class Msg2D>
+void FusionNode<Msg, Obj, Msg2D>::timer_callback()
 {
   using std::chrono_literals::operator""ms;
   timer_->cancel();
@@ -393,8 +393,8 @@ void FusionNode<Msg, Obj>::timer_callback()
   }
 }
 
-template <class Msg, class Obj>
-void FusionNode<Msg, Obj>::setPeriod(const int64_t new_period)
+template <class Msg, class Obj, class Msg2D>
+void FusionNode<Msg, Obj, Msg2D>::setPeriod(const int64_t new_period)
 {
   if (!timer_) {
     return;
@@ -410,8 +410,8 @@ void FusionNode<Msg, Obj>::setPeriod(const int64_t new_period)
   }
 }
 
-template <class Msg, class Obj>
-void FusionNode<Msg, Obj>::publish(const Msg & output_msg)
+template <class Msg, class Obj, class Msg2D>
+void FusionNode<Msg, Obj, Msg2D>::publish(const Msg & output_msg)
 {
   if (pub_ptr_->get_subscription_count() < 1) {
     return;
@@ -419,8 +419,10 @@ void FusionNode<Msg, Obj>::publish(const Msg & output_msg)
   pub_ptr_->publish(output_msg);
 }
 
-template class FusionNode<DetectedObjects, DetectedObject>;
-template class FusionNode<DetectedObjectsWithFeature, DetectedObjectWithFeature>;
-template class FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjects>;
-template class FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjectWithFeature>;
+template class FusionNode<DetectedObjects, DetectedObject, DetectedObjectsWithFeature>;
+template class FusionNode<
+  DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>;
+template class FusionNode<
+  sensor_msgs::msg::PointCloud2, DetectedObjects, DetectedObjectsWithFeature>;
+template class FusionNode<PointCloud2, PointCloud2, Image>;
 }  // 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 9f8ef7b94a123..2b5839000a105 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<sensor_msgs::msg::PointCloud2, DetectedObjects>("pointpainting_fusion", options)
+: FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjects, DetectedObjectsWithFeature>(
+    "pointpainting_fusion", options)
 {
   omp_num_threads_ = this->declare_parameter<int>("omp_num_threads", 1);
   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 4a46c8aace696..2043e2f3fcdaa 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<DetectedObjectsWithFeature, DetectedObjectWithFeature>("roi_cluster_fusion", options)
+: FusionNode<DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>(
+    "roi_cluster_fusion", options)
 {
   trust_object_iou_mode_ = declare_parameter<std::string>("trust_object_iou_mode");
   non_trust_object_iou_mode_ = declare_parameter<std::string>("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<DetectedObjects, DetectedObject>("roi_detected_object_fusion", options)
+: FusionNode<DetectedObjects, DetectedObject, DetectedObjectsWithFeature>(
+    "roi_detected_object_fusion", options)
 {
   fusion_params_.passthrough_lower_bound_probability_thresholds =
     declare_parameter<std::vector<double>>("passthrough_lower_bound_probability_thresholds");
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..7076572c49c00
--- /dev/null
+++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
@@ -0,0 +1,125 @@
+// 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 <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
+#endif
+
+namespace image_projection_based_fusion
+{
+SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options)
+: FusionNode<PointCloud2, PointCloud2, Image>("segmentation_pointcloud_fusion", options)
+{
+  fuse_unknown_only_ = static_cast<bool>(declare_parameter("fuse_unknown_only", true));
+  min_cluster_size_ = static_cast<int>(declare_parameter("min_cluster_size", 2));
+  cluster_2d_tolerance_ = declare_parameter<double>("cluster_2d_tolerance", 0.5);
+  pub_pointcloud_ptr_ = this->create_publisher<PointCloud2>("output_objects", rclcpp::QoS{1});
+}
+
+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,
+  __attribute__((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(input_mask,sensor_msgs::image_encodings::MONO8);
+  }
+  catch(const std::exception& e)
+  {
+    RCLCPP_ERROR(this->get_logger(),"cv_bridge exception:%s", e.what());
+    return;
+  }
+  
+  cv::Mat mask = in_image_ptr->image;
+
+  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);
+  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<float> 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) {
+    if (*iter_z <= 0.0) {
+      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());
+    
+    if(mask.at<uint8_t>(static_cast<int>(normalized_projected_point.x()), static_cast<int>(normalized_projected_point.y())) >0){
+      output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y,*iter_orig_z));
+    }
+  }
+
+  // TODO : popup point which is matched with vegetation and road
+  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_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::SegmentPointCloudFusionNode)

From fcb816966018496f6b9f493a1f08756611a3fbba Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Sat, 26 Aug 2023 12:00:31 +0900
Subject: [PATCH 02/16] fix: pointcloud filter

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 .../segmentation_pointcloud_fusion/node.hpp   |   5 +-
 .../segmentation_pointcloud_fusion.launch.xml | 100 ++++++++++++++++++
 .../segmentation_pointcloud_fusion/node.cpp   |  47 ++++----
 3 files changed, 127 insertions(+), 25 deletions(-)
 create mode 100644 perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml

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
index 0b776a38c90b3..b4251fdc119fb 100644
--- 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
@@ -15,7 +15,6 @@
 #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 <string>
@@ -27,7 +26,6 @@
 #include <cv_bridge/cv_bridge.h>
 #endif
 
-
 namespace image_projection_based_fusion
 {
 class SegmentPointCloudFusionNode : public FusionNode<PointCloud2, PointCloud2, Image>
@@ -48,8 +46,7 @@ class SegmentPointCloudFusionNode : public FusionNode<PointCloud2, PointCloud2,
   void postprocess(PointCloud2 & pointcloud_msg) override;
 
   void fuseOnSingleImage(
-    const PointCloud2 & input_pointcloud_msg, const std::size_t image_id,
-    const Image & input_mask,
+    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);
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..847dafc521893
--- /dev/null
+++ b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml
@@ -0,0 +1,100 @@
+<launch>
+  <arg name="input/rois_number" default="1"/>
+  <arg name="input/rois0" default="/perception/object_recognition/detection/mask0"/>
+  <arg name="input/camera_info0" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="input/rois1" default="/perception/object_recognition/detection/mask1"/>
+  <arg name="input/camera_info1" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="input/rois2" default="/perception/object_recognition/detection/mask2"/>
+  <arg name="input/camera_info2" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="input/rois3" default="/perception/object_recognition/detection/mask3"/>
+  <arg name="input/camera_info3" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="input/rois4" default="/perception/object_recognition/detection/mask4"/>
+  <arg name="input/camera_info4" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="input/rois5" default="/perception/object_recognition/detection/mask5"/>
+  <arg name="input/camera_info5" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="input/rois6" default="/perception/object_recognition/detection/mask6"/>
+  <arg name="input/camera_info6" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="input/rois7" default="/perception/object_recognition/detection/mask7"/>
+  <arg name="input/camera_info7" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="input/pointcloud" default="/sensing/lidar/top/outlier_filtered/pointcloud"/>
+  <arg name="output/pointcloud" default="output/pointcloud"/>
+  <arg name="sync_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_sync.param.yaml"/>
+  <arg name="remove_unknown" default="true"/>
+  <arg name="trust_distance" default="100.0"/>
+
+  <!-- for eval variable-->
+  <arg name="input_rois_number" default="$(var input/rois_number)"/>
+
+  <!-- debug -->
+  <!-- cspell: ignore minx, maxx, miny, maxy, minz, maxz  -->
+  <arg name="debug_mode" default="false"/>
+
+  <arg name="filter_scope_minx" default="-100"/>
+  <arg name="filter_scope_maxx" default="100"/>
+  <arg name="filter_scope_miny" default="-100"/>
+  <arg name="filter_scope_maxy" default="100"/>
+  <arg name="filter_scope_minz" default="-100"/>
+  <arg name="filter_scope_maxz" default="100"/>
+  <arg name="image_buffer_size" default="15"/>
+  <arg name="input/image0" default="/image_raw0"/>
+  <arg name="input/image1" default="/image_raw1"/>
+  <arg name="input/image2" default="/image_raw2"/>
+  <arg name="input/image3" default="/image_raw3"/>
+  <arg name="input/image4" default="/image_raw4"/>
+  <arg name="input/image5" default="/image_raw5"/>
+  <arg name="input/image6" default="/image_raw6"/>
+  <arg name="input/image7" default="/image_raw7"/>
+  <group>
+    <node pkg="image_projection_based_fusion" exec="segmentation_pointcloud_fusion_node" name="segmentation_pointcloud_fusion" output="screen">
+      <param name="use_iou" value="true"/>
+      <param name="use_iou_x" value="false"/>
+      <param name="use_iou_y" value="false"/>
+      <param name="only_allow_inside_cluster" value="true"/>
+      <param name="roi_scale_factor" value="1.1"/>
+      <param name="iou_threshold" value="0.35"/>
+      <param name="unknown_iou_threshold" value="0.1"/>
+      <param name="rois_number" value="$(var input/rois_number)"/>
+      <param name="remove_unknown" value="$(var remove_unknown)"/>
+      <param name="trust_distance" value="$(var trust_distance)"/>
+      <param from="$(var sync_param_path)"/>
+      <remap from="input" to="$(var input/pointcloud)"/>
+      <remap from="output" to="$(var output/pointcloud)"/>
+
+      <!-- rois, camera and info -->
+      <param name="input/rois0" value="$(var input/rois0)"/>
+      <param name="input/camera_info0" value="$(var input/camera_info0)"/>
+      <param name="input/image0" value="$(var input/image0)"/>
+      <param name="input/rois1" value="$(var input/rois1)"/>
+      <param name="input/camera_info1" value="$(var input/camera_info1)"/>
+      <param name="input/image1" value="$(var input/image1)"/>
+      <param name="input/rois2" value="$(var input/rois2)"/>
+      <param name="input/camera_info2" value="$(var input/camera_info2)"/>
+      <param name="input/image2" value="$(var input/image2)"/>
+      <param name="input/rois3" value="$(var input/rois3)"/>
+      <param name="input/camera_info3" value="$(var input/camera_info3)"/>
+      <param name="input/image3" value="$(var input/image3)"/>
+      <param name="input/rois4" value="$(var input/rois4)"/>
+      <param name="input/camera_info4" value="$(var input/camera_info4)"/>
+      <param name="input/image4" value="$(var input/image4)"/>
+      <param name="input/rois5" value="$(var input/rois5)"/>
+      <param name="input/camera_info5" value="$(var input/camera_info5)"/>
+      <param name="input/image5" value="$(var input/image5)"/>
+      <param name="input/rois6" value="$(var input/rois6)"/>
+      <param name="input/camera_info6" value="$(var input/camera_info6)"/>
+      <param name="input/image6" value="$(var input/image6)"/>
+      <param name="input/rois7" value="$(var input/rois7)"/>
+      <param name="input/camera_info7" value="$(var input/camera_info7)"/>
+      <param name="input/image7" value="$(var input/image7)"/>
+
+      <!-- debug -->
+      <param name="debug_mode" value="$(var debug_mode)"/>
+      <param name="filter_scope_minx" value="$(var filter_scope_minx)"/>
+      <param name="filter_scope_maxx" value="$(var filter_scope_maxx)"/>
+      <param name="filter_scope_miny" value="$(var filter_scope_miny)"/>
+      <param name="filter_scope_maxy" value="$(var filter_scope_maxy)"/>
+      <param name="filter_scope_minz" value="$(var filter_scope_minz)"/>
+      <param name="filter_scope_maxz" value="$(var filter_scope_maxz)"/>
+      <param name="image_buffer_size" value="$(var image_buffer_size)"/>
+    </node>
+  </group>
+</launch>
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
index 7076572c49c00..716e7e5c51b7a 100644
--- a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
+++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
@@ -36,20 +36,17 @@ SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptio
   pub_pointcloud_ptr_ = this->create_publisher<PointCloud2>("output_objects", rclcpp::QoS{1});
 }
 
-void SegmentPointCloudFusionNode::preprocess(__attribute__((unused))
-                                         PointCloud2 & pointcloud_msg)
+void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg)
 {
   return;
 }
 
-void SegmentPointCloudFusionNode::postprocess(__attribute__((unused))
-                                          PointCloud2 & pointcloud_msg)
+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,
+  const PointCloud2 & input_pointcloud_msg, __attribute__((unused)) const std::size_t image_id,
   __attribute__((unused)) const Image & input_mask,
   __attribute__((unused)) const CameraInfo & camera_info,
   __attribute__((unused)) PointCloud2 & output_pointcloud_msg)
@@ -58,16 +55,13 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
     return;
   }
   cv_bridge::CvImagePtr in_image_ptr;
-  try
-  {
-    in_image_ptr = cv_bridge::toCvCopy(input_mask,sensor_msgs::image_encodings::MONO8);
-  }
-  catch(const std::exception& e)
-  {
-    RCLCPP_ERROR(this->get_logger(),"cv_bridge exception:%s", e.what());
+  try {
+    in_image_ptr = cv_bridge::toCvCopy(input_mask, sensor_msgs::image_encodings::MONO8);
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(this->get_logger(), "cv_bridge exception:%s", e.what());
     return;
   }
-  
+
   cv::Mat mask = in_image_ptr->image;
 
   Eigen::Matrix4d projection;
@@ -88,7 +82,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
 
   PointCloud2 transformed_cloud;
   tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped);
-  
+
   PointCloud output_cloud;
 
   for (sensor_msgs::PointCloud2ConstIterator<float> iter_x(transformed_cloud, "x"),
@@ -103,19 +97,30 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
     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());
-    
-    if(mask.at<uint8_t>(static_cast<int>(normalized_projected_point.x()), static_cast<int>(normalized_projected_point.y())) >0){
-      output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y,*iter_orig_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));
+    // }
+    // TODO(badai-nguyen): replace single road class to a list of outlier classes
+    bool is_keep_class = mask.at<uint8_t>(
+                           static_cast<int>(normalized_projected_point.x()),
+                           static_cast<int>(normalized_projected_point.y())) > 10;
+
+    if (is_inside_image && is_keep_class) {
+      output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z));
     }
   }
 
   // TODO : popup point which is matched with vegetation and road
-  pcl::toROSMsg(output_cloud,output_pointcloud_msg);
+  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)
+bool SegmentPointCloudFusionNode::out_of_scope(__attribute__((unused))
+                                               const PointCloud2 & filtered_cloud)
 {
   return false;
 }

From 94ba048e9a72e005cbb03e600efea4705f8358c3 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Tue, 29 Aug 2023 10:37:39 +0900
Subject: [PATCH 03/16] fix: pointcloud filter bugfix

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 .../config/roi_sync.param.yaml                |  2 +-
 .../segmentation_pointcloud_fusion/node.hpp   |  4 ---
 .../segmentation_pointcloud_fusion/node.cpp   | 33 ++++++++++---------
 3 files changed, 18 insertions(+), 21 deletions(-)

diff --git a/perception/image_projection_based_fusion/config/roi_sync.param.yaml b/perception/image_projection_based_fusion/config/roi_sync.param.yaml
index 21ba13787f1c0..56e156930ca21 100644
--- a/perception/image_projection_based_fusion/config/roi_sync.param.yaml
+++ b/perception/image_projection_based_fusion/config/roi_sync.param.yaml
@@ -2,4 +2,4 @@
   ros__parameters:
     input_offset_ms: [61.67, 111.67, 45.0, 28.33, 78.33, 95.0]
     timeout_ms: 70.0
-    match_threshold_ms: 50.0
+    match_threshold_ms: 80.0
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
index b4251fdc119fb..f6700bde85102 100644
--- 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
@@ -31,10 +31,6 @@ namespace image_projection_based_fusion
 class SegmentPointCloudFusionNode : public FusionNode<PointCloud2, PointCloud2, Image>
 {
 private:
-  int min_cluster_size_{1};
-  bool fuse_unknown_only_{true};
-  double cluster_2d_tolerance_;
-
   rclcpp::Publisher<PointCloud2>::SharedPtr pub_pointcloud_ptr_;
   /* data */
 public:
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
index 716e7e5c51b7a..bd800412cc761 100644
--- a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
+++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
@@ -30,10 +30,6 @@ namespace image_projection_based_fusion
 SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options)
 : FusionNode<PointCloud2, PointCloud2, Image>("segmentation_pointcloud_fusion", options)
 {
-  fuse_unknown_only_ = static_cast<bool>(declare_parameter("fuse_unknown_only", true));
-  min_cluster_size_ = static_cast<int>(declare_parameter("min_cluster_size", 2));
-  cluster_2d_tolerance_ = declare_parameter<double>("cluster_2d_tolerance", 0.5);
-  pub_pointcloud_ptr_ = this->create_publisher<PointCloud2>("output_objects", rclcpp::QoS{1});
 }
 
 void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg)
@@ -56,18 +52,22 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
   }
   cv_bridge::CvImagePtr in_image_ptr;
   try {
-    in_image_ptr = cv_bridge::toCvCopy(input_mask, sensor_msgs::image_encodings::MONO8);
+    in_image_ptr = cv_bridge::toCvCopy(
+      std::make_shared<sensor_msgs::msg::Image>(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);
+    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
   {
@@ -92,6 +92,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
        iter_x != iter_x.end();
        ++iter_x, ++iter_y, ++iter_z, ++iter_orig_x, ++iter_orig_y, ++iter_orig_z) {
     if (*iter_z <= 0.0) {
+      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);
@@ -99,22 +100,22 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
       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));
-    // }
+      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;
+    }
     // TODO(badai-nguyen): replace single road class to a list of outlier classes
     bool is_keep_class = mask.at<uint8_t>(
-                           static_cast<int>(normalized_projected_point.x()),
-                           static_cast<int>(normalized_projected_point.y())) > 10;
+                           static_cast<uint16_t>(normalized_projected_point.y()),
+                           static_cast<uint16_t>(normalized_projected_point.x())) > 2;
 
-    if (is_inside_image && is_keep_class) {
+    if (is_keep_class) {
       output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z));
     }
   }
 
-  // TODO : popup point which is matched with vegetation and road
   pcl::toROSMsg(output_cloud, output_pointcloud_msg);
   output_pointcloud_msg.header = input_pointcloud_msg.header;
 }

From cfa1d63f69438a8cb5da518a06a480a702bd3faf Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Tue, 29 Aug 2023 14:50:12 +0900
Subject: [PATCH 04/16] fix: segmentation poincloud filter

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 .../image_projection_based_fusion/config/roi_sync.param.yaml    | 2 +-
 .../src/segmentation_pointcloud_fusion/node.cpp                 | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/perception/image_projection_based_fusion/config/roi_sync.param.yaml b/perception/image_projection_based_fusion/config/roi_sync.param.yaml
index 56e156930ca21..5a6dea5445307 100644
--- a/perception/image_projection_based_fusion/config/roi_sync.param.yaml
+++ b/perception/image_projection_based_fusion/config/roi_sync.param.yaml
@@ -2,4 +2,4 @@
   ros__parameters:
     input_offset_ms: [61.67, 111.67, 45.0, 28.33, 78.33, 95.0]
     timeout_ms: 70.0
-    match_threshold_ms: 80.0
+    match_threshold_ms: 90.0
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
index bd800412cc761..233d9b700e47a 100644
--- a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
+++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
@@ -109,7 +109,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
     // TODO(badai-nguyen): replace single road class to a list of outlier classes
     bool is_keep_class = mask.at<uint8_t>(
                            static_cast<uint16_t>(normalized_projected_point.y()),
-                           static_cast<uint16_t>(normalized_projected_point.x())) > 2;
+                           static_cast<uint16_t>(normalized_projected_point.x())) > 10;
 
     if (is_keep_class) {
       output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z));

From 73e4fd83495504c8b5ea806d9f325d229358b6f3 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Wed, 18 Oct 2023 00:04:22 +0900
Subject: [PATCH 05/16] fix: add option in launch file

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 ...ra_lidar_fusion_based_detection.launch.xml | 23 +++++++++++++++++--
 1 file changed, 21 insertions(+), 2 deletions(-)

diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
index 59fe3f13f1231..3941c1b6028e8 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -45,6 +45,7 @@
   <arg name="camera_info7" default="/camera_info7"/>
   <arg name="detection_rois7" default="/perception/object_recognition/detection/rois7"/>
   <arg name="image_number" default="1" description="choose image raw number(1-8)"/>
+  <arg name="use_image_segmentation_filter" default="false"/>
 
   <!-- Jetson AGX -->
   <!-- <include file="$(find-pkg-share tensorrt_yolo)/launch/yolo.launch.xml">
@@ -72,12 +73,30 @@
     </include>
   </group>
 
+  <!-- Pointcloud filtering by image semantic segmentation model -->
+  <group if="$(var use_image_segmentation_filter)">
+    <let name="segmentation_filter/input_pointcloud" value="pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
+    <let name="segmentation_filter/input_pointcloud" value="input/obstacle_segmentation/pointcloud" unless="$(var use_pointcloud_map)"/>
+    <include file="$(find-pkg-share image_projection_based_fusion)/launch/segmentation_pointcloud_fusion.launch.xml">
+      <arg name="input/pointcloud" value="pointcloud_map_filtered/pointcloud"/>
+      <arg name="output/pointcloud" value="image_segmentation_filtered/pointcloud"/>
+    </include>
+  </group>
   <!-- Clustering -->
   <group>
     <push-ros-namespace namespace="clustering"/>
     <group>
-      <let name="euclidean_cluster_output" value="euclidean_cluster/clusters" if="$(var use_roi_based_cluster)"/>
-      <let name="euclidean_cluster_output" value="clusters" unless="$(var use_roi_based_cluster)"/>
+      <let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/image_segmentation_filtered/pointcloud" if="$(var use_image_segmentation_filter)"/>
+      <let
+        name="clustering/input/pointcloud"
+        value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"
+        if="$(eval &quot;'$(var use_image_segmentation_filter)'=='false' and '$(var use_pointcloud_map)'=='true' &quot;)"
+      />
+      <let
+        name="clustering/input/pointcloud"
+        value="$(var input/obstacle_segmentation/pointcloud)"
+        if="$(eval &quot;'$(var use_image_segmentation_filter)'=='false' and '$(var use_pointcloud_map)'=='false' &quot;)"
+      />
       <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
         <arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
         <arg name="output_clusters" value="$(var euclidean_cluster_output)"/>

From 949ac3c25229df3b69a295897d6a47e758d5f3cb Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Fri, 10 Nov 2023 22:05:18 +0900
Subject: [PATCH 06/16] chore: refactor

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 ...ra_lidar_fusion_based_detection.launch.xml |  4 +-
 .../fusion_node.hpp                           |  4 +-
 .../roi_pointcloud_fusion/node.hpp            |  2 +-
 .../segmentation_pointcloud_fusion.launch.xml | 50 +++++++------------
 .../image_projection_based_fusion/package.xml |  1 +
 5 files changed, 24 insertions(+), 37 deletions(-)

diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
index 3941c1b6028e8..70a60d41c8995 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -97,8 +97,10 @@
         value="$(var input/obstacle_segmentation/pointcloud)"
         if="$(eval &quot;'$(var use_image_segmentation_filter)'=='false' and '$(var use_pointcloud_map)'=='false' &quot;)"
       />
+      <let name="euclidean_cluster_output" value="euclidean_cluster/clusters" if="$(var use_roi_based_cluster)"/>
+      <let name="euclidean_cluster_output" value="clusters" unless="$(var use_roi_based_cluster)"/>
       <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
-        <arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
+        <arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
         <arg name="output_clusters" value="$(var euclidean_cluster_output)"/>
         <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       </include>
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 4db12ab345727..dce84ffb6282f 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
@@ -33,9 +33,6 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl_conversions/pcl_conversions.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
@@ -59,6 +56,7 @@ using sensor_msgs::msg::PointCloud2;
 using tier4_perception_msgs::msg::DetectedObjectsWithFeature;
 using tier4_perception_msgs::msg::DetectedObjectWithFeature;
 using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
+using autoware_auto_perception_msgs::msg::ObjectClassification;
 template <class Msg, class ObjType, class Msg2D>
 class FusionNode : public rclcpp::Node
 {
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<sensor_msgs::msg::PointCloud2, DetectedObjectWithFeature>
+: public FusionNode<PointCloud2, DetectedObjectWithFeature, DetectedObjectsWithFeature>
 {
 private:
   int min_cluster_size_{1};
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
index 847dafc521893..76e2fe53aae11 100644
--- a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml
+++ b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml
@@ -1,29 +1,24 @@
 <launch>
-  <arg name="input/rois_number" default="1"/>
-  <arg name="input/rois0" default="/perception/object_recognition/detection/mask0"/>
+  <arg name="input/camera_number" default="1"/>
+  <arg name="input/mask0" default="/perception/object_recognition/detection/mask0"/>
   <arg name="input/camera_info0" default="/sensing/camera/camera0/camera_info"/>
-  <arg name="input/rois1" default="/perception/object_recognition/detection/mask1"/>
+  <arg name="input/mask1" default="/perception/object_recognition/detection/mask1"/>
   <arg name="input/camera_info1" default="/sensing/camera/camera0/camera_info"/>
-  <arg name="input/rois2" default="/perception/object_recognition/detection/mask2"/>
+  <arg name="input/mask2" default="/perception/object_recognition/detection/mask2"/>
   <arg name="input/camera_info2" default="/sensing/camera/camera0/camera_info"/>
-  <arg name="input/rois3" default="/perception/object_recognition/detection/mask3"/>
+  <arg name="input/mask3" default="/perception/object_recognition/detection/mask3"/>
   <arg name="input/camera_info3" default="/sensing/camera/camera0/camera_info"/>
-  <arg name="input/rois4" default="/perception/object_recognition/detection/mask4"/>
+  <arg name="input/mask4" default="/perception/object_recognition/detection/mask4"/>
   <arg name="input/camera_info4" default="/sensing/camera/camera0/camera_info"/>
-  <arg name="input/rois5" default="/perception/object_recognition/detection/mask5"/>
+  <arg name="input/mask5" default="/perception/object_recognition/detection/mask5"/>
   <arg name="input/camera_info5" default="/sensing/camera/camera0/camera_info"/>
-  <arg name="input/rois6" default="/perception/object_recognition/detection/mask6"/>
+  <arg name="input/mask6" default="/perception/object_recognition/detection/mask6"/>
   <arg name="input/camera_info6" default="/sensing/camera/camera0/camera_info"/>
-  <arg name="input/rois7" default="/perception/object_recognition/detection/mask7"/>
+  <arg name="input/mask7" default="/perception/object_recognition/detection/mask7"/>
   <arg name="input/camera_info7" default="/sensing/camera/camera0/camera_info"/>
   <arg name="input/pointcloud" default="/sensing/lidar/top/outlier_filtered/pointcloud"/>
   <arg name="output/pointcloud" default="output/pointcloud"/>
   <arg name="sync_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_sync.param.yaml"/>
-  <arg name="remove_unknown" default="true"/>
-  <arg name="trust_distance" default="100.0"/>
-
-  <!-- for eval variable-->
-  <arg name="input_rois_number" default="$(var input/rois_number)"/>
 
   <!-- debug -->
   <!-- cspell: ignore minx, maxx, miny, maxy, minz, maxz  -->
@@ -46,43 +41,34 @@
   <arg name="input/image7" default="/image_raw7"/>
   <group>
     <node pkg="image_projection_based_fusion" exec="segmentation_pointcloud_fusion_node" name="segmentation_pointcloud_fusion" output="screen">
-      <param name="use_iou" value="true"/>
-      <param name="use_iou_x" value="false"/>
-      <param name="use_iou_y" value="false"/>
-      <param name="only_allow_inside_cluster" value="true"/>
-      <param name="roi_scale_factor" value="1.1"/>
-      <param name="iou_threshold" value="0.35"/>
-      <param name="unknown_iou_threshold" value="0.1"/>
-      <param name="rois_number" value="$(var input/rois_number)"/>
-      <param name="remove_unknown" value="$(var remove_unknown)"/>
-      <param name="trust_distance" value="$(var trust_distance)"/>
+      <param name="rois_number" value="$(var input/camera_number)"/>
       <param from="$(var sync_param_path)"/>
       <remap from="input" to="$(var input/pointcloud)"/>
       <remap from="output" to="$(var output/pointcloud)"/>
 
       <!-- rois, camera and info -->
-      <param name="input/rois0" value="$(var input/rois0)"/>
+      <param name="input/rois0" value="$(var input/mask0)"/>
       <param name="input/camera_info0" value="$(var input/camera_info0)"/>
       <param name="input/image0" value="$(var input/image0)"/>
-      <param name="input/rois1" value="$(var input/rois1)"/>
+      <param name="input/rois1" value="$(var input/mask1)"/>
       <param name="input/camera_info1" value="$(var input/camera_info1)"/>
       <param name="input/image1" value="$(var input/image1)"/>
-      <param name="input/rois2" value="$(var input/rois2)"/>
+      <param name="input/rois2" value="$(var input/mask2)"/>
       <param name="input/camera_info2" value="$(var input/camera_info2)"/>
       <param name="input/image2" value="$(var input/image2)"/>
-      <param name="input/rois3" value="$(var input/rois3)"/>
+      <param name="input/rois3" value="$(var input/mask3)"/>
       <param name="input/camera_info3" value="$(var input/camera_info3)"/>
       <param name="input/image3" value="$(var input/image3)"/>
-      <param name="input/rois4" value="$(var input/rois4)"/>
+      <param name="input/rois4" value="$(var input/mask4)"/>
       <param name="input/camera_info4" value="$(var input/camera_info4)"/>
       <param name="input/image4" value="$(var input/image4)"/>
-      <param name="input/rois5" value="$(var input/rois5)"/>
+      <param name="input/rois5" value="$(var input/mask5)"/>
       <param name="input/camera_info5" value="$(var input/camera_info5)"/>
       <param name="input/image5" value="$(var input/image5)"/>
-      <param name="input/rois6" value="$(var input/rois6)"/>
+      <param name="input/rois6" value="$(var input/mask6)"/>
       <param name="input/camera_info6" value="$(var input/camera_info6)"/>
       <param name="input/image6" value="$(var input/image6)"/>
-      <param name="input/rois7" value="$(var input/rois7)"/>
+      <param name="input/rois7" value="$(var input/mask7)"/>
       <param name="input/camera_info7" value="$(var input/camera_info7)"/>
       <param name="input/image7" value="$(var input/image7)"/>
 
diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml
index 5ff99af2ebb21..6ace2de55ee28 100644
--- a/perception/image_projection_based_fusion/package.xml
+++ b/perception/image_projection_based_fusion/package.xml
@@ -6,6 +6,7 @@
   <description>The image_projection_based_fusion package</description>
   <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
   <maintainer email="yusuke.muramatsu@tier4.jp">Yusuke Muramatsu</maintainer>
+  <maintainer email="dai.nguyen@tier4.jp">badai nguyen</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

From 0702bf6782cb2b3258ed23a72f0702800ae44f5c Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Fri, 10 Nov 2023 22:05:43 +0900
Subject: [PATCH 07/16] docs: update docs

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 .../docs/segmentation-pointcloud-fusion.md    | 87 +++++++++++++++++++
 1 file changed, 87 insertions(+)
 create mode 100644 perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md

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
+
+<!-- Write assumptions and limitations of your implementation.
+
+Example:
+  This algorithm assumes obstacles are not moving, so if they rapidly move after the vehicle started to avoid them, it might collide with them.
+  Also, this algorithm doesn't care about blind spots. In general, since too close obstacles aren't visible due to the sensing performance limit, please take enough margin to obstacles.
+-->
+
+## (Optional) Error detection and handling
+
+<!-- Write how to detect errors and how to recover from them.
+
+Example:
+  This package can handle up to 20 obstacles. If more obstacles found, this node will give up and raise diagnostic errors.
+-->
+
+## (Optional) Performance characterization
+
+<!-- Write performance information like complexity. If it wouldn't be the bottleneck, not necessary.
+
+Example:
+
+  ### Complexity
+
+  This algorithm is O(N).
+
+  ### Processing time
+
+  ...
+-->
+
+## (Optional) References/External links
+
+<!-- Write links you referred to when you implemented.
+
+Example:
+  [1] {link_to_a_thesis}
+  [2] {link_to_an_issue}
+-->
+
+## (Optional) Future extensions / Unimplemented parts
+
+<!-- Write future extensions of this package.
+
+Example:
+  Currently, this package can't handle the chattering obstacles well. We plan to add some probabilistic filters in the perception layer to improve it.
+  Also, there are some parameters that should be global(e.g. vehicle size, max steering, etc.). These will be refactored and defined as global parameters so that we can share the same parameters between different nodes.
+-->

From 55da06628edd462a84e7c1fd290fc39a8900cfc8 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Sun, 12 Nov 2023 16:59:46 +0900
Subject: [PATCH 08/16] fix: roi_pointcloud_fusion with new template

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 perception/image_projection_based_fusion/src/fusion_node.cpp  | 4 ++--
 .../src/roi_pointcloud_fusion/node.cpp                        | 3 ++-
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp
index aa2a5890dc09c..d8928315522c4 100644
--- a/perception/image_projection_based_fusion/src/fusion_node.cpp
+++ b/perception/image_projection_based_fusion/src/fusion_node.cpp
@@ -422,7 +422,7 @@ void FusionNode<Msg, Obj, Msg2D>::publish(const Msg & output_msg)
 template class FusionNode<DetectedObjects, DetectedObject, DetectedObjectsWithFeature>;
 template class FusionNode<
   DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>;
-template class FusionNode<
-  sensor_msgs::msg::PointCloud2, DetectedObjects, DetectedObjectsWithFeature>;
+template class FusionNode<PointCloud2, DetectedObjects, DetectedObjectsWithFeature>;
+template class FusionNode<PointCloud2, DetectedObjectWithFeature, DetectedObjectsWithFeature>;
 template class FusionNode<PointCloud2, PointCloud2, Image>;
 }  // namespace image_projection_based_fusion
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<PointCloud2, DetectedObjectWithFeature>("roi_pointcloud_fusion", options)
+: FusionNode<PointCloud2, DetectedObjectWithFeature, DetectedObjectsWithFeature>(
+    "roi_pointcloud_fusion", options)
 {
   fuse_unknown_only_ = declare_parameter<bool>("fuse_unknown_only");
   min_cluster_size_ = declare_parameter<int>("min_cluster_size");

From d4ebbb8f554502b67672de3cd90841525d2ab7be Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Tue, 21 Nov 2023 10:51:29 +0900
Subject: [PATCH 09/16] fix: add param of selectable semantic id

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 .../segmentation_pointcloud_fusion.param.yaml | 30 +++++++++++++++++++
 .../segmentation_pointcloud_fusion/node.hpp   |  2 ++
 .../segmentation_pointcloud_fusion.launch.xml |  3 +-
 .../segmentation_pointcloud_fusion/node.cpp   | 21 +++++++++----
 4 files changed, 49 insertions(+), 7 deletions(-)
 create mode 100644 perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml

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/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
index f6700bde85102..4168c483469ab 100644
--- 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
@@ -32,6 +32,8 @@ class SegmentPointCloudFusionNode : public FusionNode<PointCloud2, PointCloud2,
 {
 private:
   rclcpp::Publisher<PointCloud2>::SharedPtr pub_pointcloud_ptr_;
+  std::vector<bool> filter_semantic_label_target_;
+  float filter_distance_threshold_;
   /* data */
 public:
   explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options);
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
index 76e2fe53aae11..96bf47594bfe8 100644
--- a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml
+++ b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml
@@ -19,7 +19,7 @@
   <arg name="input/pointcloud" default="/sensing/lidar/top/outlier_filtered/pointcloud"/>
   <arg name="output/pointcloud" default="output/pointcloud"/>
   <arg name="sync_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_sync.param.yaml"/>
-
+  <arg name="semantic_segmentation_based_filter_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/segmentation_pointcloud_fusion.param.yaml"/>
   <!-- debug -->
   <!-- cspell: ignore minx, maxx, miny, maxy, minz, maxz  -->
   <arg name="debug_mode" default="false"/>
@@ -42,6 +42,7 @@
   <group>
     <node pkg="image_projection_based_fusion" exec="segmentation_pointcloud_fusion_node" name="segmentation_pointcloud_fusion" output="screen">
       <param name="rois_number" value="$(var input/camera_number)"/>
+      <param from="$(var semantic_segmentation_based_filter_param_path)"/>
       <param from="$(var sync_param_path)"/>
       <remap from="input" to="$(var input/pointcloud)"/>
       <remap from="output" to="$(var output/pointcloud)"/>
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
index 233d9b700e47a..c037c8dfaba1f 100644
--- a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
+++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
@@ -30,6 +30,9 @@ namespace image_projection_based_fusion
 SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options)
 : FusionNode<PointCloud2, PointCloud2, Image>("segmentation_pointcloud_fusion", options)
 {
+  filter_distance_threshold_ = declare_parameter<float>("filter_distance_threshold");
+  filter_semantic_label_target_ =
+    declare_parameter<std::vector<bool>>("filter_semantic_label_target");
 }
 
 void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg)
@@ -91,10 +94,12 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
        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) {
-    if (*iter_z <= 0.0) {
+    // 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());
@@ -106,12 +111,16 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
       output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z));
       continue;
     }
-    // TODO(badai-nguyen): replace single road class to a list of outlier classes
-    bool is_keep_class = mask.at<uint8_t>(
-                           static_cast<uint16_t>(normalized_projected_point.y()),
-                           static_cast<uint16_t>(normalized_projected_point.x())) > 10;
 
-    if (is_keep_class) {
+    // skip filtering pointcloud where semantic id out of the defined list
+    uint8_t semantic_id = mask.at<uint8_t>(
+      static_cast<uint16_t>(normalized_projected_point.y()),
+      static_cast<uint16_t>(normalized_projected_point.x()));
+    if (static_cast<size_t>(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));
     }
   }

From 2ff0ff3160df7e52bea7cec9a9096792691ab8a8 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Wed, 22 Nov 2023 00:49:21 +0900
Subject: [PATCH 10/16] chore: typo

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 .../image_projection_based_fusion/config/roi_sync.param.yaml    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/perception/image_projection_based_fusion/config/roi_sync.param.yaml b/perception/image_projection_based_fusion/config/roi_sync.param.yaml
index 5a6dea5445307..21ba13787f1c0 100644
--- a/perception/image_projection_based_fusion/config/roi_sync.param.yaml
+++ b/perception/image_projection_based_fusion/config/roi_sync.param.yaml
@@ -2,4 +2,4 @@
   ros__parameters:
     input_offset_ms: [61.67, 111.67, 45.0, 28.33, 78.33, 95.0]
     timeout_ms: 70.0
-    match_threshold_ms: 90.0
+    match_threshold_ms: 50.0

From 5dfa160964c6db2bcdb402bf47a2ebe30f177010 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Wed, 22 Nov 2023 01:14:17 +0900
Subject: [PATCH 11/16] fix: launch file

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 ...ra_lidar_fusion_based_detection.launch.xml |  2 +-
 ...ar_radar_fusion_based_detection.launch.xml | 24 ++++++++++++++++++-
 .../launch/perception.launch.xml              |  1 +
 3 files changed, 25 insertions(+), 2 deletions(-)

diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
index 70a60d41c8995..bc3b45b13bf0d 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -18,6 +18,7 @@
   <arg name="fusion_distance" default="100.0"/>
   <arg name="trust_object_distance" default="100.0"/>
   <arg name="use_roi_based_cluster" default="false"/>
+  <arg name="use_image_segmentation_filter" default="false" description="use image sematic segmentation based pointcloud filter"/>
 
   <!-- Camera parameters -->
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
@@ -45,7 +46,6 @@
   <arg name="camera_info7" default="/camera_info7"/>
   <arg name="detection_rois7" default="/perception/object_recognition/detection/rois7"/>
   <arg name="image_number" default="1" description="choose image raw number(1-8)"/>
-  <arg name="use_image_segmentation_filter" default="false"/>
 
   <!-- Jetson AGX -->
   <!-- <include file="$(find-pkg-share tensorrt_yolo)/launch/yolo.launch.xml">
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
index 11deaad5d06cc..5d23cc582a287 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
@@ -17,6 +17,7 @@
   <arg name="remove_unknown" default="true"/>
   <arg name="fusion_distance" default="100.0"/>
   <arg name="trust_object_distance" default="100.0"/>
+  <arg name="use_image_segmentation_filter" default="false" description="use image sematic segmentation based pointcloud filter"/>
 
   <!-- Camera parameters -->
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
@@ -97,14 +98,35 @@
     </include>
   </group>
 
+  <!-- Pointcloud filtering by image semantic segmentation model -->
+  <group if="$(var use_image_segmentation_filter)">
+    <let name="segmentation_filter/input_pointcloud" value="pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
+    <let name="segmentation_filter/input_pointcloud" value="input/obstacle_segmentation/pointcloud" unless="$(var use_pointcloud_map)"/>
+    <include file="$(find-pkg-share image_projection_based_fusion)/launch/segmentation_pointcloud_fusion.launch.xml">
+      <arg name="input/pointcloud" value="pointcloud_map_filtered/pointcloud"/>
+      <arg name="output/pointcloud" value="image_segmentation_filtered/pointcloud"/>
+    </include>
+  </group>
+
   <!-- Clustering -->
   <group>
     <push-ros-namespace namespace="clustering"/>
     <group>
+      <let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/image_segmentation_filtered/pointcloud" if="$(var use_image_segmentation_filter)"/>
+      <let
+        name="clustering/input/pointcloud"
+        value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"
+        if="$(eval &quot;'$(var use_image_segmentation_filter)'=='false' and '$(var use_pointcloud_map)'=='true' &quot;)"
+      />
+      <let
+        name="clustering/input/pointcloud"
+        value="$(var input/obstacle_segmentation/pointcloud)"
+        if="$(eval &quot;'$(var use_image_segmentation_filter)'=='false' and '$(var use_pointcloud_map)'=='false' &quot;)"
+      />
       <let name="euclidean_cluster_output" value="euclidean_cluster/clusters" if="$(var use_roi_based_cluster)"/>
       <let name="euclidean_cluster_output" value="clusters" unless="$(var use_roi_based_cluster)"/>
       <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
-        <arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
+        <arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
         <arg name="output_clusters" value="$(var euclidean_cluster_output)"/>
         <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       </include>
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index 347606d91759f..a18868ec563fc 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -63,6 +63,7 @@
   <arg name="use_low_height_cropbox" default="true" description="use low height crop filter in clustering"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_roi_based_cluster" default="true" description="use roi_based_cluster in clustering"/>
+  <arg name="use_image_segmentation_filter" default="false" description="use image sematic segmentation based pointcloud filter"/>
   <arg
     name="use_empty_dynamic_object_publisher"
     default="false"

From b999c788e342b5909b3d0d9e4518cc590135876c Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Tue, 26 Dec 2023 15:55:22 +0900
Subject: [PATCH 12/16] Revert "fix: launch file"

This reverts commit 5dfa160964c6db2bcdb402bf47a2ebe30f177010.
---
 ...ra_lidar_fusion_based_detection.launch.xml |  2 +-
 ...ar_radar_fusion_based_detection.launch.xml | 24 +------------------
 .../launch/perception.launch.xml              |  1 -
 3 files changed, 2 insertions(+), 25 deletions(-)

diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
index bc3b45b13bf0d..70a60d41c8995 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -18,7 +18,6 @@
   <arg name="fusion_distance" default="100.0"/>
   <arg name="trust_object_distance" default="100.0"/>
   <arg name="use_roi_based_cluster" default="false"/>
-  <arg name="use_image_segmentation_filter" default="false" description="use image sematic segmentation based pointcloud filter"/>
 
   <!-- Camera parameters -->
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
@@ -46,6 +45,7 @@
   <arg name="camera_info7" default="/camera_info7"/>
   <arg name="detection_rois7" default="/perception/object_recognition/detection/rois7"/>
   <arg name="image_number" default="1" description="choose image raw number(1-8)"/>
+  <arg name="use_image_segmentation_filter" default="false"/>
 
   <!-- Jetson AGX -->
   <!-- <include file="$(find-pkg-share tensorrt_yolo)/launch/yolo.launch.xml">
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
index 5d23cc582a287..11deaad5d06cc 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
@@ -17,7 +17,6 @@
   <arg name="remove_unknown" default="true"/>
   <arg name="fusion_distance" default="100.0"/>
   <arg name="trust_object_distance" default="100.0"/>
-  <arg name="use_image_segmentation_filter" default="false" description="use image sematic segmentation based pointcloud filter"/>
 
   <!-- Camera parameters -->
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
@@ -98,35 +97,14 @@
     </include>
   </group>
 
-  <!-- Pointcloud filtering by image semantic segmentation model -->
-  <group if="$(var use_image_segmentation_filter)">
-    <let name="segmentation_filter/input_pointcloud" value="pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
-    <let name="segmentation_filter/input_pointcloud" value="input/obstacle_segmentation/pointcloud" unless="$(var use_pointcloud_map)"/>
-    <include file="$(find-pkg-share image_projection_based_fusion)/launch/segmentation_pointcloud_fusion.launch.xml">
-      <arg name="input/pointcloud" value="pointcloud_map_filtered/pointcloud"/>
-      <arg name="output/pointcloud" value="image_segmentation_filtered/pointcloud"/>
-    </include>
-  </group>
-
   <!-- Clustering -->
   <group>
     <push-ros-namespace namespace="clustering"/>
     <group>
-      <let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/image_segmentation_filtered/pointcloud" if="$(var use_image_segmentation_filter)"/>
-      <let
-        name="clustering/input/pointcloud"
-        value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"
-        if="$(eval &quot;'$(var use_image_segmentation_filter)'=='false' and '$(var use_pointcloud_map)'=='true' &quot;)"
-      />
-      <let
-        name="clustering/input/pointcloud"
-        value="$(var input/obstacle_segmentation/pointcloud)"
-        if="$(eval &quot;'$(var use_image_segmentation_filter)'=='false' and '$(var use_pointcloud_map)'=='false' &quot;)"
-      />
       <let name="euclidean_cluster_output" value="euclidean_cluster/clusters" if="$(var use_roi_based_cluster)"/>
       <let name="euclidean_cluster_output" value="clusters" unless="$(var use_roi_based_cluster)"/>
       <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
-        <arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
+        <arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
         <arg name="output_clusters" value="$(var euclidean_cluster_output)"/>
         <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       </include>
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index a18868ec563fc..347606d91759f 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -63,7 +63,6 @@
   <arg name="use_low_height_cropbox" default="true" description="use low height crop filter in clustering"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_roi_based_cluster" default="true" description="use roi_based_cluster in clustering"/>
-  <arg name="use_image_segmentation_filter" default="false" description="use image sematic segmentation based pointcloud filter"/>
   <arg
     name="use_empty_dynamic_object_publisher"
     default="false"

From 6a9331ab1ed1da673da6d619252a444fda4fb2e4 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Tue, 26 Dec 2023 15:57:12 +0900
Subject: [PATCH 13/16] fix: reverse perception launcher

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 ...ra_lidar_fusion_based_detection.launch.xml | 29 +++----------------
 1 file changed, 4 insertions(+), 25 deletions(-)

diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
index 70a60d41c8995..227aac50d6d90 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -7,10 +7,8 @@
   <arg name="input/pointcloud"/>
   <arg name="input/obstacle_segmentation/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
   <arg name="container_name" default="pointcloud_container"/>
-
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <arg name="score_threshold" default="0.35"/>
-
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
@@ -45,7 +43,6 @@
   <arg name="camera_info7" default="/camera_info7"/>
   <arg name="detection_rois7" default="/perception/object_recognition/detection/rois7"/>
   <arg name="image_number" default="1" description="choose image raw number(1-8)"/>
-  <arg name="use_image_segmentation_filter" default="false"/>
 
   <!-- Jetson AGX -->
   <!-- <include file="$(find-pkg-share tensorrt_yolo)/launch/yolo.launch.xml">
@@ -73,34 +70,14 @@
     </include>
   </group>
 
-  <!-- Pointcloud filtering by image semantic segmentation model -->
-  <group if="$(var use_image_segmentation_filter)">
-    <let name="segmentation_filter/input_pointcloud" value="pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
-    <let name="segmentation_filter/input_pointcloud" value="input/obstacle_segmentation/pointcloud" unless="$(var use_pointcloud_map)"/>
-    <include file="$(find-pkg-share image_projection_based_fusion)/launch/segmentation_pointcloud_fusion.launch.xml">
-      <arg name="input/pointcloud" value="pointcloud_map_filtered/pointcloud"/>
-      <arg name="output/pointcloud" value="image_segmentation_filtered/pointcloud"/>
-    </include>
-  </group>
   <!-- Clustering -->
   <group>
     <push-ros-namespace namespace="clustering"/>
     <group>
-      <let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/image_segmentation_filtered/pointcloud" if="$(var use_image_segmentation_filter)"/>
-      <let
-        name="clustering/input/pointcloud"
-        value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"
-        if="$(eval &quot;'$(var use_image_segmentation_filter)'=='false' and '$(var use_pointcloud_map)'=='true' &quot;)"
-      />
-      <let
-        name="clustering/input/pointcloud"
-        value="$(var input/obstacle_segmentation/pointcloud)"
-        if="$(eval &quot;'$(var use_image_segmentation_filter)'=='false' and '$(var use_pointcloud_map)'=='false' &quot;)"
-      />
       <let name="euclidean_cluster_output" value="euclidean_cluster/clusters" if="$(var use_roi_based_cluster)"/>
       <let name="euclidean_cluster_output" value="clusters" unless="$(var use_roi_based_cluster)"/>
       <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
-        <arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
+        <arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
         <arg name="output_clusters" value="$(var euclidean_cluster_output)"/>
         <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       </include>
@@ -215,7 +192,9 @@
   <!-- DetectionByTracker -->
   <group if="$(var use_detection_by_tracker)">
     <push-ros-namespace namespace="detection_by_tracker"/>
-    <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml"/>
+    <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml">
+      <arg name="detection_by_tracker_param_path" value="$(var object_recognition_detection_detection_by_tracker_param)"/>
+    </include>
   </group>
 
   <!-- CenterPoint -->

From 4c0f3534bd2a47f49ccb7c4083611aa583e654fa Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Tue, 26 Dec 2023 07:18:19 +0000
Subject: [PATCH 14/16] style(pre-commit): autofix

---
 .../include/image_projection_based_fusion/fusion_node.hpp        | 1 -
 1 file changed, 1 deletion(-)

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 85b510b25face..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
@@ -125,7 +125,6 @@ class FusionNode : public rclcpp::Node
   std::vector<std::map<int64_t, typename Msg2D::ConstSharedPtr>> cached_roi_msgs_;
   std::mutex mutex_cached_msgs_;
 
-
   // output publisher
   typename rclcpp::Publisher<Msg>::SharedPtr pub_ptr_;
 

From 1499f3007030787249d1491c0425d8284c0ed28a Mon Sep 17 00:00:00 2001
From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com>
Date: Tue, 9 Jan 2024 23:49:43 +0900
Subject: [PATCH 15/16] Update
 perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp

Co-authored-by: Yoshi Ri <yoshiyoshidetteiu@gmail.com>
---
 .../src/segmentation_pointcloud_fusion/node.cpp                 | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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
index c037c8dfaba1f..d635c8a71e1ea 100644
--- a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
+++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
@@ -46,7 +46,7 @@ void SegmentPointCloudFusionNode::postprocess(__attribute__((unused)) PointCloud
 }
 void SegmentPointCloudFusionNode::fuseOnSingleImage(
   const PointCloud2 & input_pointcloud_msg, __attribute__((unused)) const std::size_t image_id,
-  __attribute__((unused)) const Image & input_mask,
+  [[maybe_unused]] const Image & input_mask,
   __attribute__((unused)) const CameraInfo & camera_info,
   __attribute__((unused)) PointCloud2 & output_pointcloud_msg)
 {

From ef36ead5e127eb063acc10877e331062ce45faae Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Tue, 9 Jan 2024 14:52:08 +0000
Subject: [PATCH 16/16] style(pre-commit): autofix

---
 .../src/segmentation_pointcloud_fusion/node.cpp                | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

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
index d635c8a71e1ea..1c9c865b8d21e 100644
--- a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
+++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
@@ -46,8 +46,7 @@ void SegmentPointCloudFusionNode::postprocess(__attribute__((unused)) PointCloud
 }
 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,
+  [[maybe_unused]] const Image & input_mask, __attribute__((unused)) const CameraInfo & camera_info,
   __attribute__((unused)) PointCloud2 & output_pointcloud_msg)
 {
   if (input_pointcloud_msg.data.empty()) {