feat(autoware_image_based_projection_fusion): redesign image based pr…

…ojection fusion node (#10016)

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,12 +1,15 @@
 /**:
   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
+    # Example of how to set offset parameters (Check the readme for more information), both lidar and camera run in 10 hz.
+    # msg3d's header timestamp: 0.000
+    # rois's header timestamp (camera 0-5):  0.059, 0.010, 0.026, 0.042, 0.076, 0.093
+    # offset = rois_timestamp- msg3d_timestamp
+    rois_timestamp_offsets: [0.059, 0.010, 0.026, 0.042, 0.076, 0.093]
+    rois_timeout_sec: 0.5
+    msg3d_timeout_sec: 0.05
     image_buffer_size: 15
     # projection setting for each ROI whether unrectify image
     point_project_to_unrectified_image: [false, false, false, false, false, false]
-    debug_mode: false
     filter_scope_min_x: -100.0
     filter_scope_min_y: -100.0
     filter_scope_min_z: -100.0
@@ -21,4 +24,16 @@
     approximation_grid_cell_height: 1.0
 
     # debug parameters
+    debug_mode: false
+    collector_debug_mode: false
     publish_processing_time_detail: false
+
+    publish_previous_but_late_output_msg: false
+    rosbag_length: 10.0
+    # matching strategy
+    matching_strategy:
+      type: advanced
+      msg3d_noise_window: 0.02
+      rois_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
+      # type: naive
+      # threshold: 0.05

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

@@ -0,0 +1,115 @@
+// 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/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{nullptr};
+  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)
+  {
+  }
+};
+
+enum class CollectorStatus { Idle, Processing, Finished };
+
+template <class Msg3D, class Msg2D, class ExportObj>
+class FusionCollector
+{
+public:
+  FusionCollector(
+    std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> && ros2_parent_node,
+    std::size_t rois_number, const std::vector<Det2dStatus<Msg2D>> & det2d_status_list,
+    bool debug_mode);
+  void process_msg3d(const typename Msg3D::ConstSharedPtr msg3d, double msg3d_timeout);
+  void process_rois(
+    const std::size_t & rois_id, const typename Msg2D::ConstSharedPtr rois_msg,
+    double rois_timeout);
+  void fusion_callback();
+
+  [[nodiscard]] CollectorStatus get_status();
+
+  void set_info(std::shared_ptr<FusionCollectorInfoBase> collector_info);
+  [[nodiscard]] std::shared_ptr<FusionCollectorInfoBase> get_info() const;
+  bool ready_to_fuse();
+  bool rois_exists(const std::size_t & rois_id);
+  bool msg3d_exists();
+  void add_camera_projection(
+    std::size_t rois_id, std::shared_ptr<CameraProjection> camera_projector_ptr);
+  void set_period(const std::chrono::nanoseconds period);
+  void reset();
+  void show_debug_message();
+
+private:
+  std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> ros2_parent_node_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  std::size_t rois_number_;
+  typename Msg3D::ConstSharedPtr msg3d_{nullptr};
+  std::vector<Det2dStatus<Msg2D>> det2d_status_list_;
+  std::unordered_map<std::size_t, typename Msg2D::ConstSharedPtr> id_to_rois_map_;
+  bool is_first_msg3d_{false};
+  bool debug_mode_;
+  std::mutex fusion_mutex_;
+  std::shared_ptr<FusionCollectorInfoBase> fusion_collector_info_;
+  CollectorStatus status_;
+};
+
+}  // 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 MatchingContextBase
+{
+  virtual ~MatchingContextBase() = default;
+};
+
+struct Msg3dMatchingContext : public MatchingContextBase
+{
+  double msg3d_timestamp;
+
+  explicit Msg3dMatchingContext(double msg3d_timestamp = 0.0) : msg3d_timestamp(msg3d_timestamp) {}
+};
+
+struct RoisMatchingContext : public MatchingContextBase
+{
+  double rois_timestamp;
+  std::size_t rois_id;
+
+  explicit RoisMatchingContext(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<RoisMatchingContext> & matching_context) const = 0;
+
+  [[nodiscard]] virtual std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>>
+  match_msg3d_to_collector(
+    const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors,
+    const std::shared_ptr<Msg3dMatchingContext> & matching_context) = 0;
+  virtual void set_collector_info(
+    std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>> & collector,
+    const std::shared_ptr<MatchingContextBase> & matching_context) = 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,
+    const std::unordered_map<std::size_t, double> & id_to_offset_map);
+
+  [[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<RoisMatchingContext> & matching_context) const override;
+
+  [[nodiscard]] std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>>
+  match_msg3d_to_collector(
+    const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors,
+    const std::shared_ptr<Msg3dMatchingContext> & matching_context) override;
+
+  void set_collector_info(
+    std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>> & collector,
+    const std::shared_ptr<MatchingContextBase> & matching_context) override;
+
+private:
+  std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> ros2_parent_node_;
+  std::unordered_map<std::size_t, double> id_to_offset_map_;
+  double threshold_;
+};
+
+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,
+    const std::unordered_map<std::size_t, double> & id_to_offset_map);
+
+  [[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<RoisMatchingContext> & matching_context) const override;
+
+  [[nodiscard]] std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>>
+  match_msg3d_to_collector(
+    const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors,
+    const std::shared_ptr<Msg3dMatchingContext> & matching_context) override;
+  void set_collector_info(
+    std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>> & collector,
+    const std::shared_ptr<MatchingContextBase> & matching_context) override;
+
+  double get_concatenated_offset(
+    const double & msg3d_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::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> ros2_parent_node_;
+  std::unordered_map<std::size_t, double> id_to_offset_map_;
+  std::unordered_map<std::size_t, double> id_to_noise_window_map_;
+  double msg3d_noise_window_;
+  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