feat: add detection by tracker package (autowarefoundation#109)

* Detection by tracker (autowarefoundation#1910) * initial commit * backup * apply format * cosmetic change * implement divided under segmenterd clusters * cosmetic change * bug fix * bug fix * bug fix * modify launch * add debug and bug fix * bug fix * bug fix * add no found tracked object * modify parameters and cmake * bug fix * remove debug info * add readme * modify clustering launch * run pre-commit * cosmetic change * cosmetic change * cosmetic change * apply markdownlint * modify launch * modify for cpplint * modify qos * change int to size_T * bug fix * change perception qos * Update perception/object_recognition/detection/detection_by_tracker/package.xml Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * cosmetic change * cosmetic change * fix launch * Update perception/object_recognition/detection/detection_by_tracker/src/utils.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * modify header include order * change include order * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * change to std::optional * cosmetic change * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * bug fix * modify readme Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * fix for createQuaternionFromRPY/Yaw (autowarefoundation#2154) * sync rc rc/v0.23.0 (autowarefoundation#2219) * Fix qos in roi cluster fusion (autowarefoundation#2218) * fix confidence (autowarefoundation#2220) Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> * too high confidence (autowarefoundation#2229) (autowarefoundation#2230) Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * port detection by tracker (autowarefoundation#589) * delete COLCON_IGNORE * potring autoware_auto_perception_msgs * add comments * Auto/fix perception viz (autowarefoundation#639) * add ns of uuid * remove dynamic_object_visualization * Update README.md Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
13304490790 · Dec 6, 2021 · 4d393e5 · 4d393e5
1 parent f385924
commit 4d393e5
Show file tree

Hide file tree

Showing 10 changed files with 983 additions and 0 deletions.
diff --git a/perception/detection_by_tracker/CMakeLists.txt b/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
+)
diff --git a/perception/detection_by_tracker/README.md b/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::TrackedObjects`                | 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
diff --git a/perception/detection_by_tracker/image/purpose.svg b/perception/detection_by_tracker/image/purpose.svg
diff --git a/perception/detection_by_tracker/image/segmentation_fail.png b/perception/detection_by_tracker/image/segmentation_fail.png
diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/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_