feat: add detection by tracker

perception/detection_by_tracker/CMakeLists.txt

@@ -0,0 +1,65 @@
+cmake_minimum_required(VERSION 3.5)
+project(detection_by_tracker)
+
+### 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 PCL Dependencies
+find_package(PCL REQUIRED QUIET COMPONENTS common search filters segmentation)
+
+### 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
+  ${PCL_COMMON_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+)
+
+# Generate exe file
+set(DETECTION_BY_TRACKER_SRC
+  src/detection_by_tracker_core.cpp
+  src/utils.cpp
+)
+
+ament_auto_add_library(detection_by_tracker_node SHARED
+  ${DETECTION_BY_TRACKER_SRC}
+)
+
+target_link_libraries(detection_by_tracker_node
+  Eigen3::Eigen
+  ${PCL_LIBRARIES}
+)
+
+rclcpp_components_register_node(detection_by_tracker_node
+  PLUGIN "DetectionByTracker"
+  EXECUTABLE detection_by_tracker
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)

perception/detection_by_tracker/README.md

@@ -0,0 +1,63 @@
+# detection_by_tracker
+
+## Purpose
+
+This package feeds back the tracked objects to the detection module to keep it stable and keep detecting objects.
+![purpose](image/purpose.svg)
+
+The detection by tracker takes as input an unknown object containing a cluster of points and a tracker.
+The unknown object is optimized to fit the size of the tracker so that it can continue to be detected.
+
+## Inner-workings / Algorithms
+
+The detection by tracker receives an unknown object containing a point cloud and a tracker, where the unknown object is mainly shape-fitted using euclidean clustering.
+Shape fitting using euclidean clustering and other methods has a problem called under segmentation and over segmentation.
+
+[![segmentation_fail](image/segmentation_fail.png)](https://www.researchgate.net/figure/Examples-of-an-undersegmentation-error-top-and-an-oversegmentation-error-bottom-Each_fig1_304533062)
+_Adapted from [3]_
+
+Simply looking at the overlap between the unknown object and the tracker does not work. We need to take measures for under segmentation and over segmentation.
+
+### Policy for dealing with over segmentation
+
+1. Merge the unknown objects in the tracker as a single object.
+2. Shape fitting using the tracker information such as angle and size as reference information.
+
+### Policy for dealing with under segmentation
+
+1. Compare the tracker and unknown objects, and determine that those with large recall and small precision are under segmented objects.
+2. In order to divide the cluster of under segmented objects, it iterate the parameters to make small clusters.
+3. Adjust the parameters several times and adopt the one with the highest IoU.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                      | Type                                                           | Description     |
+| ------------------------- | -------------------------------------------------------------- | --------------- |
+| `~/input/initial_objects` | `autoware_perception_msgs::msg::DynamicObjectWithFeatureArray` | unknown objects |
+| `~/input/tracked_objects` | `autoware_perception_msgs::msg::DynamicObjectArray`            | trackers        |
+
+### Output
+
+| Name       | Type                                                           | Description |
+| ---------- | -------------------------------------------------------------- | ----------- |
+| `~/output` | `autoware_perception_msgs::msg::DynamicObjectWithFeatureArray` | objects     |
+
+## Parameters
+
+## Assumptions / Known limits
+
+## (Optional) Error detection and handling
+
+## (Optional) Performance characterization
+
+## (Optional) References/External links
+
+[1] M. Himmelsbach, et al. "Tracking and classification of arbitrary objects with bottom-up/top-down detection." (2012).
+
+[2] Arya Senna Abdul Rachman, Arya. "3D-LIDAR Multi Object Tracking for Autonomous Driving: Multi-target Detection and Tracking under Urban Road Uncertainties." (2017).
+
+[3] David Held, et al. "A Probabilistic Framework for Real-time 3D Segmentation using Spatial, Temporal, and Semantic Cues." (2016).
+
+## (Optional) Future extensions / Unimplemented parts

perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp

@@ -0,0 +1,96 @@
+// 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 DETECTION_BY_TRACKER__DETECTION_BY_TRACKER_CORE_HPP_
+#define DETECTION_BY_TRACKER__DETECTION_BY_TRACKER_CORE_HPP_
+
+#include <autoware_utils/autoware_utils.hpp>
+#include <euclidean_cluster/euclidean_cluster.hpp>
+#include <euclidean_cluster/utils.hpp>
+#include <euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <shape_estimation/shape_estimator.hpp>
+
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
+#include <autoware_perception_msgs/msg/detected_objects_with_feature.hpp>
+#include <autoware_perception_msgs/msg/dynamic_object_array.hpp>
+#include <autoware_perception_msgs/msg/dynamic_object_with_feature_array.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <tf2/LinearMath/Transform.h>
+#include <tf2/convert.h>
+#include <tf2/transform_datatypes.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <deque>
+#include <memory>
+#include <vector>
+
+class TrackerHandler
+{
+private:
+  std::deque<autoware_auto_perception_msgs::msg::TrackedObjects> objects_buffer_;
+
+public:
+  TrackerHandler() = default;
+  void onTrackedObjects(
+    const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objects_msg);
+  bool estimateTrackedObjects(
+    const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObjects & output);
+};
+
+class DetectionByTracker : public rclcpp::Node
+{
+public:
+  explicit DetectionByTracker(const rclcpp::NodeOptions & node_options);
+
+private:
+  rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr objects_pub_;
+  rclcpp::Subscription<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr trackers_sub_;
+  rclcpp::Subscription<autoware_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr
+    initial_objects_sub_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  TrackerHandler tracker_handler_;
+  std::shared_ptr<ShapeEstimator> shape_estimator_;
+  std::shared_ptr<euclidean_cluster::EuclideanClusterInterface> cluster_;
+
+  void onObjects(
+    const autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg);
+
+  void divideUnderSegmentedObjects(
+    const autoware_auto_perception_msgs::msg::DetectedObjects & tracked_objects,
+    const autoware_perception_msgs::msg::DetectedObjectsWithFeature & in_objects,
+    autoware_auto_perception_msgs::msg::DetectedObjects & out_no_found_tracked_objects,
+    autoware_perception_msgs::msg::DetectedObjectsWithFeature & out_objects);
+
+  float optimizeUnderSegmentedObject(
+    const autoware_auto_perception_msgs::msg::DetectedObject & target_object,
+    const sensor_msgs::msg::PointCloud2 & under_segmented_cluster,
+    autoware_perception_msgs::msg::DetectedObjectWithFeature & output);
+
+  void mergeOverSegmentedObjects(
+    const autoware_auto_perception_msgs::msg::DetectedObjects & tracked_objects,
+    const autoware_perception_msgs::msg::DetectedObjectsWithFeature & in_objects,
+    autoware_auto_perception_msgs::msg::DetectedObjects & out_no_found_tracked_objects,
+    autoware_perception_msgs::msg::DetectedObjectsWithFeature & out_objects);
+};
+
+#endif  // DETECTION_BY_TRACKER__DETECTION_BY_TRACKER_CORE_HPP_