refactor(autoware_image_projection_based_fusion): organize 2d-detection related members

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

@@ -52,14 +52,33 @@
 using autoware_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 PointCloudMsgType = sensor_msgs::msg::PointCloud2;
+using RoiMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature;
+using ClusterMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature;
+using ClusterObjType = tier4_perception_msgs::msg::DetectedObjectWithFeature;
 using tier4_perception_msgs::msg::DetectedObjectWithFeature;
 using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
 using autoware::image_projection_based_fusion::CameraProjection;
 using autoware_perception_msgs::msg::ObjectClassification;
 
-template <class TargetMsg3D, class ObjType, class Msg2D>
+template <class Msg2D>
+struct Det2dStatus
+{
+  // camera index
+  std::size_t id = 0;
+  // camera projection
+  std::unique_ptr<CameraProjection> camera_projector_ptr;
+  bool project_to_unrectified_image = false;
+  bool approximate_camera_projection = false;
+  // process flags
+  bool is_fused = false;
+  // timing
+  double input_offset_ms = 0.0;
+  // cache
+  std::map<int64_t, typename Msg2D::ConstSharedPtr> cached_det2d_msgs;
+};
+
+template <class Msg3D, class Msg2D, class ExportObj>
 class FusionNode : public rclcpp::Node
 {
 public:
@@ -71,82 +90,85 @@
   explicit FusionNode(
     const std::string & node_name, const rclcpp::NodeOptions & options, int queue_size);
 
-protected:
+private:
+  // Common process methods
   void cameraInfoCallback(
     const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg,
     const std::size_t camera_id);
 
-  virtual void preprocess(TargetMsg3D & output_msg);
-
-  // callback for Msg subscription
-  virtual void subCallback(const typename TargetMsg3D::ConstSharedPtr input_msg);
-
-  // callback for roi subscription
-
-  virtual void roiCallback(
-    const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i);
-
-  virtual void fuseOnSingleImage(
-    const TargetMsg3D & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg,
-    TargetMsg3D & output_msg) = 0;
-
-  // set args if you need
-  virtual void postprocess(TargetMsg3D & output_msg);
-
-  virtual void publish(const TargetMsg3D & output_msg);
-
+  // callback for timer
   void timer_callback();
   void setPeriod(const int64_t new_period);
+  void exportProcess();
+
+  // 2d detection management methods
+  bool checkAllDet2dFused()
+  {
+    for (const auto & det2d : det2d_list_) {
+      if (!det2d.is_fused) {
+        return false;
+      }
+    }
+    return true;
+  }
 
-  std::size_t rois_number_{1};
-  tf2_ros::Buffer tf_buffer_;
-  tf2_ros::TransformListener tf_listener_;
-
-  std::vector<bool> point_project_to_unrectified_image_;
-
-  // camera_info
-  std::map<std::size_t, sensor_msgs::msg::CameraInfo> camera_info_map_;
-  std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr> camera_info_subs_;
   // camera projection
-  std::vector<CameraProjection> camera_projectors_;
-  std::vector<bool> approx_camera_projection_;
   float approx_grid_cell_w_size_;
   float approx_grid_cell_h_size_;
 
+  // timer
   rclcpp::TimerBase::SharedPtr timer_;
   double timeout_ms_{};
   double match_threshold_ms_{};
-  std::vector<std::string> input_rois_topics_;
-  std::vector<std::string> input_camera_info_topics_;
-  std::vector<std::string> input_camera_topics_;
 
-  /** \brief A vector of subscriber. */
-  typename rclcpp::Subscription<TargetMsg3D>::SharedPtr sub_;
   std::vector<typename rclcpp::Subscription<Msg2D>::SharedPtr> rois_subs_;
-
-  // offsets between cameras and the lidars
-  std::vector<double> input_offset_ms_;
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr> camera_info_subs_;
 
   // cache for fusion
-  std::vector<bool> is_fused_;
-  std::pair<int64_t, typename TargetMsg3D::SharedPtr>
-    cached_msg_;  // first element is the timestamp in nanoseconds, second element is the message
-  std::vector<std::map<int64_t, typename Msg2D::ConstSharedPtr>> cached_roi_msgs_;
+  int64_t cached_det3d_msg_timestamp_;
+  typename Msg3D::SharedPtr cached_det3d_msg_ptr_;
   std::mutex mutex_cached_msgs_;
 
-  // output publisher
-  typename rclcpp::Publisher<TargetMsg3D>::SharedPtr pub_ptr_;
+protected:
+  void setDet2DStatus(std::size_t rois_number);
 
-  // debugger
-  std::shared_ptr<Debugger> debugger_;
-  virtual bool out_of_scope(const ObjType & obj) = 0;
+  // callback for main subscription
+  void subCallback(const typename Msg3D::ConstSharedPtr input_msg);
+  // callback for roi subscription
+  void roiCallback(const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i);
+
+  // Custom process methods
+  virtual void preprocess(Msg3D & output_msg);
+  virtual void fuseOnSingleImage(
+    const Msg3D & input_msg, const Det2dStatus<Msg2D> & det2d, const Msg2D & input_roi_msg,
+    Msg3D & output_msg) = 0;
+  virtual void postprocess(const Msg3D & processing_msg, ExportObj & output_msg);
+  virtual void publish(const ExportObj & output_msg);
+
+  // Members
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  // 2d detection management
+  std::vector<Det2dStatus<Msg2D>> det2d_list_;
+
+  /** \brief A vector of subscriber. */
+  typename rclcpp::Subscription<Msg3D>::SharedPtr sub_;
+
+  // parameters for out_of_scope filter
   float filter_scope_min_x_;
   float filter_scope_max_x_;
   float filter_scope_min_y_;
   float filter_scope_max_y_;
   float filter_scope_min_z_;
   float filter_scope_max_z_;
 
+  // output publisher
+  typename rclcpp::Publisher<ExportObj>::SharedPtr pub_ptr_;
+
+  // debugger
+  std::shared_ptr<Debugger> debugger_;
+
   /** \brief processing time publisher. **/
   std::unique_ptr<autoware::universe_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
   std::unique_ptr<autoware::universe_utils::DebugPublisher> debug_publisher_;

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp

@@ -41,23 +41,22 @@ inline bool isInsideBbox(
          proj_y >= roi.y_offset * zc && proj_y <= (roi.y_offset + roi.height) * zc;
 }
 
-class PointPaintingFusionNode
-: public FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjects, DetectedObjectsWithFeature>
+class PointPaintingFusionNode : public FusionNode<PointCloudMsgType, RoiMsgType, DetectedObjects>
 {
 public:
   explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options);
 
-protected:
-  void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override;
+private:
+  void preprocess(PointCloudMsgType & pointcloud_msg) override;
 
   void fuseOnSingleImage(
-    const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id,
-    const DetectedObjectsWithFeature & input_roi_msg,
-    sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override;
+    const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus<RoiMsgType> & det2d,
+    const RoiMsgType & input_roi_msg, PointCloudMsgType & painted_pointcloud_msg) override;
 
-  void postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override;
+  void postprocess(
+    const PointCloudMsgType & painted_pointcloud_msg, DetectedObjects & output_msg) override;
 
-  rclcpp::Publisher<DetectedObjects>::SharedPtr obj_pub_ptr_;
+  rclcpp::Publisher<PointCloudMsgType>::SharedPtr point_pub_ptr_;
 
   int omp_num_threads_{1};
   float score_threshold_{0.0};
@@ -72,8 +71,6 @@ class PointPaintingFusionNode
   autoware::lidar_centerpoint::DetectionClassRemapper detection_class_remapper_;
 
   std::unique_ptr<image_projection_based_fusion::PointPaintingTRT> detector_ptr_{nullptr};
-
-  bool out_of_scope(const DetectedObjects & obj) override;
 };
 }  // namespace autoware::image_projection_based_fusion
 #endif  // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__NODE_HPP_

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp

@@ -24,21 +24,19 @@ namespace autoware::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, DetectedObjectsWithFeature>
+class RoiClusterFusionNode : public FusionNode<ClusterMsgType, RoiMsgType, ClusterMsgType>
 {
 public:
   explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options);
 
-protected:
-  void preprocess(DetectedObjectsWithFeature & output_cluster_msg) override;
-  void postprocess(DetectedObjectsWithFeature & output_cluster_msg) override;
+private:
+  void preprocess(ClusterMsgType & output_cluster_msg) override;
 
   void fuseOnSingleImage(
-    const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id,
-    const DetectedObjectsWithFeature & input_roi_msg,
-    DetectedObjectsWithFeature & output_cluster_msg) override;
+    const ClusterMsgType & input_cluster_msg, const Det2dStatus<RoiMsgType> & det2d,
+    const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) override;
+
+  void postprocess(const ClusterMsgType & output_cluster_msg, ClusterMsgType & output_msg) override;
 
   std::string trust_object_iou_mode_{"iou"};
   bool use_cluster_semantic_type_{false};
@@ -53,12 +51,11 @@ class RoiClusterFusionNode
   double trust_object_distance_;
   std::string non_trust_object_iou_mode_{"iou_x"};
 
-  bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold);
-  bool out_of_scope(const DetectedObjectWithFeature & obj) override;
+  bool is_far_enough(const ClusterObjType & obj, const double distance_threshold);
+  bool out_of_scope(const ClusterObjType & obj);
   double cal_iou_by_mode(
     const sensor_msgs::msg::RegionOfInterest & roi_1,
     const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode);
-  // bool CheckUnknown(const DetectedObjectsWithFeature & obj);
 };
 
 }  // namespace autoware::image_projection_based_fusion

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp

@@ -31,33 +31,31 @@ namespace autoware::image_projection_based_fusion
 
 using sensor_msgs::msg::RegionOfInterest;
 
-class RoiDetectedObjectFusionNode
-: public FusionNode<DetectedObjects, DetectedObject, DetectedObjectsWithFeature>
+class RoiDetectedObjectFusionNode : public FusionNode<DetectedObjects, RoiMsgType, DetectedObjects>
 {
 public:
   explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options);
 
-protected:
+private:
   void preprocess(DetectedObjects & output_msg) override;
 
   void fuseOnSingleImage(
-    const DetectedObjects & input_object_msg, const std::size_t image_id,
-    const DetectedObjectsWithFeature & input_roi_msg, DetectedObjects & output_object_msg) override;
+    const DetectedObjects & input_object_msg, const Det2dStatus<RoiMsgType> & det2d,
+    const RoiMsgType & input_roi_msg, DetectedObjects & output_object_msg) override;
 
   std::map<std::size_t, DetectedObjectWithFeature> generateDetectedObjectRoIs(
-    const DetectedObjects & input_object_msg, const std::size_t & image_id,
+    const DetectedObjects & input_object_msg, const Det2dStatus<RoiMsgType> & det2d,
     const Eigen::Affine3d & object2camera_affine);
 
   void fuseObjectsOnImage(
     const DetectedObjects & input_object_msg,
     const std::vector<DetectedObjectWithFeature> & image_rois,
     const std::map<std::size_t, DetectedObjectWithFeature> & object_roi_map);
 
-  void publish(const DetectedObjects & output_msg) override;
+  void postprocess(const DetectedObjects & processing_msg, DetectedObjects & output_msg) override;
 
-  bool out_of_scope(const DetectedObject & obj) override;
+  bool out_of_scope(const DetectedObject & obj);
 
-private:
   struct
   {
     std::vector<double> passthrough_lower_bound_probability_thresholds{};

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp

@@ -25,32 +25,29 @@
 
 namespace autoware::image_projection_based_fusion
 {
-class RoiPointCloudFusionNode
-: public FusionNode<PointCloud2, DetectedObjectWithFeature, DetectedObjectsWithFeature>
+class RoiPointCloudFusionNode : public FusionNode<PointCloudMsgType, RoiMsgType, ClusterMsgType>
 {
+public:
+  explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options);
+
 private:
-  int min_cluster_size_{1};
-  int max_cluster_size_{20};
-  bool fuse_unknown_only_{true};
-  double cluster_2d_tolerance_;
+  rclcpp::Publisher<PointCloudMsgType>::SharedPtr point_pub_ptr_;
+  rclcpp::Publisher<PointCloudMsgType>::SharedPtr cluster_debug_pub_;
 
-  rclcpp::Publisher<DetectedObjectsWithFeature>::SharedPtr pub_objects_ptr_;
-  std::vector<DetectedObjectWithFeature> output_fused_objects_;
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cluster_debug_pub_;
+  void fuseOnSingleImage(
+    const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus<RoiMsgType> & det2d,
+    const RoiMsgType & input_roi_msg, PointCloudMsgType & output_pointcloud_msg) override;
 
-  /* data */
-public:
-  explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options);
+  void postprocess(const PointCloudMsgType & pointcloud_msg, ClusterMsgType & output_msg) override;
 
-protected:
-  void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override;
+  void publish(const ClusterMsgType & output_msg) override;
 
-  void postprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override;
+  int min_cluster_size_{1};
+  int max_cluster_size_{20};
+  bool fuse_unknown_only_{true};
+  double cluster_2d_tolerance_;
 
-  void fuseOnSingleImage(
-    const PointCloud2 & input_pointcloud_msg, const std::size_t image_id,
-    const DetectedObjectsWithFeature & input_roi_msg, PointCloud2 & output_pointcloud_msg) override;
-  bool out_of_scope(const DetectedObjectWithFeature & obj) override;
+  std::vector<ClusterObjType> output_fused_objects_;
 };
 
 }  // namespace autoware::image_projection_based_fusion