feat(autoware_image_based_projection_fusion): redesign image based projection fusion node

perception/autoware_image_projection_based_fusion/CMakeLists.txt

@@ -17,6 +17,8 @@ endif()
 # Build non-CUDA dependent nodes
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/camera_projection.cpp
+  src/fusion_collector.cpp
+  src/fusion_matching_strategy.cpp
   src/fusion_node.cpp
   src/debugger.cpp
   src/utils/geometry.cpp

perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml

@@ -1,8 +1,8 @@
 /**:
   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
+    rois_timestamp_offsets: [0.06167, 0.011167, 0.045, 0.02833, 0.07833, 0.095]
+    timeout_sec: 0.070
+    # match_threshold_ms: 50.0
     image_buffer_size: 15
     # projection setting for each ROI whether unrectify image
     point_project_to_unrectified_image: [false, false, false, false, false, false]
@@ -22,3 +22,7 @@
 
     # debug parameters
     publish_processing_time_detail: false
+    matching_strategy:
+      type: advanced
+      det3d_noise_window: 0.01
+      rois_timestamp_noise_window: [0.005, 0.005, 0.005, 0.005, 0.005, 0.005]

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp

@@ -0,0 +1,107 @@
+// Copyright 2024 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.
+
+#pragma once
+
+#include "autoware/image_projection_based_fusion/camera_projection.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <cstddef>
+#include <memory>
+#include <mutex>
+#include <unordered_map>
+#include <vector>
+
+namespace autoware::image_projection_based_fusion
+{
+using autoware::image_projection_based_fusion::CameraProjection;
+
+template <class Msg3D, class Msg2D, class ExportObj>
+class FusionNode;
+
+template <class Msg2D>
+struct Det2dStatus
+{
+  // camera index
+  std::size_t id = 0;
+  // camera projection
+  std::shared_ptr<CameraProjection> camera_projector_ptr;
+  bool project_to_unrectified_image = false;
+  bool approximate_camera_projection = false;
+};
+
+struct FusionCollectorInfoBase
+{
+  virtual ~FusionCollectorInfoBase() = default;
+};
+
+struct NaiveCollectorInfo : public FusionCollectorInfoBase
+{
+  double timestamp;
+  double threshold;
+
+  explicit NaiveCollectorInfo(double timestamp = 0.0, double threshold = 0.0)
+  : timestamp(timestamp), threshold(threshold)
+  {
+  }
+};
+
+struct AdvancedCollectorInfo : public FusionCollectorInfoBase
+{
+  double timestamp;
+  double noise_window;
+
+  explicit AdvancedCollectorInfo(double timestamp = 0.0, double noise_window = 0.0)
+  : timestamp(timestamp), noise_window(noise_window)
+  {
+  }
+};
+
+template <class Msg3D, class Msg2D, class ExportObj>
+class FusionCollector
+{
+public:
+  FusionCollector(
+    std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> && ros2_parent_node, double timeout_sec,
+    const std::vector<Det2dStatus<Msg2D>> & det2d_list, bool debug_mode);
+  bool process_msg_3d(const typename Msg3D::ConstSharedPtr msg_3d);
+  bool process_rois(const std::size_t & roi_id, const typename Msg2D::ConstSharedPtr det2d_msg);
+  void fusion_callback();
+
+  [[nodiscard]] bool fusion_finished() const;
+
+  void set_info(std::shared_ptr<FusionCollectorInfoBase> collector_info);
+  [[nodiscard]] std::shared_ptr<FusionCollectorInfoBase> get_info() const;
+  // void show_debug_message();
+  bool ready_to_fuse();
+  bool rois_exists(const std::size_t & rois_id);
+  bool det3d_exists();
+  void show_debug_message();
+
+private:
+  std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> ros2_parent_node_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  double timeout_sec_;
+  typename Msg3D::ConstSharedPtr det3d_msg_{nullptr};
+  std::vector<Det2dStatus<Msg2D>> det2d_list_;
+  std::unordered_map<std::size_t, typename Msg2D::ConstSharedPtr> id_to_roi_map_;
+  double rois_number_;
+  bool debug_mode_;
+  bool fusion_finished_{false};
+  std::mutex fusion_mutex_;
+  std::shared_ptr<FusionCollectorInfoBase> fusion_collector_info_;
+};
+
+}  // namespace autoware::image_projection_based_fusion

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp

@@ -0,0 +1,145 @@
+// Copyright 2025 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.
+
+#pragma once
+
+#include "autoware/image_projection_based_fusion/fusion_collector.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <cstddef>
+#include <list>
+#include <memory>
+#include <optional>
+#include <set>
+#include <string>
+#include <unordered_map>
+
+namespace autoware::image_projection_based_fusion
+{
+
+template <class Msg3D, class Msg2D, class ExportObj>
+class FusionNode;
+
+struct MatchingParamsBase
+{
+  virtual ~MatchingParamsBase() = default;
+};
+
+struct Det3dMatchingParams : public MatchingParamsBase
+{
+  double det3d_timestamp;
+
+  explicit Det3dMatchingParams(double det3d_timestamp = 0.0) : det3d_timestamp(det3d_timestamp) {}
+};
+
+struct RoisMatchingParams : public MatchingParamsBase
+{
+  double rois_timestamp;
+  std::size_t rois_id;
+
+  explicit RoisMatchingParams(double rois_timestamp = 0.0, std::size_t rois_id = 0)
+  : rois_timestamp(rois_timestamp), rois_id(rois_id)
+  {
+  }
+};
+
+template <class Msg3D, class Msg2D, class ExportObj>
+class FusionMatchingStrategy
+{
+public:
+  virtual ~FusionMatchingStrategy() = default;
+
+  [[nodiscard]] virtual std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>>
+  match_rois_to_collector(
+    const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors,
+    const std::shared_ptr<RoisMatchingParams> & params) const = 0;
+
+  [[nodiscard]] virtual std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>>
+  match_det3d_to_collector(
+    const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors,
+    const std::shared_ptr<Det3dMatchingParams> & params) = 0;
+  virtual void set_collector_info(
+    std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>> & collector,
+    const std::shared_ptr<MatchingParamsBase> & matching_params) = 0;
+};
+
+template <class Msg3D, class Msg2D, class ExportObj>
+class NaiveMatchingStrategy : public FusionMatchingStrategy<Msg3D, Msg2D, ExportObj>
+{
+public:
+  explicit NaiveMatchingStrategy(
+    std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> && ros2_parent_node,
+    std::size_t rois_number);
+
+  [[nodiscard]] std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>>
+  match_rois_to_collector(
+    const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors,
+    const std::shared_ptr<RoisMatchingParams> & params) const override;
+
+  [[nodiscard]] std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>>
+  match_det3d_to_collector(
+    const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors,
+    const std::shared_ptr<Det3dMatchingParams> & params) override;
+
+  void set_collector_info(
+    std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>> & collector,
+    const std::shared_ptr<MatchingParamsBase> & matching_params) override;
+
+private:
+  std::unordered_map<std::size_t, double> id_to_offset_map_;
+  double threshold_;
+  std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> ros2_parent_node_;
+};
+
+template <class Msg3D, class Msg2D, class ExportObj>
+class AdvancedMatchingStrategy : public FusionMatchingStrategy<Msg3D, Msg2D, ExportObj>
+{
+public:
+  explicit AdvancedMatchingStrategy(
+    std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> && ros2_parent_node,
+    std::size_t rois_number);
+
+  [[nodiscard]] std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>>
+  match_rois_to_collector(
+    const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors,
+    const std::shared_ptr<RoisMatchingParams> & params) const override;
+
+  [[nodiscard]] std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>>
+  match_det3d_to_collector(
+    const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors,
+    const std::shared_ptr<Det3dMatchingParams> & params) override;
+  void set_collector_info(
+    std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>> & collector,
+    const std::shared_ptr<MatchingParamsBase> & matching_params) override;
+
+  double get_offset(
+    const double & det3d_timestamp,
+    const std::optional<std::unordered_map<std::string, std::string>> & concatenated_status);
+
+  double extract_fractional(double timestamp);
+  void update_fractional_timestamp_set(double new_timestamp);
+  double compute_offset(double input_timestamp);
+
+private:
+  std::unordered_map<std::size_t, double> id_to_offset_map_;
+  std::unordered_map<std::size_t, double> id_to_noise_window_map_;
+  double det3d_noise_window_;
+  std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> ros2_parent_node_;
+  std::set<double> fractional_timestamp_set_;  // Use set to store unique fractional timestamps
+  int success_status_counter_{0};
+  static constexpr int success_threshold{100};
+  bool database_created_{false};
+};
+}  // namespace autoware::image_projection_based_fusion