feat(object_filter): add detected object filter (#1221)

* Add detected object filter Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * Refactor class name Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * Add readme Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * ADd lanelet filter option Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * change default parameter Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * refactor Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * Update readme Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * change detection launch Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * ADd unknown only option Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * Update launcher Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * Fix bug Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * Move object filter into detected_object_validator Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * ci(pre-commit): autofix * Add config parameter yaml for position filter Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * Add config for each class Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * ci(pre-commit): autofix * Fix config Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * Use shape instead of position Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * Update read me Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * Use disjoint instead of intersects Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * ci(pre-commit): autofix * Fix typo, remove debug code. Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * Use shared_ptr Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Jul 6, 2022 · 47e517a · 47e517a
1 parent 4022551
commit 47e517a
Show file tree

Hide file tree

Showing 15 changed files with 645 additions and 1 deletion.
diff --git a/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -177,6 +177,14 @@
   <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
     <arg name="input/object0" value="temporary_merged_objects"/>
     <arg name="input/object1" value="clustering/objects"/>
-    <arg name="output/object" value="objects"/>
+    <arg name="output/object" value="objects_before_filter"/>
   </include>
+
+  <!-- Filter -->
+  <group>
+    <include file="$(find-pkg-share detected_object_validation)/launch/object_lanelet_filter.launch.xml">
+      <arg name="input/object" value="objects_before_filter"/>
+      <arg name="output/object" value="objects"/>
+    </include>
+  </group>
 </launch>
diff --git a/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -99,6 +99,14 @@
     <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
       <arg name="input/object0" value="temporary_merged_objects"/>
       <arg name="input/object1" value="detection_by_tracker/objects"/>
+      <arg name="output/object" value="objects_before_filter"/>
+    </include>
+  </group>
+
+  <!-- Filter -->
+  <group>
+    <include file="$(find-pkg-share detected_object_validation)/launch/object_lanelet_filter.launch.xml">
+      <arg name="input/object" value="objects_before_filter"/>
       <arg name="output/object" value="objects"/>
     </include>
   </group>

diff --git a/perception/detected_object_validation/CMakeLists.txt b/perception/detected_object_validation/CMakeLists.txt
@@ -14,13 +14,17 @@ find_package(PCL REQUIRED QUIET COMPONENTS common search filters segmentation)
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
 
+### Find Boost Dependencies
+find_package(Boost REQUIRED)
+
 include_directories(
   include
   SYSTEM
   ${OpenCV_INCLUDE_DIRS}
   ${PCL_COMMON_INCLUDE_DIRS}
   ${PCL_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIR}
+  ${Boost_INCLUDE_DIRS}
 )
 
 # Generate occupancy grid based validator exe file
@@ -51,11 +55,30 @@ target_link_libraries(obstacle_pointcloud_based_validator
   Eigen3::Eigen
 )
 
+ament_auto_add_library(object_lanelet_filter SHARED
+  src/object_lanelet_filter.cpp
+)
+
+ament_auto_add_library(object_position_filter SHARED
+  src/object_position_filter.cpp
+)
+
 rclcpp_components_register_node(obstacle_pointcloud_based_validator
   PLUGIN "obstacle_pointcloud_based_validator::ObstaclePointCloudBasedValidator"
   EXECUTABLE obstacle_pointcloud_based_validator_node
 )
 
+rclcpp_components_register_node(object_lanelet_filter
+  PLUGIN "object_lanelet_filter::ObjectLaneletFilterNode"
+  EXECUTABLE object_lanelet_filter_node
+)
+
+rclcpp_components_register_node(object_position_filter
+  PLUGIN "object_position_filter::ObjectPositionFilterNode"
+  EXECUTABLE object_position_filter_node
+)
+
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  config
 )
diff --git a/perception/detected_object_validation/README.md b/perception/detected_object_validation/README.md
@@ -8,3 +8,5 @@ The purpose of this package is to eliminate obvious false positives of DetectedO
 
 - [Obstacle pointcloud based validator](obstacle-pointcloud-based-validator.md)
 - [Occupancy grid based validator](occupancy-grid-based-validator.md)
+- [Object lanelet filter](object-lanelet-filter.md)
+- [Object position filter](object-position-filter.md)
diff --git a/perception/detected_object_validation/config/object_lanelet_filter.param.yaml b/perception/detected_object_validation/config/object_lanelet_filter.param.yaml
@@ -0,0 +1,11 @@
+/**:
+  ros__parameters:
+    filter_target_label:
+      UNKNOWN : true
+      CAR : false
+      TRUCK : false
+      BUS : false
+      TRAILER : false
+      MOTORCYCLE : false
+      BICYCLE : false
+      PEDESTRIAN : false
diff --git a/perception/detected_object_validation/config/object_position_filter.param.yaml b/perception/detected_object_validation/config/object_position_filter.param.yaml
@@ -0,0 +1,16 @@
+/**:
+  ros__parameters:
+    filter_target_label:
+      UNKNOWN : true
+      CAR : false
+      TRUCK : false
+      BUS : false
+      TRAILER : false
+      MOTORCYCLE : false
+      BICYCLE : false
+      PEDESTRIAN : false
+
+    upper_bound_x: 100.0
+    lower_bound_x: 0.0
+    upper_bound_y: 10.0
+    lower_bound_y: -10.0
diff --git a/...ption/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp b/...ption/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp
@@ -0,0 +1,82 @@
+// 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 DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_
+#define DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_
+
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/query.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <string>
+
+namespace object_lanelet_filter
+{
+using tier4_autoware_utils::LinearRing2d;
+using tier4_autoware_utils::MultiPoint2d;
+using tier4_autoware_utils::Point2d;
+using tier4_autoware_utils::Polygon2d;
+
+struct Filter_target_label
+{
+  bool UNKNOWN;
+  bool CAR;
+  bool TRUCK;
+  bool BUS;
+  bool TRAILER;
+  bool MOTORCYCLE;
+  bool BICYCLE;
+  bool PEDESTRIAN;
+};
+
+class ObjectLaneletFilterNode : public rclcpp::Node
+{
+public:
+  explicit ObjectLaneletFilterNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  void objectCallback(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr);
+  void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr);
+
+  rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr object_pub_;
+  rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr map_sub_;
+  rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr object_sub_;
+
+  lanelet::LaneletMapPtr lanelet_map_ptr_;
+  lanelet::ConstLanelets road_lanelets_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  Filter_target_label filter_target_;
+
+  bool transformDetectedObjects(
+    const autoware_auto_perception_msgs::msg::DetectedObjects &, const std::string &,
+    const tf2_ros::Buffer &, autoware_auto_perception_msgs::msg::DetectedObjects &);
+  LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &);
+  lanelet::ConstLanelets getIntersectedLanelets(
+    const LinearRing2d &, const lanelet::ConstLanelets &);
+  bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &);
+};
+
+}  // namespace object_lanelet_filter
+
+#endif  // DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_
diff --git a/...tion/detected_object_validation/include/detected_object_filter/object_position_filter.hpp b/...tion/detected_object_validation/include/detected_object_filter/object_position_filter.hpp
@@ -0,0 +1,68 @@
+// 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 DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_
+#define DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_
+
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/query.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <string>
+
+namespace object_position_filter
+{
+struct Filter_target_label
+{
+  bool UNKNOWN;
+  bool CAR;
+  bool TRUCK;
+  bool BUS;
+  bool TRAILER;
+  bool MOTORCYCLE;
+  bool BICYCLE;
+  bool PEDESTRIAN;
+};
+
+class ObjectPositionFilterNode : public rclcpp::Node
+{
+public:
+  explicit ObjectPositionFilterNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  void objectCallback(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr);
+
+  rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr object_pub_;
+  rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr object_sub_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  float upper_bound_x_;
+  float upper_bound_y_;
+  float lower_bound_x_;
+  float lower_bound_y_;
+  Filter_target_label filter_target_;
+};
+
+}  // namespace object_position_filter
+
+#endif  // DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_
diff --git a/perception/detected_object_validation/launch/object_lanelet_filter.launch.xml b/perception/detected_object_validation/launch/object_lanelet_filter.launch.xml
@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="vector_map_topic" default="/map/vector_map"/>
+  <arg name="input/object" default="in_objects"/>
+  <arg name="output/object" default="out_objects"/>
+  <arg name="filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_lanelet_filter.param.yaml"/>
+
+  <node pkg="detected_object_validation" exec="object_lanelet_filter_node" name="object_lanelet_filter" output="screen">
+    <remap from="input/vector_map" to="$(var vector_map_topic)"/>
+    <remap from="input/object" to="$(var input/object)"/>
+    <remap from="output/object" to="$(var output/object)"/>
+    <param from="$(var filtering_range_param)"/>
+  </node>
+</launch>
diff --git a/perception/detected_object_validation/launch/object_position_filter.launch.xml b/perception/detected_object_validation/launch/object_position_filter.launch.xml
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="input/object" default="in_objects"/>
+  <arg name="output/object" default="out_objects"/>
+  <arg name="filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_position_filter.param.yaml"/>
+
+  <node pkg="detected_object_validation" exec="object_position_filter_node" name="object_position_filter" output="screen">
+    <remap from="input/object" to="$(var input/object)"/>
+    <remap from="output/object" to="$(var output/object)"/>
+    <param from="$(var filtering_range_param)"/>
+  </node>
+</launch>
diff --git a/perception/detected_object_validation/object-lanelet-filter.md b/perception/detected_object_validation/object-lanelet-filter.md
@@ -0,0 +1,50 @@
+# object_lanelet_filter
+
+## Purpose
+
+The `object_lanelet_filter` is a node that filters detected object by using vector map.  
+The objects only inside of the vector map will be published.
+
+## Inner-workings / Algorithms
+
+## Inputs / Outputs
+
+### Input
+
+| Name               | Type                                                  | Description            |
+| ------------------ | ----------------------------------------------------- | ---------------------- |
+| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin`          | vector map             |
+| `input/object`     | `autoware_auto_perception_msgs::msg::DetectedObjects` | input detected objects |
+
+### Output
+
+| Name            | Type                                                  | Description               |
+| --------------- | ----------------------------------------------------- | ------------------------- |
+| `output/object` | `autoware_auto_perception_msgs::msg::DetectedObjects` | filtered detected objects |
+
+## Parameters
+
+### Core Parameters
+
+| Name                             | Type | Default Value | Description                               |
+| -------------------------------- | ---- | ------------- | ----------------------------------------- |
+| `filter_target_label.UNKNOWN`    | bool | false         | If true, unknown objects are filtered.    |
+| `filter_target_label.CAR`        | bool | false         | If true, car objects are filtered.        |
+| `filter_target_label.TRUCK`      | bool | false         | If true, truck objects are filtered.      |
+| `filter_target_label.BUS`        | bool | false         | If true, bus objects are filtered.        |
+| `filter_target_label.TRAILER`    | bool | false         | If true, trailer objects are filtered.    |
+| `filter_target_label.MOTORCYCLE` | bool | false         | If true, motorcycle objects are filtered. |
+| `filter_target_label.BICYCLE`    | bool | false         | If true, bicycle objects are filtered.    |
+| `filter_target_label.PEDESTRIAN` | bool | false         | If true, pedestrian objects are filtered. |
+
+## Assumptions / Known limits
+
+Filtering is performed based on the shape polygon of the object.
+
+## (Optional) Error detection and handling
+
+## (Optional) Performance characterization
+
+## (Optional) References/External links
+
+## (Optional) Future extensions / Unimplemented parts
diff --git a/perception/detected_object_validation/object-position-filter.md b/perception/detected_object_validation/object-position-filter.md
@@ -0,0 +1,53 @@
+# object_position_filter
+
+## Purpose
+
+The `object_position_filter` is a node that filters detected object based on x,y values.  
+The objects only inside of the x, y bound will be published.
+
+## Inner-workings / Algorithms
+
+## Inputs / Outputs
+
+### Input
+
+| Name           | Type                                                  | Description            |
+| -------------- | ----------------------------------------------------- | ---------------------- |
+| `input/object` | `autoware_auto_perception_msgs::msg::DetectedObjects` | input detected objects |
+
+### Output
+
+| Name            | Type                                                  | Description               |
+| --------------- | ----------------------------------------------------- | ------------------------- |
+| `output/object` | `autoware_auto_perception_msgs::msg::DetectedObjects` | filtered detected objects |
+
+## Parameters
+
+### Core Parameters
+
+| Name                             | Type  | Default Value | Description                                                     |
+| -------------------------------- | ----- | ------------- | --------------------------------------------------------------- |
+| `filter_target_label.UNKNOWN`    | bool  | false         | If true, unknown objects are filtered.                          |
+| `filter_target_label.CAR`        | bool  | false         | If true, car objects are filtered.                              |
+| `filter_target_label.TRUCK`      | bool  | false         | If true, truck objects are filtered.                            |
+| `filter_target_label.BUS`        | bool  | false         | If true, bus objects are filtered.                              |
+| `filter_target_label.TRAILER`    | bool  | false         | If true, trailer objects are filtered.                          |
+| `filter_target_label.MOTORCYCLE` | bool  | false         | If true, motorcycle objects are filtered.                       |
+| `filter_target_label.BICYCLE`    | bool  | false         | If true, bicycle objects are filtered.                          |
+| `filter_target_label.PEDESTRIAN` | bool  | false         | If true, pedestrian objects are filtered.                       |
+| `upper_bound_x`                  | float | 100.00        | Bound for filtering. Only used if filter_by_xy_position is true |
+| `lower_bound_x`                  | float | 0.00          | Bound for filtering. Only used if filter_by_xy_position is true |
+| `upper_bound_y`                  | float | 50.00         | Bound for filtering. Only used if filter_by_xy_position is true |
+| `lower_bound_y`                  | float | -50.00        | Bound for filtering. Only used if filter_by_xy_position is true |
+
+## Assumptions / Known limits
+
+Filtering is performed based on the center position of the object.
+
+## (Optional) Error detection and handling
+
+## (Optional) Performance characterization
+
+## (Optional) References/External links
+
+## (Optional) Future extensions / Unimplemented parts