Skip to content

Commit 2635df3

Browse files
badai-nguyenpre-commit-ci[bot]YoshiRi
authored and
karishma1911
committed
feat(image_projection_based_fusion): add image segmentation_pointcloud_fusion for pointcloud filter (autowarefoundation#5562)
* fix: roiCallback for Image Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: pointcloud filter Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: pointcloud filter bugfix Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: segmentation poincloud filter Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: add option in launch file Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: refactor Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * docs: update docs Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: roi_pointcloud_fusion with new template Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: add param of selectable semantic id Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: launch file Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * Revert "fix: launch file" This reverts commit 5dfa160. * fix: reverse perception launcher Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * style(pre-commit): autofix * Update perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp Co-authored-by: Yoshi Ri <yoshiyoshidetteiu@gmail.com> * style(pre-commit): autofix --------- Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yoshi Ri <yoshiyoshidetteiu@gmail.com>
1 parent 7838b82 commit 2635df3

File tree

16 files changed

+455
-41
lines changed

16 files changed

+455
-41
lines changed

‎perception/image_projection_based_fusion/CMakeLists.txt

+6
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
2222
src/utils/utils.cpp
2323
src/roi_cluster_fusion/node.cpp
2424
src/roi_detected_object_fusion/node.cpp
25+
src/segmentation_pointcloud_fusion/node.cpp
2526
src/roi_pointcloud_fusion/node.cpp
2627
)
2728

@@ -40,6 +41,11 @@ rclcpp_components_register_node(${PROJECT_NAME}
4041
EXECUTABLE roi_cluster_fusion_node
4142
)
4243

44+
rclcpp_components_register_node(${PROJECT_NAME}
45+
PLUGIN "image_projection_based_fusion::SegmentPointCloudFusionNode"
46+
EXECUTABLE segmentation_pointcloud_fusion_node
47+
)
48+
4349
rclcpp_components_register_node(${PROJECT_NAME}
4450
PLUGIN "image_projection_based_fusion::RoiPointCloudFusionNode"
4551
EXECUTABLE roi_pointcloud_fusion_node

‎perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml

+30
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
/**:
2+
ros__parameters:
3+
# if the semantic label is applied for pointcloud filtering
4+
filter_semantic_label_target:
5+
[
6+
true, # road
7+
true, # sidewalk
8+
true, # building
9+
true, # wall
10+
true, # fence
11+
true, # pole
12+
true, # traffic_light
13+
true, # traffic_sign
14+
true, # vegetation
15+
true, # terrain
16+
true, # sky
17+
false, # person
18+
false, # ride
19+
false, # car
20+
false, # truck
21+
false, # bus
22+
false, # train
23+
false, # motorcycle
24+
false, # bicycle
25+
false, # others
26+
]
27+
# the maximum distance of pointcloud to be applied filter,
28+
# this is selected based on semantic segmentation model accuracy,
29+
# calibration accuracy and unknown reaction distance
30+
filter_distance_threshold: 60.0

‎perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md

+87
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
# segmentation_pointcloud_fusion
2+
3+
## Purpose
4+
5+
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.
6+
7+
## Inner-workings / Algorithms
8+
9+
- The pointclouds are projected onto the semantic/instance segmentation mask image which is the output from 2D segmentation neural network.
10+
- 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.
11+
12+
![segmentation_pointcloud_fusion_image](./images/segmentation_pointcloud_fusion.png)
13+
14+
## Inputs / Outputs
15+
16+
### Input
17+
18+
| Name | Type | Description |
19+
| ------------------------ | ----------------------------------- | --------------------------------------------------------- |
20+
| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud |
21+
| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes |
22+
| `input/rois[0-7]` | `tier4_perception_msgs::msg::Image` | semantic segmentation mask image |
23+
| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization |
24+
25+
### Output
26+
27+
| Name | Type | Description |
28+
| -------- | ------------------------------- | -------------------------- |
29+
| `output` | `sensor_msgs::msg::PointCloud2` | output filtered pointcloud |
30+
31+
## Parameters
32+
33+
### Core Parameters
34+
35+
| Name | Type | Description |
36+
| ------------- | ---- | ------------------------ |
37+
| `rois_number` | int | the number of input rois |
38+
39+
## Assumptions / Known limits
40+
41+
<!-- Write assumptions and limitations of your implementation.
42+
43+
Example:
44+
This algorithm assumes obstacles are not moving, so if they rapidly move after the vehicle started to avoid them, it might collide with them.
45+
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.
46+
-->
47+
48+
## (Optional) Error detection and handling
49+
50+
<!-- Write how to detect errors and how to recover from them.
51+
52+
Example:
53+
This package can handle up to 20 obstacles. If more obstacles found, this node will give up and raise diagnostic errors.
54+
-->
55+
56+
## (Optional) Performance characterization
57+
58+
<!-- Write performance information like complexity. If it wouldn't be the bottleneck, not necessary.
59+
60+
Example:
61+
62+
### Complexity
63+
64+
This algorithm is O(N).
65+
66+
### Processing time
67+
68+
...
69+
-->
70+
71+
## (Optional) References/External links
72+
73+
<!-- Write links you referred to when you implemented.
74+
75+
Example:
76+
[1] {link_to_a_thesis}
77+
[2] {link_to_an_issue}
78+
-->
79+
80+
## (Optional) Future extensions / Unimplemented parts
81+
82+
<!-- Write future extensions of this package.
83+
84+
Example:
85+
Currently, this package can't handle the chattering obstacles well. We plan to add some probabilistic filters in the perception layer to improve it.
86+
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.
87+
-->

‎perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp

+9-7
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222

2323
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
2424
#include <sensor_msgs/msg/camera_info.hpp>
25+
#include <sensor_msgs/msg/image.hpp>
2526
#include <sensor_msgs/point_cloud2_iterator.hpp>
2627
#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
2728

@@ -49,13 +50,14 @@ namespace image_projection_based_fusion
4950
{
5051
using autoware_auto_perception_msgs::msg::DetectedObject;
5152
using autoware_auto_perception_msgs::msg::DetectedObjects;
53+
using sensor_msgs::msg::CameraInfo;
54+
using sensor_msgs::msg::Image;
5255
using sensor_msgs::msg::PointCloud2;
5356
using tier4_perception_msgs::msg::DetectedObjectsWithFeature;
5457
using tier4_perception_msgs::msg::DetectedObjectWithFeature;
5558
using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
5659
using autoware_auto_perception_msgs::msg::ObjectClassification;
57-
58-
template <class Msg, class ObjType>
60+
template <class Msg, class ObjType, class Msg2D>
5961
class FusionNode : public rclcpp::Node
6062
{
6163
public:
@@ -78,12 +80,12 @@ class FusionNode : public rclcpp::Node
7880
virtual void subCallback(const typename Msg::ConstSharedPtr input_msg);
7981

8082
// callback for roi subscription
83+
8184
virtual void roiCallback(
82-
const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i);
85+
const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i);
8386

8487
virtual void fuseOnSingleImage(
85-
const Msg & input_msg, const std::size_t image_id,
86-
const DetectedObjectsWithFeature & input_roi_msg,
88+
const Msg & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg,
8789
const sensor_msgs::msg::CameraInfo & camera_info, Msg & output_msg) = 0;
8890

8991
// set args if you need
@@ -111,7 +113,7 @@ class FusionNode : public rclcpp::Node
111113

112114
/** \brief A vector of subscriber. */
113115
typename rclcpp::Subscription<Msg>::SharedPtr sub_;
114-
std::vector<rclcpp::Subscription<DetectedObjectsWithFeature>::SharedPtr> rois_subs_;
116+
std::vector<typename rclcpp::Subscription<Msg2D>::SharedPtr> rois_subs_;
115117

116118
// offsets between cameras and the lidars
117119
std::vector<double> input_offset_ms_;
@@ -120,7 +122,7 @@ class FusionNode : public rclcpp::Node
120122
std::vector<bool> is_fused_;
121123
std::pair<int64_t, typename Msg::SharedPtr>
122124
cached_msg_; // first element is the timestamp in nanoseconds, second element is the message
123-
std::vector<std::map<int64_t, DetectedObjectsWithFeature::ConstSharedPtr>> cached_roi_msgs_;
125+
std::vector<std::map<int64_t, typename Msg2D::ConstSharedPtr>> cached_roi_msgs_;
124126
std::mutex mutex_cached_msgs_;
125127

126128
// output publisher

‎perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,8 @@ namespace image_projection_based_fusion
3333
{
3434
using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
3535

36-
class PointPaintingFusionNode : public FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjects>
36+
class PointPaintingFusionNode
37+
: public FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjects, DetectedObjectsWithFeature>
3738
{
3839
public:
3940
explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options);

‎perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,8 @@ namespace image_projection_based_fusion
2525
const std::map<std::string, uint8_t> IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}};
2626

2727
class RoiClusterFusionNode
28-
: public FusionNode<DetectedObjectsWithFeature, DetectedObjectWithFeature>
28+
: public FusionNode<
29+
DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>
2930
{
3031
public:
3132
explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options);

‎perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,8 @@ namespace image_projection_based_fusion
2929

3030
using sensor_msgs::msg::RegionOfInterest;
3131

32-
class RoiDetectedObjectFusionNode : public FusionNode<DetectedObjects, DetectedObject>
32+
class RoiDetectedObjectFusionNode
33+
: public FusionNode<DetectedObjects, DetectedObject, DetectedObjectsWithFeature>
3334
{
3435
public:
3536
explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options);

‎perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,7 @@
2222
namespace image_projection_based_fusion
2323
{
2424
class RoiPointCloudFusionNode
25-
: public FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjectWithFeature>
25+
: public FusionNode<PointCloud2, DetectedObjectWithFeature, DetectedObjectsWithFeature>
2626
{
2727
private:
2828
int min_cluster_size_{1};

‎perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp

+54
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
// Copyright 2023 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_
16+
#define IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_
17+
18+
#include "image_projection_based_fusion/fusion_node.hpp"
19+
20+
#include <string>
21+
#include <vector>
22+
23+
#if __has_include(<cv_bridge/cv_bridge.hpp>)
24+
#include <cv_bridge/cv_bridge.hpp>
25+
#else
26+
#include <cv_bridge/cv_bridge.h>
27+
#endif
28+
29+
namespace image_projection_based_fusion
30+
{
31+
class SegmentPointCloudFusionNode : public FusionNode<PointCloud2, PointCloud2, Image>
32+
{
33+
private:
34+
rclcpp::Publisher<PointCloud2>::SharedPtr pub_pointcloud_ptr_;
35+
std::vector<bool> filter_semantic_label_target_;
36+
float filter_distance_threshold_;
37+
/* data */
38+
public:
39+
explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options);
40+
41+
protected:
42+
void preprocess(PointCloud2 & pointcloud_msg) override;
43+
44+
void postprocess(PointCloud2 & pointcloud_msg) override;
45+
46+
void fuseOnSingleImage(
47+
const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const Image & input_mask,
48+
const CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override;
49+
50+
bool out_of_scope(const PointCloud2 & filtered_cloud);
51+
};
52+
53+
} // namespace image_projection_based_fusion
54+
#endif // IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_

‎perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml

+87
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
<launch>
2+
<arg name="input/camera_number" default="1"/>
3+
<arg name="input/mask0" default="/perception/object_recognition/detection/mask0"/>
4+
<arg name="input/camera_info0" default="/sensing/camera/camera0/camera_info"/>
5+
<arg name="input/mask1" default="/perception/object_recognition/detection/mask1"/>
6+
<arg name="input/camera_info1" default="/sensing/camera/camera0/camera_info"/>
7+
<arg name="input/mask2" default="/perception/object_recognition/detection/mask2"/>
8+
<arg name="input/camera_info2" default="/sensing/camera/camera0/camera_info"/>
9+
<arg name="input/mask3" default="/perception/object_recognition/detection/mask3"/>
10+
<arg name="input/camera_info3" default="/sensing/camera/camera0/camera_info"/>
11+
<arg name="input/mask4" default="/perception/object_recognition/detection/mask4"/>
12+
<arg name="input/camera_info4" default="/sensing/camera/camera0/camera_info"/>
13+
<arg name="input/mask5" default="/perception/object_recognition/detection/mask5"/>
14+
<arg name="input/camera_info5" default="/sensing/camera/camera0/camera_info"/>
15+
<arg name="input/mask6" default="/perception/object_recognition/detection/mask6"/>
16+
<arg name="input/camera_info6" default="/sensing/camera/camera0/camera_info"/>
17+
<arg name="input/mask7" default="/perception/object_recognition/detection/mask7"/>
18+
<arg name="input/camera_info7" default="/sensing/camera/camera0/camera_info"/>
19+
<arg name="input/pointcloud" default="/sensing/lidar/top/outlier_filtered/pointcloud"/>
20+
<arg name="output/pointcloud" default="output/pointcloud"/>
21+
<arg name="sync_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_sync.param.yaml"/>
22+
<arg name="semantic_segmentation_based_filter_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/segmentation_pointcloud_fusion.param.yaml"/>
23+
<!-- debug -->
24+
<!-- cspell: ignore minx, maxx, miny, maxy, minz, maxz -->
25+
<arg name="debug_mode" default="false"/>
26+
27+
<arg name="filter_scope_minx" default="-100"/>
28+
<arg name="filter_scope_maxx" default="100"/>
29+
<arg name="filter_scope_miny" default="-100"/>
30+
<arg name="filter_scope_maxy" default="100"/>
31+
<arg name="filter_scope_minz" default="-100"/>
32+
<arg name="filter_scope_maxz" default="100"/>
33+
<arg name="image_buffer_size" default="15"/>
34+
<arg name="input/image0" default="/image_raw0"/>
35+
<arg name="input/image1" default="/image_raw1"/>
36+
<arg name="input/image2" default="/image_raw2"/>
37+
<arg name="input/image3" default="/image_raw3"/>
38+
<arg name="input/image4" default="/image_raw4"/>
39+
<arg name="input/image5" default="/image_raw5"/>
40+
<arg name="input/image6" default="/image_raw6"/>
41+
<arg name="input/image7" default="/image_raw7"/>
42+
<group>
43+
<node pkg="image_projection_based_fusion" exec="segmentation_pointcloud_fusion_node" name="segmentation_pointcloud_fusion" output="screen">
44+
<param name="rois_number" value="$(var input/camera_number)"/>
45+
<param from="$(var semantic_segmentation_based_filter_param_path)"/>
46+
<param from="$(var sync_param_path)"/>
47+
<remap from="input" to="$(var input/pointcloud)"/>
48+
<remap from="output" to="$(var output/pointcloud)"/>
49+
50+
<!-- rois, camera and info -->
51+
<param name="input/rois0" value="$(var input/mask0)"/>
52+
<param name="input/camera_info0" value="$(var input/camera_info0)"/>
53+
<param name="input/image0" value="$(var input/image0)"/>
54+
<param name="input/rois1" value="$(var input/mask1)"/>
55+
<param name="input/camera_info1" value="$(var input/camera_info1)"/>
56+
<param name="input/image1" value="$(var input/image1)"/>
57+
<param name="input/rois2" value="$(var input/mask2)"/>
58+
<param name="input/camera_info2" value="$(var input/camera_info2)"/>
59+
<param name="input/image2" value="$(var input/image2)"/>
60+
<param name="input/rois3" value="$(var input/mask3)"/>
61+
<param name="input/camera_info3" value="$(var input/camera_info3)"/>
62+
<param name="input/image3" value="$(var input/image3)"/>
63+
<param name="input/rois4" value="$(var input/mask4)"/>
64+
<param name="input/camera_info4" value="$(var input/camera_info4)"/>
65+
<param name="input/image4" value="$(var input/image4)"/>
66+
<param name="input/rois5" value="$(var input/mask5)"/>
67+
<param name="input/camera_info5" value="$(var input/camera_info5)"/>
68+
<param name="input/image5" value="$(var input/image5)"/>
69+
<param name="input/rois6" value="$(var input/mask6)"/>
70+
<param name="input/camera_info6" value="$(var input/camera_info6)"/>
71+
<param name="input/image6" value="$(var input/image6)"/>
72+
<param name="input/rois7" value="$(var input/mask7)"/>
73+
<param name="input/camera_info7" value="$(var input/camera_info7)"/>
74+
<param name="input/image7" value="$(var input/image7)"/>
75+
76+
<!-- debug -->
77+
<param name="debug_mode" value="$(var debug_mode)"/>
78+
<param name="filter_scope_minx" value="$(var filter_scope_minx)"/>
79+
<param name="filter_scope_maxx" value="$(var filter_scope_maxx)"/>
80+
<param name="filter_scope_miny" value="$(var filter_scope_miny)"/>
81+
<param name="filter_scope_maxy" value="$(var filter_scope_maxy)"/>
82+
<param name="filter_scope_minz" value="$(var filter_scope_minz)"/>
83+
<param name="filter_scope_maxz" value="$(var filter_scope_maxz)"/>
84+
<param name="image_buffer_size" value="$(var image_buffer_size)"/>
85+
</node>
86+
</group>
87+
</launch>

0 commit comments

Comments
 (0)