feat(autoware_bytetrack3d): add new 3d multi-object tracking algorithm

perception/autoware_bytetrack3D/CMakeLists.txt

@@ -0,0 +1,74 @@
+cmake_minimum_required(VERSION 3.5)
+project(autoware_bytetrack3d)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(OpenCV REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(OpenMP REQUIRED)
+if(OpenMP_FOUND)
+  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+endif()
+find_package(Boost REQUIRED)
+
+#
+# Core library
+#
+file(GLOB bytetrack3D_lib_src
+  "lib/src/*.cpp"
+)
+
+ament_auto_add_library(bytetrack3D_lib SHARED
+  ${bytetrack3D_lib_src}
+)
+
+target_include_directories(bytetrack3D_lib
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/lib/include>
+  $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(bytetrack3D_lib
+  Eigen3::Eigen
+  yaml-cpp
+)
+
+#
+# ROS node
+#
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/bytetrack3D.cpp
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  OpenCV
+)
+
+target_include_directories(${PROJECT_NAME}
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/lib/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_auto_add_library(${PROJECT_NAME}_node SHARED
+  src/bytetrack3D_node.cpp
+)
+
+ament_target_dependencies(${PROJECT_NAME}_node
+  OpenCV
+)
+
+target_link_libraries(${PROJECT_NAME}_node
+  ${PROJECT_NAME}
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_node
+  PLUGIN "bytetrack3D::ByteTrack3DNode"
+  EXECUTABLE ${PROJECT_NAME}_node_exe
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+  config
+)

perception/autoware_bytetrack3D/README.md

@@ -0,0 +1,82 @@
+# bytetrack3d
+
+## Purpose
+
+Algorithm `ByteTrack3D` is improved based on 2D tracking algorithm `ByteTrack`, mainly aims to perform 3D multi-object tracking.
+Because the algorithm associates almost every detection box including ones with low detection scores, the number of false negatives is expected to decrease by using it.
+
+## Inner-workings / Algorithms
+
+### Cite
+
+<!-- cspell: ignore Yifu Peize Jiang Dongdong Fucheng Weng Zehuan Xinggang -->
+
+- Yifu Zhang, Peize Sun, Yi Jiang, Dongdong Yu, Fucheng Weng, Zehuan Yuan, Ping Luo, Wenyu Liu, and Xinggang Wang,
+  "ByteTrack: Multi-Object Tracking by Associating Every Detection Box", in the proc. of the ECCV
+  2022, [[ref](https://arxiv.org/abs/2110.06864)]
+- This package is ported version toward Autoware from [this repository](https://github.com/ifzhang/ByteTrack)
+  (The C++ implementation by the ByteTrack's authors)
+
+### 3d tracking modification from original codes
+
+Design a 3D tracker based on the tracking concept of `ByteTrack`, mainly changing the state vector of the Kalman filter.  
+Original codes use the `top-left` and `size` as the state vector, and for 3D tracking, we uses `3d-center-point` and `3d-size` and `yaw` as the state vector.
+
+Kalman filter settings can be controlled by the parameters in `config/kalman_filter.param.yaml`.
+
+## Inputs / Outputs
+
+### bytetrack3d_node
+
+#### Input
+
+| Name         | Type                                             | Description                                 |
+| ------------ | ------------------------------------------------ | ------------------------------------------- |
+| `in/objects` | `autoware_perception_msgs::msg::DetectedObjects` | The detected objects with 3D bounding boxes |
+
+#### Output
+
+| Name          | Type                                            | Description                    |
+| ------------- | ----------------------------------------------- | ------------------------------ |
+| `out/objects` | `autoware_perception_msgs::msg::TrackedObjects` | The 3D tracking bounding boxes |
+
+## Parameters
+
+### bytetrack3d_node
+
+| Name                  | Type | Default Value | Description                                              |
+| --------------------- | ---- | ------------- | -------------------------------------------------------- |
+| `track_buffer_length` | int  | 30            | The frame count that a tracklet is considered to be lost |
+
+## Assumptions/Known limits
+
+## Reference repositories
+
+- <https://github.com/ifzhang/ByteTrack>
+
+## License
+
+The codes under the `lib` directory are copied from [the original codes](https://github.com/ifzhang/ByteTrack/tree/72ca8b45d36caf5a39e949c6aa815d9abffd1ab5/deploy/TensorRT/cpp) and modified.
+The original codes belong to the MIT license stated as follows, while this ported packages are provided with Apache License 2.0:
+
+> MIT License
+>
+> Copyright (c) 2021 Yifu Zhang
+>
+> Permission is hereby granted, free of charge, to any person obtaining a copy
+> of this software and associated documentation files (the "Software"), to deal
+> in the Software without restriction, including without limitation the rights
+> to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+> copies of the Software, and to permit persons to whom the Software is
+> furnished to do so, subject to the following conditions:
+>
+> The above copyright notice and this permission notice shall be included in all
+> copies or substantial portions of the Software.
+>
+> THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+> IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+> FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+> AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+> LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+> OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+> SOFTWARE.

perception/autoware_bytetrack3D/config/bytetrack3D.param.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    track_buffer_length: 30

perception/autoware_bytetrack3D/config/kalman_filter.param.yaml

@@ -0,0 +1,23 @@
+# Kalman filter parameters for 3d tracking
+dt: 0.1           # time step[s]
+dim_x: 11         # x,y,z,yaw,l,w,h,vx,vy,vz,vyaw
+dim_z: 7          # x,y,z,yaw,l,w,h
+
+# Process Noise Covariance
+q_cov_p: 0.2
+q_cov_yaw: 0.01
+q_cov_d: 0.1
+q_cov_v: 0.5
+q_cov_vyaw: 0.001
+
+# Observation Noise Covariance
+r_cov_p: 0.1
+r_cov_yaw: 0.01
+r_cov_d: 0.1
+
+# State Transition Noise Covariance
+p0_cov_p: 0.1     # pose
+p0_cov_yaw: 0.1 #
+p0_cov_d: 0.1     # dimension
+p0_cov_v: 0.1     # velocity
+p0_cov_vyaw: 0.01 #

perception/autoware_bytetrack3D/include/bytetrack3D/bytetrack3D.hpp

@@ -0,0 +1,75 @@
+// Copyright 2023 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.
+
+// Copyright 2024 AutoCore, 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 BYTETRACK3D__BYTETRACK3D_HPP_
+#define BYTETRACK3D__BYTETRACK3D_HPP_
+
+#include "byte_tracker.h"
+#include "strack.h"
+
+#include <opencv2/core/types.hpp>
+#include <opencv2/opencv.hpp>
+
+#include <boost/uuid/uuid.hpp>
+
+#include <cstdlib>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace bytetrack3D
+{
+struct Object
+{
+  float x, y, z, yaw;
+  float l, w, h;
+  float vx, vy, vz, vyaw;
+  float score;
+  int32_t type;
+  int32_t track_id;
+  boost::uuids::uuid unique_id;
+};
+
+using ObjectArray = std::vector<Object>;
+
+class ByteTrack3D
+{
+public:
+  explicit ByteTrack3D(const int track_buffer_length = 30);
+
+  bool do_inference(ObjectArray & objects);
+  ObjectArray update_tracker(ObjectArray & input_objects);
+
+private:
+  std::unique_ptr<ByteTracker> tracker_;
+  ObjectArray latest_objects_;
+};
+
+}  // namespace bytetrack3D
+
+#endif  // BYTETRACK3D__BYTETRACK3D_HPP_

perception/autoware_bytetrack3D/include/bytetrack3D/bytetrack3D_node.hpp

@@ -0,0 +1,74 @@
+// Copyright 2023 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.
+
+// Copyright 2024 AutoCore, 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 BYTETRACK3D__BYTETRACK3D_NODE_HPP_
+#define BYTETRACK3D__BYTETRACK3D_NODE_HPP_
+
+#include <bytetrack3D/bytetrack3D.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include "autoware_perception_msgs/msg/detected_objects.hpp"
+#include "autoware_perception_msgs/msg/tracked_objects.hpp"
+#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
+#include <tier4_perception_msgs/msg/dynamic_object_array.hpp>
+#if __has_include(<cv_bridge/cv_bridge.hpp>)
+#include <cv_bridge/cv_bridge.hpp>
+#else
+#include <cv_bridge/cv_bridge.h>
+#endif
+
+#include <chrono>
+#include <fstream>
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace bytetrack3D
+{
+using LabelMap = std::map<int, std::string>;
+
+class ByteTrack3DNode : public rclcpp::Node
+{
+public:
+  explicit ByteTrack3DNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  void on_rect(const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg);
+
+  rclcpp::Publisher<autoware_perception_msgs::msg::TrackedObjects>::SharedPtr objects_pub_;
+
+  rclcpp::Subscription<autoware_perception_msgs::msg::DetectedObjects>::SharedPtr
+    detection_rect_sub_;
+
+  std::unique_ptr<bytetrack3D::ByteTrack3D> bytetrack3D_;
+};
+
+}  // namespace bytetrack3D
+
+#endif  // BYTETRACK3D__BYTETRACK3D_NODE_HPP_

perception/autoware_bytetrack3D/launch/bytetrack3D.launch.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="detection_rect" default="/perception/object_recognition/detection/objects"/>
+  <arg name="tracked_rect" default="/perception/object_recognition/bytetrack3d/objects"/>
+  <arg name="bytetrack_param_path" default="$(find-pkg-share autoware_bytetrack3d)/config/bytetrack3D.param.yaml"/>
+
+  <node pkg="autoware_bytetrack3d" exec="autoware_bytetrack3d_node_exe" output="screen">
+    <remap from="~/in/objects" to="$(var detection_rect)"/>
+    <remap from="~/out/objects" to="$(var tracked_rect)"/>
+    <param from="$(var bytetrack_param_path)"/>
+  </node>
+</launch>