feat: detected object validation (#507)

perception/detected_object_validation/CMakeLists.txt

@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 3.5)
+project(detected_object_validation)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+# Ignore -Wnonportable-include-path in Clang for mussp
+if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wno-nonportable-include-path)
+endif()
+
+### Find Packages
+find_package(ament_cmake_auto REQUIRED)
+
+### Find OpenCV Dependencies
+find_package(OpenCV REQUIRED)
+
+### Find Eigen Dependencies
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+### Find dependencies listed in the package.xml
+ament_auto_find_build_dependencies()
+
+include_directories(
+  include
+  SYSTEM
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+# Generate exe file
+set(OCCUPANCY_GRID_BASED_VALIDATOR_SRC
+  src/occupancy_grid_based_validator.cpp
+)
+
+ament_auto_add_library(occupancy_grid_based_validator SHARED
+  ${OCCUPANCY_GRID_BASED_VALIDATOR_SRC}
+)
+
+target_link_libraries(occupancy_grid_based_validator
+  ${OpenCV_LIBRARIES}
+  Eigen3::Eigen
+)
+
+rclcpp_components_register_node(occupancy_grid_based_validator
+  PLUGIN "occupancy_grid_based_validator::OccupancyGridBasedValidator"
+  EXECUTABLE occupancy_grid_based_validator_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)

perception/detected_object_validation/README.md

@@ -0,0 +1,50 @@
+# detected_object_validation (occupancy grid based validator)
+
+## Purpose
+
+The purpose of this package is to eliminate obvious false positives of DetectedObjects.
+
+## Inner-workings / Algorithms
+
+Compare the occupancy grid map with the DetectedObject, and if a larger percentage of obstacles are in freespace, delete them.
+
+![debug sample image](image/debug_image.png)
+
+Basically, it takes an occupancy grid map as input and generates a binary image of freespace or other.
+
+A mask image is generated for each DetectedObject and the average value (percentage) in the mask image is calculated.
+If the percentage is low, it is deleted.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                         | Type                                                  | Description                                                 |
+| ---------------------------- | ----------------------------------------------------- | ----------------------------------------------------------- |
+| `~/input/detected_objects`   | `autoware_auto_perception_msgs::msg::DetectedObjects` | DetectedObjects                                             |
+| `~/input/occupancy_grid_map` | `nav_msgs::msg::OccupancyGrid`                        | OccupancyGrid with no time series calculation is preferred. |
+
+### Output
+
+| Name               | Type                                                  | Description               |
+| ------------------ | ----------------------------------------------------- | ------------------------- |
+| `~/output/objects` | `autoware_auto_perception_msgs::msg::DetectedObjects` | validated DetectedObjects |
+
+## Parameters
+
+| Name             | Type  | Description                                        |
+| ---------------- | ----- | -------------------------------------------------- |
+| `mean_threshold` | float | The percentage threshold of allowed non-freespace. |
+| `enable_debug`   | bool  | Whether to display debug images or not?            |
+
+## Assumptions / Known limits
+
+Currently, only vehicle represented as BoundingBox are supported.
+
+## (Optional) Error detection and handling
+
+## (Optional) Performance characterization
+
+## (Optional) References/External links
+
+## (Optional) Future extensions / Unimplemented parts

perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp

@@ -0,0 +1,74 @@
+// 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 OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_
+#define OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_
+
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/opencv.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+#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>
+
+namespace occupancy_grid_based_validator
+{
+class OccupancyGridBasedValidator : public rclcpp::Node
+{
+public:
+  explicit OccupancyGridBasedValidator(const rclcpp::NodeOptions & node_options);
+
+private:
+  rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr objects_pub_;
+  message_filters::Subscriber<autoware_auto_perception_msgs::msg::DetectedObjects> objects_sub_;
+  message_filters::Subscriber<nav_msgs::msg::OccupancyGrid> occ_grid_sub_;
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  typedef message_filters::sync_policies::ApproximateTime<
+    autoware_auto_perception_msgs::msg::DetectedObjects, nav_msgs::msg::OccupancyGrid>
+    SyncPolicy;
+  typedef message_filters::Synchronizer<SyncPolicy> Sync;
+  Sync sync_;
+  float mean_threshold_;
+  bool enable_debug_;
+
+  void onObjectsAndOccGrid(
+    const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects,
+    const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & input_occ_grid);
+
+  cv::Mat fromOccupancyGrid(const nav_msgs::msg::OccupancyGrid & occupancy_grid);
+  std::optional<cv::Mat> getMask(
+    const nav_msgs::msg::OccupancyGrid & occupancy_grid,
+    const autoware_auto_perception_msgs::msg::DetectedObject & object);
+  std::optional<cv::Mat> getMask(
+    const nav_msgs::msg::OccupancyGrid & occupancy_grid,
+    const autoware_auto_perception_msgs::msg::DetectedObject & object, cv::Mat mask);
+  void toPolygon2d(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object,
+    std::vector<cv::Point2f> & vertices);
+  void showDebugImage(
+    const nav_msgs::msg::OccupancyGrid & ros_occ_grid,
+    const autoware_auto_perception_msgs::msg::DetectedObjects & objects, const cv::Mat & occ_grid);
+};
+}  // namespace occupancy_grid_based_validator
+
+#endif  // OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_

perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <arg name="input/detected_objects" default="/perception/object_recognition/detection/objects" />
+  <arg name="output/objects" default="/perception/object_recognition/detection/validattion/occupancy_grid_based/objects" />
+
+  <arg name="input/lasescan" default="/perception/occupancy_grid_map/virtual_scan/laserscan" />
+  <arg name="input/occupancy_grid_map" default="/perception/object_recognition/detection/validation/occupancy_grid_based/single_frame_occupancy_grid_map" />
+
+
+  <node pkg="laserscan_to_occupancy_grid_map" exec="laserscan_to_occupancy_grid_map_node" name="single_frame_laserscan_occupancy_grid_map" output="screen">
+    <remap from="~/input/laserscan" to="/perception/occupancy_grid_map/virtual_scan/laserscan" />
+    <remap from="~/output/occupancy_grid_map" to="$(var input/occupancy_grid_map)" />
+    <param name="input_obstacle_pointcloud" value="false" />
+    <param name="input_obstacle_and_raw_pointcloud" value="false" />
+    <param name="map_resolution" value="0.2" />
+    <param name="map_length" value="200.0" />
+    <param name="map_width" value="20.0" />
+    <param name="map_frame" value="base_link" />
+    <param name="enable_single_frame_mode" value="true" />
+  </node>
+
+  <node pkg="detected_object_validation" exec="occupancy_grid_based_validator_node" name="occupancy_grid_based_validator_node" output="screen">
+    <remap from="~/input/detected_objects" to="$(var input/detected_objects)" />
+    <remap from="~/input/occupancy_grid_map" to="$(var input/occupancy_grid_map)" />
+    <remap from="~/output/objects" to="$(var output/objects)" />
+    <param name="map_frame" value="base_link" />
+    <param name="enable_debug" value="false" />
+  </node>
+
+</launch>

perception/detected_object_validation/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>detected_object_validation</name>
+  <version>1.0.0</version>
+  <description>The ROS2 detected_object_validation package</description>
+  <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>message_filters</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>tier4_perception_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>