forked from tier4/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(image_projection_based_fusion, roi_cluster_fusion): roi and obst…
…acle fusion method (tier4#548) * feat: init image_projection_based_fusion package Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * feat: debugger Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * feat: port roi_cluster_fusion to image_projection_based_fusion Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * feat: project detected_objects onto image Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * feat: update detected_object.classification Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * fix: add reset_cluster_semantic_type of roi_cluster_fusion Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * refactor: organize code Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * feat: add cylinderToVertices Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * feat: get transform_stamped at the fixed stamp Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * feat: not miss outside points of object on the image Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * chore: change the name of Copyright Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * kfeat: use image_projection_based_fusion instead of roi_cluster_fusion Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * docs: add roi_cluster_fusion and roi_detected_object_fusion Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * ci(pre-commit): autofix * docs: fix typo Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * refactor: rename function * refactor: delete member variables of input/output msg Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * fix: change when to clear a debugger Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * ci(pre-commit): autofix * refactor: use pre-increment Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * refactor: use range-based for loop Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * chore: add maintainer Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * feat: change the output in perception_launch Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
- Loading branch information
Showing
25 changed files
with
1,491 additions
and
703 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,54 @@ | ||
| cmake_minimum_required(VERSION 3.8) | ||
| project(image_projection_based_fusion) | ||
|
|
||
| if (NOT CMAKE_CXX_STANDARD) | ||
| set(CMAKE_CXX_STANDARD 14) | ||
| endif() | ||
| if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") | ||
| add_compile_options(-Wall -Wextra -Wpedantic) | ||
| endif() | ||
|
|
||
| # find dependencies | ||
| find_package(ament_lint_auto REQUIRED) | ||
| find_package(ament_cmake_auto REQUIRED) | ||
| find_package(OpenCV REQUIRED) | ||
| find_package(Eigen3 REQUIRED) | ||
|
|
||
| ament_auto_find_build_dependencies() | ||
|
|
||
| include_directories( | ||
| include | ||
| ${OpenCV_INCLUDE_DIRS} | ||
| ${EIGEN3_INCLUDE_DIRS} | ||
| ) | ||
|
|
||
| ament_auto_add_library(${PROJECT_NAME} SHARED | ||
| src/fusion_node.cpp | ||
| src/debugger.cpp | ||
| src/utils/geometry.cpp | ||
| src/utils/utils.cpp | ||
| src/roi_cluster_fusion/node.cpp | ||
| src/roi_detected_object_fusion/node.cpp | ||
| ) | ||
| target_link_libraries(${PROJECT_NAME} | ||
| ${OpenCV_LIBRARIES} | ||
| ${EIGEN3_LIBRARIES} | ||
| ) | ||
|
|
||
| rclcpp_components_register_node(${PROJECT_NAME} | ||
| PLUGIN "image_projection_based_fusion::RoiClusterFusionNode" | ||
| EXECUTABLE roi_cluster_fusion_node | ||
| ) | ||
|
|
||
| rclcpp_components_register_node(${PROJECT_NAME} | ||
| PLUGIN "image_projection_based_fusion::RoiDetectedObjectFusionNode" | ||
| EXECUTABLE roi_detected_object_fusion_node | ||
| ) | ||
|
|
||
| if(BUILD_TESTING) | ||
| find_package(ament_lint_auto REQUIRED) | ||
| endif() | ||
|
|
||
| ament_auto_package(INSTALL_TO_SHARE | ||
| launch | ||
| ) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,14 @@ | ||
| # image_projection_based_fusion | ||
|
|
||
| ## Purpose | ||
|
|
||
| The `image_projection_based_fusion` is a package to fuse detected obstacles (bounding box or segmentation) from image and 3d pointcloud or obstacles (bounding box, cluster or segmentation). | ||
|
|
||
| ## Inner-workings / Algorithms | ||
|
|
||
| Detail description of each fusion's algorithm is in the following links. | ||
|
|
||
| | Fusion Name | Description | Detail | | ||
| | -------------------------- | ----------------------------------------------------------------------------------------------- | -------------------------------------------- | | ||
| | roi_cluster_fusion | Overwrite a classification label of clusters by that of ROIs from a 2D object detector. | [link](./docs/roi-cluster-fusion.md) | | ||
| | roi_detected_object_fusion | Overwrite a classification label of detected objects by that of ROIs from a 2D object detector. | [link](./docs/roi-detected-object-fusion.md) | |
File renamed without changes
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
50 changes: 50 additions & 0 deletions
50
perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,50 @@ | ||
| # roi_detected_object_fusion | ||
|
|
||
| ## Purpose | ||
|
|
||
| The `roi_detected_object_fusion` is a package to overwrite labels of detected objects with that of Region Of Interests (ROIs) by a 2D object detector. | ||
|
|
||
| ## Inner-workings / Algorithms | ||
|
|
||
| The detected objects are projected onto image planes, and then if the ROIs of detected objects (3D ROIs) and from a 2D detector (2D ROIs) are overlapped, the labels of detected objects are overwritten with that of 2D ROIs. Intersection over Union (IoU) is used to determine if there are overlaps between them. | ||
|
|
||
| The `DetectedObject` has three shape and the polygon vertices of a object are as below: | ||
|
|
||
| - `BOUNDING_BOX`: The 8 corners of a bounding box. | ||
| - `CYLINDER`: The circle is approximated by a hexagon. | ||
| - `POLYGON`: Not implemented yet. | ||
|
|
||
| ## Inputs / Outputs | ||
|
|
||
| ### Input | ||
|
|
||
| | Name | Type | Description | | ||
| | --------------------- | -------------------------------------------------------- | ---------------------------------------------------------------------------------- | | ||
| | `input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects | | ||
| | `input/camera_infoID` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes, `ID` is between 0 and 7 | | ||
| | `input/roisID` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image, `ID` is between 0 and 7 | | ||
| | `input/image_rawID` | `sensor_msgs::msg::Image` | images for visualization, `ID` is between 0 and 7 | | ||
|
|
||
| ### Output | ||
|
|
||
| | Name | Type | Description | | ||
| | -------------------- | ----------------------------------------------------- | ------------------------------------------------- | | ||
| | `output` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects | | ||
| | `output/image_rawID` | `sensor_msgs::msg::Image` | images for visualization, `ID` is between 0 and 7 | | ||
|
|
||
| ## Parameters | ||
|
|
||
| ### Core Parameters | ||
|
|
||
| | Name | Type | Description | | ||
| | --------------- | ----- | ------------------------------------------------------------------------------ | | ||
| | `use_iou_x` | bool | calculate IoU only along x-axis | | ||
| | `use_iou_y` | bool | calculate IoU only along y-axis | | ||
| | `use_iou` | bool | calculate IoU both along x-axis and y-axis | | ||
| | `iou_threshold` | float | the IoU threshold to overwrite a label of a detected object with that of a roi | | ||
| | `rois_number` | int | the number of input rois | | ||
| | `debug_mode` | bool | If `true`, subscribe and publish images for visualization. | | ||
|
|
||
| ## Assumptions / Known limits | ||
|
|
||
| `POLYGON`, which is a shape of a detected object, isn't supported yet. |
65 changes: 65 additions & 0 deletions
65
perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,65 @@ | ||
| // Copyright 2022 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__DEBUGGER_HPP_ | ||
| #define IMAGE_PROJECTION_BASED_FUSION__DEBUGGER_HPP_ | ||
|
|
||
| #define EIGEN_MPL2_ONLY | ||
|
|
||
| #include <Eigen/Core> | ||
| #include <Eigen/Geometry> | ||
| #include <image_transport/image_transport.hpp> | ||
| #include <rclcpp/rclcpp.hpp> | ||
|
|
||
| #include <sensor_msgs/msg/image.hpp> | ||
| #include <sensor_msgs/msg/region_of_interest.hpp> | ||
|
|
||
| #include <boost/circular_buffer.hpp> | ||
|
|
||
| #include <memory> | ||
| #include <vector> | ||
|
|
||
| namespace image_projection_based_fusion | ||
| { | ||
|
|
||
| using sensor_msgs::msg::RegionOfInterest; | ||
|
|
||
| class Debugger | ||
| { | ||
| public: | ||
| explicit Debugger(rclcpp::Node * node_ptr, const std::size_t image_num); | ||
|
|
||
| void publishImage(const std::size_t image_id, const rclcpp::Time & stamp); | ||
|
|
||
| void clear(); | ||
|
|
||
| std::vector<RegionOfInterest> image_rois_; | ||
| std::vector<RegionOfInterest> obstacle_rois_; | ||
| std::vector<Eigen::Vector2d> obstacle_points_; | ||
|
|
||
| private: | ||
| void imageCallback( | ||
| const sensor_msgs::msg::Image::ConstSharedPtr input_image_msg, const std::size_t image_id); | ||
|
|
||
| rclcpp::Node * node_ptr_; | ||
| std::shared_ptr<image_transport::ImageTransport> image_transport_; | ||
| std::vector<image_transport::Subscriber> image_subs_; | ||
| std::vector<image_transport::Publisher> image_pubs_; | ||
| std::vector<boost::circular_buffer<sensor_msgs::msg::Image::ConstSharedPtr>> image_buffers_; | ||
| std::size_t image_buffer_size{5}; | ||
| }; | ||
|
|
||
| } // namespace image_projection_based_fusion | ||
|
|
||
| #endif // IMAGE_PROJECTION_BASED_FUSION__DEBUGGER_HPP_ |
110 changes: 110 additions & 0 deletions
110
...ption/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,110 @@ | ||
| // Copyright 2021 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__FUSION_NODE_HPP_ | ||
| #define IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ | ||
|
|
||
| #include <image_projection_based_fusion/debugger.hpp> | ||
| #include <rclcpp/rclcpp.hpp> | ||
|
|
||
| #include <autoware_auto_perception_msgs/msg/detected_objects.hpp> | ||
| #include <sensor_msgs/msg/camera_info.hpp> | ||
| #include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp> | ||
|
|
||
| #include <message_filters/pass_through.h> | ||
| #include <message_filters/subscriber.h> | ||
| #include <message_filters/sync_policies/approximate_time.h> | ||
| #include <message_filters/synchronizer.h> | ||
| #include <tf2_ros/buffer.h> | ||
| #include <tf2_ros/transform_listener.h> | ||
|
|
||
| #include <map> | ||
| #include <memory> | ||
| #include <string> | ||
| #include <vector> | ||
|
|
||
| namespace image_projection_based_fusion | ||
| { | ||
| using autoware_auto_perception_msgs::msg::DetectedObject; | ||
| using autoware_auto_perception_msgs::msg::DetectedObjects; | ||
| using tier4_perception_msgs::msg::DetectedObjectsWithFeature; | ||
| using tier4_perception_msgs::msg::DetectedObjectWithFeature; | ||
|
|
||
| template <class Msg> | ||
| class FusionNode : public rclcpp::Node | ||
| { | ||
| public: | ||
| explicit FusionNode(const std::string & node_name, const rclcpp::NodeOptions & options); | ||
|
|
||
| protected: | ||
| void cameraInfoCallback( | ||
| const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, | ||
| const std::size_t camera_id); | ||
|
|
||
| void fusionCallback( | ||
| typename Msg::ConstSharedPtr input_msg, | ||
| DetectedObjectsWithFeature::ConstSharedPtr input_roi0_msg, | ||
| DetectedObjectsWithFeature::ConstSharedPtr input_roi1_msg, | ||
| DetectedObjectsWithFeature::ConstSharedPtr input_roi2_msg, | ||
| DetectedObjectsWithFeature::ConstSharedPtr input_roi3_msg, | ||
| DetectedObjectsWithFeature::ConstSharedPtr input_roi4_msg, | ||
| DetectedObjectsWithFeature::ConstSharedPtr input_roi5_msg, | ||
| DetectedObjectsWithFeature::ConstSharedPtr input_roi6_msg, | ||
| DetectedObjectsWithFeature::ConstSharedPtr input_roi7_msg); | ||
|
|
||
| virtual void preprocess(Msg & output_msg); | ||
|
|
||
| virtual void fuseOnSingleImage( | ||
| const Msg & input_msg, const std::size_t image_id, | ||
| const DetectedObjectsWithFeature & input_roi_msg, | ||
| const sensor_msgs::msg::CameraInfo & camera_info, Msg & output_msg) = 0; | ||
|
|
||
| // set args if you need | ||
| virtual void postprocess(); | ||
|
|
||
| void publish(const Msg & output_msg); | ||
|
|
||
| std::size_t rois_number_{1}; | ||
| tf2_ros::Buffer tf_buffer_; | ||
| tf2_ros::TransformListener tf_listener_; | ||
|
|
||
| // 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_; | ||
|
|
||
| // fusion | ||
| typename message_filters::Subscriber<Msg> sub_; | ||
| message_filters::PassThrough<DetectedObjectsWithFeature> passthrough_; | ||
| std::vector<std::shared_ptr<message_filters::Subscriber<DetectedObjectsWithFeature>>> rois_subs_; | ||
| inline void dummyCallback(DetectedObjectsWithFeature::ConstSharedPtr input) | ||
| { | ||
| passthrough_.add(input); | ||
| } | ||
| using SyncPolicy = message_filters::sync_policies::ApproximateTime< | ||
| Msg, DetectedObjectsWithFeature, DetectedObjectsWithFeature, DetectedObjectsWithFeature, | ||
| DetectedObjectsWithFeature, DetectedObjectsWithFeature, DetectedObjectsWithFeature, | ||
| DetectedObjectsWithFeature, DetectedObjectsWithFeature>; | ||
| using Sync = message_filters::Synchronizer<SyncPolicy>; | ||
| typename std::shared_ptr<Sync> sync_ptr_; | ||
|
|
||
| // output | ||
| typename rclcpp::Publisher<Msg>::SharedPtr pub_ptr_; | ||
|
|
||
| // debugger | ||
| std::shared_ptr<Debugger> debugger_; | ||
| }; | ||
|
|
||
| } // namespace image_projection_based_fusion | ||
|
|
||
| #endif // IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ |
Oops, something went wrong.