diff --git a/perception/autoware_bytetrack3D/CMakeLists.txt b/perception/autoware_bytetrack3D/CMakeLists.txt new file mode 100644 index 0000000000000..cb5004abe5774 --- /dev/null +++ b/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 + $ + $ +) +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 + $ + $ +) + +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 +) diff --git a/perception/autoware_bytetrack3D/README.md b/perception/autoware_bytetrack3D/README.md new file mode 100644 index 0000000000000..4548bdb7631ab --- /dev/null +++ b/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 + + + +- 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 + +- + +## 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. diff --git a/perception/autoware_bytetrack3D/config/bytetrack3D.param.yaml b/perception/autoware_bytetrack3D/config/bytetrack3D.param.yaml new file mode 100644 index 0000000000000..e38dbdb4d3efa --- /dev/null +++ b/perception/autoware_bytetrack3D/config/bytetrack3D.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + track_buffer_length: 30 diff --git a/perception/autoware_bytetrack3D/config/kalman_filter.param.yaml b/perception/autoware_bytetrack3D/config/kalman_filter.param.yaml new file mode 100644 index 0000000000000..1ba1194b9d8a4 --- /dev/null +++ b/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 # diff --git a/perception/autoware_bytetrack3D/include/bytetrack3D/bytetrack3D.hpp b/perception/autoware_bytetrack3D/include/bytetrack3D/bytetrack3D.hpp new file mode 100644 index 0000000000000..97dc0faf369bf --- /dev/null +++ b/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 +#include + +#include + +#include +#include +#include +#include + +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; + +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 tracker_; + ObjectArray latest_objects_; +}; + +} // namespace bytetrack3D + +#endif // BYTETRACK3D__BYTETRACK3D_HPP_ diff --git a/perception/autoware_bytetrack3D/include/bytetrack3D/bytetrack3D_node.hpp b/perception/autoware_bytetrack3D/include/bytetrack3D/bytetrack3D_node.hpp new file mode 100644 index 0000000000000..78aee506031bc --- /dev/null +++ b/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 +#include + +#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include "autoware_perception_msgs/msg/tracked_objects.hpp" +#include +#include +#if __has_include() +#include +#else +#include +#endif + +#include +#include +#include +#include +#include +#include + +namespace bytetrack3D +{ +using LabelMap = std::map; + +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::SharedPtr objects_pub_; + + rclcpp::Subscription::SharedPtr + detection_rect_sub_; + + std::unique_ptr bytetrack3D_; +}; + +} // namespace bytetrack3D + +#endif // BYTETRACK3D__BYTETRACK3D_NODE_HPP_ diff --git a/perception/autoware_bytetrack3D/launch/bytetrack3D.launch.xml b/perception/autoware_bytetrack3D/launch/bytetrack3D.launch.xml new file mode 100644 index 0000000000000..ae62ab7a96176 --- /dev/null +++ b/perception/autoware_bytetrack3D/launch/bytetrack3D.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/perception/autoware_bytetrack3D/lib/include/byte_tracker.h b/perception/autoware_bytetrack3D/lib/include/byte_tracker.h new file mode 100644 index 0000000000000..c90a0fe7d1706 --- /dev/null +++ b/perception/autoware_bytetrack3D/lib/include/byte_tracker.h @@ -0,0 +1,101 @@ +/* + * 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. + */ + +// 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. + +#pragma once + +#include "strack.h" + +#include + +struct ByteTrackObject +{ + float x, y, z, yaw; + float l, w, h; + int label; + float prob; +}; + +class ByteTracker +{ +public: + ByteTracker( + int track_buffer = 30, // Matching queue length + float track_thresh = 0.35, // detection score thresh + float high_thresh = 0.6, // init new obj thresh + float match_thresh = 5.0); // Euclidean distance(meter) + ~ByteTracker(); + + std::vector update(const std::vector & objects); + cv::Scalar get_color(int idx); + +private: + std::vector joint_stracks(std::vector & tlista, std::vector & tlistb); + std::vector joint_stracks(std::vector & tlista, std::vector & tlistb); + + std::vector sub_stracks(std::vector & tlista, std::vector & tlistb); + void remove_duplicate_stracks( + std::vector & resa, std::vector & resb, std::vector & stracksa, + std::vector & stracksb); + + void linear_assignment( + std::vector> & cost_matrix, int cost_matrix_size, int cost_matrix_size_size, + float thresh, std::vector> & matches, std::vector & unmatched_a, + std::vector & unmatched_b); + std::vector> distance( + std::vector & atracks, std::vector & btracks, int & dist_size, + int & dist_size_size); + std::vector> distance( + std::vector & atracks, std::vector & btracks); + + double lapjv( + const std::vector> & cost, std::vector & rowsol, + std::vector & colsol, bool extend_cost = false, float cost_limit = LONG_MAX, + bool return_cost = true); + +private: + float track_thresh; + float high_thresh; + float match_thresh; + int frame_id; + int max_time_lost; + + std::vector tracked_stracks; + std::vector lost_stracks; + std::vector removed_stracks; + KalmanFilter kalman_filter; +}; diff --git a/perception/autoware_bytetrack3D/lib/include/data_type.h b/perception/autoware_bytetrack3D/lib/include/data_type.h new file mode 100644 index 0000000000000..64e8acc6c1e50 --- /dev/null +++ b/perception/autoware_bytetrack3D/lib/include/data_type.h @@ -0,0 +1,76 @@ +/* + * 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. + */ + +// 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. + +#pragma once + +#include +#include + +#include +#include +#include +typedef Eigen::Matrix DETECTBOX; +typedef Eigen::Matrix DETECTBOXSS; +typedef Eigen::Matrix FEATURE; +typedef Eigen::Matrix FEATURESS; +// typedef std::vector FEATURESS; + +// Kalman Filter +// typedef Eigen::Matrix KAL_FILTER; +typedef Eigen::Matrix KAL_MEAN; +typedef Eigen::Matrix KAL_COVA; +typedef Eigen::Matrix KAL_HMEAN; +typedef Eigen::Matrix KAL_HCOVA; +using KAL_DATA = std::pair; +using KAL_HDATA = std::pair; + +// main +using RESULT_DATA = std::pair; + +// tracker: +using TRACKER_DATA = std::pair; +using MATCH_DATA = std::pair; +typedef struct t +{ + std::vector matches; + std::vector unmatched_tracks; + std::vector unmatched_detections; +} TRACHER_MATCHD; + +// linear_assignment: +typedef Eigen::Matrix DYNAMICM; diff --git a/perception/autoware_bytetrack3D/lib/include/lapjv.h b/perception/autoware_bytetrack3D/lib/include/lapjv.h new file mode 100644 index 0000000000000..d197b747e6f7d --- /dev/null +++ b/perception/autoware_bytetrack3D/lib/include/lapjv.h @@ -0,0 +1,110 @@ +/* + * 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. + */ + +// 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. + +#ifndef LAPJV_H_ +#define LAPJV_H_ + +#define LARGE 1000000 + +#if !defined TRUE +#define TRUE 1 +#endif +#if !defined FALSE +#define FALSE 0 +#endif + +#define NEW(x, t, n) \ + if ((x = reinterpret_cast(malloc(sizeof(t) * (n)))) == 0) { \ + return -1; \ + } +#define FREE(x) \ + if (x != 0) { \ + free(x); \ + x = 0; \ + } +#define SWAP_INDICES(a, b) \ + { \ + int_t _temp_index = a; \ + a = b; \ + b = _temp_index; \ + } + +#if 0 +#include +#define ASSERT(cond) assert(cond) +#define PRINTF(fmt, ...) printf(fmt, ##__VA_ARGS__) +#define PRINT_COST_ARRAY(a, n) \ + while (1) { \ + printf(#a " = ["); \ + if ((n) > 0) { \ + printf("%f", (a)[0]); \ + for (uint_t j = 1; j < n; j++) { \ + printf(", %f", (a)[j]); \ + } \ + } \ + printf("]\n"); \ + break; \ + } +#define PRINT_INDEX_ARRAY(a, n) \ + while (1) { \ + printf(#a " = ["); \ + if ((n) > 0) { \ + printf("%d", (a)[0]); \ + for (uint_t j = 1; j < n; j++) { \ + printf(", %d", (a)[j]); \ + } \ + } \ + printf("]\n"); \ + break; \ + } +#else +#define ASSERT(cond) +#define PRINTF(fmt, ...) +#define PRINT_COST_ARRAY(a, n) +#define PRINT_INDEX_ARRAY(a, n) +#endif + +typedef signed int int_t; +typedef unsigned int uint_t; +typedef double cost_t; +typedef char boolean; +typedef enum fp_t { FP_1 = 1, FP_2 = 2, FP_DYNAMIC = 3 } fp_t; + +extern int_t lapjv_internal(const uint_t n, cost_t * cost[], int_t * x, int_t * y); + +#endif // LAPJV_H_ diff --git a/perception/autoware_bytetrack3D/lib/include/strack.h b/perception/autoware_bytetrack3D/lib/include/strack.h new file mode 100644 index 0000000000000..75d0914168db6 --- /dev/null +++ b/perception/autoware_bytetrack3D/lib/include/strack.h @@ -0,0 +1,140 @@ +/* + * 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. + */ + +// 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. + +#pragma once + +#include +#include + +#include + +#include +#include + +enum TrackState { New = 0, Tracked, Lost, Removed }; +using autoware::kalman_filter::KalmanFilter; + +/** manage one tracklet*/ +class STrack +{ +public: + STrack(std::vector in_pose, std::vector in_lwh, float score, int label); + ~STrack(); + + static void multi_predict(std::vector & stracks); + // void static_pose(); + void mark_lost(); + void mark_removed(); + int next_id(); + int end_frame(); + void init_kalman_filter(); + void update_kalman_filter(const Eigen::MatrixXd & measurement); + void reflect_state(); + + void activate(int frame_id); + void re_activate(STrack & new_track, int frame_id, bool new_id = false); + void update(STrack & new_track, int frame_id); + // void predict(const int frame_id); + void predict(); + + void load_parameters(const std::string & filename); + + void init_kf_params(); + + float normalize_theta(float theta); + + float yaw_correction(float pre_yaw, float obs_yaw); + +public: + bool is_activated; + int track_id; + boost::uuids::uuid unique_id; + int state; + + // std::vector original_pose; // x,y,z,yaw + std::vector pose; // x,y,z,yaw + std::vector lwh; // l,w,h + std::vector velocity; // vx,xy,vz + float time_elapsed; + // 构造时不需要赋值，只在最后将速度读出来 + + int frame_id; + int tracklet_len; + int start_frame; + + float score; + int label; + +private: + KalmanFilter kalman_filter_; + struct KfParams + { + // dimension + char dim_x = 11; + char dim_z = 7; + // system noise + float q_cov_p; + float q_cov_yaw; + float q_cov_d; + float q_cov_v; + float q_cov_vyaw; + // measurement noise + float r_cov_p; + float r_cov_yaw; + float r_cov_d; + // initial state covariance + float p0_cov_p; + float p0_cov_yaw; + float p0_cov_d; + float p0_cov_v; + float p0_cov_vyaw; + // other parameters + float dt; // sampling time + }; + static KfParams _kf_parameters; + static bool _parameters_loaded; + enum IDX { X = 0, Y = 1, Z = 2, Yaw = 3, L = 4, W = 5, H = 6, VX = 7, VY = 8, VZ = 9, VYaw = 10 }; + + Eigen::MatrixXd A; + Eigen::MatrixXd u; + Eigen::MatrixXd B; + Eigen::MatrixXd C; + Eigen::MatrixXd R; + Eigen::MatrixXd Q; + Eigen::MatrixXd P0; +}; diff --git a/perception/autoware_bytetrack3D/lib/include/ykalman_filter.h b/perception/autoware_bytetrack3D/lib/include/ykalman_filter.h new file mode 100644 index 0000000000000..f69553ae86eff --- /dev/null +++ b/perception/autoware_bytetrack3D/lib/include/ykalman_filter.h @@ -0,0 +1,29 @@ +// 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. + +#pragma once +#include "autoware/kalman_filter/kalman_filter.hpp" +#include "math.h" + +using autoware::kalman_filter::KalmanFilter; + +class yKalmanFilter : public KalmanFilter +{ +public: + void setX(Eigen::MatrixXd state); + + float normalize_theta(float theta); + + float yaw_correction(float pre_yaw, float obs_yaw); +}; diff --git a/perception/autoware_bytetrack3D/lib/src/byte_tracker.cpp b/perception/autoware_bytetrack3D/lib/src/byte_tracker.cpp new file mode 100644 index 0000000000000..a6263556ed083 --- /dev/null +++ b/perception/autoware_bytetrack3D/lib/src/byte_tracker.cpp @@ -0,0 +1,276 @@ +/* + * 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. + */ + +// 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. + +#include "byte_tracker.h" + +#include +#include + +ByteTracker::ByteTracker( + int track_buffer, float track_thresh, float high_thresh, float match_thresh) +: track_thresh(track_thresh), high_thresh(high_thresh), match_thresh(match_thresh) + +{ + frame_id = 0; + max_time_lost = track_buffer; + std::cout << "Init ByteTrack3D!" << std::endl; +} + +ByteTracker::~ByteTracker() +{ +} + +std::vector ByteTracker::update(const std::vector & objects) +{ + ////////////////// Step 1: Get detections ////////////////// + this->frame_id++; + std::vector activated_stracks; + std::vector refind_stracks; + std::vector removed_stracks; + std::vector lost_stracks; + std::vector detections; + std::vector detections_low; + + std::vector detections_cp; + std::vector tracked_stracks_swap; + std::vector resa, resb; + std::vector output_stracks; + + std::vector unconfirmed; + std::vector tracked_stracks; + std::vector strack_pool; + std::vector r_tracked_stracks; + + if (objects.size() > 0) { + for (size_t i = 0; i < objects.size(); i++) { + std::vector pose_; + pose_.resize(4); + pose_[0] = objects[i].x; + pose_[1] = objects[i].y; + pose_[2] = objects[i].z; + pose_[3] = objects[i].yaw; + + std::vector dim_; + dim_.resize(3); + dim_[0] = objects[i].l; + dim_[1] = objects[i].w; + dim_[2] = objects[i].h; + + float score = objects[i].prob; + + STrack strack(pose_, dim_, score, objects[i].label); + if (score >= track_thresh) { + detections.push_back(strack); + } else { + detections_low.push_back(strack); + } + } + } + + // Add newly detected tracklets to tracked_stracks + for (size_t i = 0; i < this->tracked_stracks.size(); i++) { + if (!this->tracked_stracks[i].is_activated) + unconfirmed.push_back(&this->tracked_stracks[i]); + else + tracked_stracks.push_back(&this->tracked_stracks[i]); + } + + ////////////////// Step 2: First association, with IoU ////////////////// + + strack_pool = joint_stracks(tracked_stracks, this->lost_stracks); + + // do prediction for each stracks + for (size_t i = 0; i < strack_pool.size(); i++) { + strack_pool[i]->predict(); + } + + std::vector > dists; + int dist_size = 0, dist_size_size = 0; + dists = distance(strack_pool, detections, dist_size, dist_size_size); + + std::vector > matches; + std::vector u_track, u_detection; + linear_assignment(dists, dist_size, dist_size_size, match_thresh, matches, u_track, u_detection); + + for (size_t i = 0; i < matches.size(); i++) { + STrack * track = strack_pool[matches[i][0]]; + STrack * det = &detections[matches[i][1]]; + + if (track->state == TrackState::Tracked) { + track->update(*det, this->frame_id); + activated_stracks.push_back(*track); + } else { + track->re_activate(*det, this->frame_id, false); + refind_stracks.push_back(*track); + } + } + + ////////////////// Step 3: Second association, using low score dets ////////////////// + for (size_t i = 0; i < u_detection.size(); i++) { + detections_cp.push_back(detections[u_detection[i]]); + } + detections.clear(); + detections.assign(detections_low.begin(), detections_low.end()); + + for (size_t i = 0; i < u_track.size(); i++) { + if (strack_pool[u_track[i]]->state == TrackState::Tracked) { + r_tracked_stracks.push_back(strack_pool[u_track[i]]); + } + } + + dists.clear(); + dists = distance(r_tracked_stracks, detections, dist_size, dist_size_size); + + matches.clear(); + u_track.clear(); + u_detection.clear(); + linear_assignment( + dists, dist_size, dist_size_size, match_thresh - 0.5, matches, u_track, u_detection); + + for (size_t i = 0; i < matches.size(); i++) { + STrack * track = r_tracked_stracks[matches[i][0]]; + STrack * det = &detections[matches[i][1]]; + + if (track->state == TrackState::Tracked) { + track->update(*det, this->frame_id); + activated_stracks.push_back(*track); + } else { + track->re_activate(*det, this->frame_id, false); + refind_stracks.push_back(*track); + } + } + + for (size_t i = 0; i < u_track.size(); i++) { + STrack * track = r_tracked_stracks[u_track[i]]; + if (track->state != TrackState::Lost) { + track->mark_lost(); + lost_stracks.push_back(*track); + } + } + + // Deal with unconfirmed tracks, usually tracks with only one beginning frame + detections.clear(); + detections.assign(detections_cp.begin(), detections_cp.end()); + + dists.clear(); + dists = distance(unconfirmed, detections, dist_size, dist_size_size); + + matches.clear(); + std::vector u_unconfirmed; + u_detection.clear(); + linear_assignment( + dists, dist_size, dist_size_size, match_thresh - 0.7, matches, u_unconfirmed, u_detection); + + for (size_t i = 0; i < matches.size(); i++) { + if (unconfirmed[matches[i][0]]->state == TrackState::New) { + unconfirmed[matches[i][0]]->activate(this->frame_id); + } + unconfirmed[matches[i][0]]->update(detections[matches[i][1]], this->frame_id); + activated_stracks.push_back(*unconfirmed[matches[i][0]]); + } + + for (size_t i = 0; i < u_unconfirmed.size(); i++) { + STrack * track = unconfirmed[u_unconfirmed[i]]; + track->mark_removed(); + removed_stracks.push_back(*track); + } + + ////////////////// Step 4: Init new stracks ////////////////// + std::vector low_conf_tracks; + for (size_t i = 0; i < u_detection.size(); i++) { + STrack * track = &detections[u_detection[i]]; + if (track->score < this->high_thresh) { + if (track->score > track_thresh) { + low_conf_tracks.push_back(*track); + } + } else { + track->activate(this->frame_id); + activated_stracks.push_back(*track); + } + } + + ////////////////// Step 5: Update state ////////////////// + for (size_t i = 0; i < this->lost_stracks.size(); i++) { + if ( + this->frame_id - this->lost_stracks[i].end_frame() > this->max_time_lost || + this->lost_stracks[i].state == TrackState::New) { + this->lost_stracks[i].mark_removed(); + removed_stracks.push_back(this->lost_stracks[i]); + } + } + + for (size_t i = 0; i < this->tracked_stracks.size(); i++) { + if (this->tracked_stracks[i].state == TrackState::Tracked) { + tracked_stracks_swap.push_back(this->tracked_stracks[i]); + } + } + this->tracked_stracks.clear(); + this->tracked_stracks.assign(tracked_stracks_swap.begin(), tracked_stracks_swap.end()); + + this->tracked_stracks = joint_stracks(this->tracked_stracks, activated_stracks); + this->tracked_stracks = joint_stracks(this->tracked_stracks, refind_stracks); + + this->lost_stracks = sub_stracks(this->lost_stracks, this->tracked_stracks); + for (size_t i = 0; i < lost_stracks.size(); i++) { + this->lost_stracks.push_back(lost_stracks[i]); + } + + this->lost_stracks = sub_stracks(this->lost_stracks, this->removed_stracks); + for (size_t i = 0; i < removed_stracks.size(); i++) { + this->removed_stracks.push_back(removed_stracks[i]); + } + + remove_duplicate_stracks(resa, resb, this->tracked_stracks, this->lost_stracks); + + this->tracked_stracks.clear(); + this->tracked_stracks.assign(resa.begin(), resa.end()); + this->lost_stracks.clear(); + this->lost_stracks.assign(resb.begin(), resb.end()); + + for (auto & track : low_conf_tracks) { + this->tracked_stracks.push_back(track); + } + + for (size_t i = 0; i < this->tracked_stracks.size(); i++) { + if (this->tracked_stracks[i].is_activated) { + output_stracks.push_back(this->tracked_stracks[i]); + } + } + + return output_stracks; +} diff --git a/perception/autoware_bytetrack3D/lib/src/lapjv.cpp b/perception/autoware_bytetrack3D/lib/src/lapjv.cpp new file mode 100644 index 0000000000000..1b8b39ccbb9f7 --- /dev/null +++ b/perception/autoware_bytetrack3D/lib/src/lapjv.cpp @@ -0,0 +1,363 @@ +/* + * 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. + */ + +// 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. + +#include "lapjv.h" + +#include +#include +#include + +/** Column-reduction and reduction transfer for a dense cost matrix. + */ +int_t _ccrrt_dense( + const uint_t n, cost_t * cost[], int_t * free_rows, int_t * x, int_t * y, cost_t * v) +{ + int_t n_free_rows; + boolean * unique; + + for (uint_t i = 0; i < n; i++) { + x[i] = -1; + v[i] = LARGE; + y[i] = 0; + } + for (uint_t i = 0; i < n; i++) { + for (uint_t j = 0; j < n; j++) { + const cost_t c = cost[i][j]; + if (c < v[j]) { + v[j] = c; + y[j] = i; + } + PRINTF("i=%d, j=%d, c[i,j]=%f, v[j]=%f y[j]=%d\n", i, j, c, v[j], y[j]); + } + } + PRINT_COST_ARRAY(v, n); + PRINT_INDEX_ARRAY(y, n); + NEW(unique, boolean, n); + memset(unique, TRUE, n); + { + int_t j = n; + do { + j--; + const int_t i = y[j]; + if (x[i] < 0) { + x[i] = j; + } else { + unique[i] = FALSE; + y[j] = -1; + } + } while (j > 0); + } + n_free_rows = 0; + for (uint_t i = 0; i < n; i++) { + if (x[i] < 0) { + free_rows[n_free_rows++] = i; + } else if (unique[i]) { + const int_t j = x[i]; + cost_t min = LARGE; + for (uint_t j2 = 0; j2 < n; j2++) { + if (j2 == (uint_t)j) { + continue; + } + const cost_t c = cost[i][j2] - v[j2]; + if (c < min) { + min = c; + } + } + PRINTF("v[%d] = %f - %f\n", j, v[j], min); + v[j] -= min; + } + } + FREE(unique); + return n_free_rows; +} + +/** Augmenting row reduction for a dense cost matrix. + */ +int_t _carr_dense( + const uint_t n, cost_t * cost[], const uint_t n_free_rows, int_t * free_rows, int_t * x, + int_t * y, cost_t * v) +{ + uint_t current = 0; + int_t new_free_rows = 0; + uint_t rr_cnt = 0; + PRINT_INDEX_ARRAY(x, n); + PRINT_INDEX_ARRAY(y, n); + PRINT_COST_ARRAY(v, n); + PRINT_INDEX_ARRAY(free_rows, n_free_rows); + while (current < n_free_rows) { + int_t i0; + int_t j1, j2; + cost_t v1, v2, v1_new; + boolean v1_lowers; + + rr_cnt++; + PRINTF("current = %d rr_cnt = %d\n", current, rr_cnt); + const int_t free_i = free_rows[current++]; + j1 = 0; + v1 = cost[free_i][0] - v[0]; + j2 = -1; + v2 = LARGE; + for (uint_t j = 1; j < n; j++) { + PRINTF("%d = %f %d = %f\n", j1, v1, j2, v2); + const cost_t c = cost[free_i][j] - v[j]; + if (c < v2) { + if (c >= v1) { + v2 = c; + j2 = j; + } else { + v2 = v1; + v1 = c; + j2 = j1; + j1 = j; + } + } + } + i0 = y[j1]; + v1_new = v[j1] - (v2 - v1); + v1_lowers = v1_new < v[j1]; + PRINTF( + "%d %d 1=%d,%f 2=%d,%f v1'=%f(%d,%g) \n", free_i, i0, j1, v1, j2, v2, v1_new, v1_lowers, + v[j1] - v1_new); + if (rr_cnt < current * n) { + if (v1_lowers) { + v[j1] = v1_new; + } else if (i0 >= 0 && j2 >= 0) { + j1 = j2; + i0 = y[j2]; + } + if (i0 >= 0) { + if (v1_lowers) { + free_rows[--current] = i0; + } else { + free_rows[new_free_rows++] = i0; + } + } + } else { + PRINTF("rr_cnt=%d >= %d (current=%d * n=%d)\n", rr_cnt, current * n, current, n); + if (i0 >= 0) { + free_rows[new_free_rows++] = i0; + } + } + x[free_i] = j1; + y[j1] = free_i; + } + return new_free_rows; +} + +/** Find columns with minimum d[j] and put them on the SCAN list. + */ +uint_t _find_dense( + const uint_t n, uint_t lo, const cost_t * d, int_t * cols, [[maybe_unused]] int_t * y) +{ + uint_t hi = lo + 1; + cost_t mind = d[cols[lo]]; + for (uint_t k = hi; k < n; k++) { + int_t j = cols[k]; + if (d[j] <= mind) { + if (d[j] < mind) { + hi = lo; + mind = d[j]; + } + cols[k] = cols[hi]; + cols[hi++] = j; + } + } + return hi; +} + +// Scan all columns in TODO starting from arbitrary column in SCAN +// and try to decrease d of the TODO columns using the SCAN column. +int_t _scan_dense( + const uint_t n, cost_t * cost[], uint_t * plo, uint_t * phi, cost_t * d, int_t * cols, + int_t * pred, const int_t * y, const cost_t * v) +{ + uint_t lo = *plo; + uint_t hi = *phi; + cost_t h, cred_ij; + + while (lo != hi) { + int_t j = cols[lo++]; + const int_t i = y[j]; + const cost_t mind = d[j]; + h = cost[i][j] - v[j] - mind; + PRINTF("i=%d j=%d h=%f\n", i, j, h); + // For all columns in TODO + for (uint_t k = hi; k < n; k++) { + j = cols[k]; + cred_ij = cost[i][j] - v[j] - h; + if (cred_ij < d[j]) { + d[j] = cred_ij; + pred[j] = i; + if (cred_ij == mind) { + if (y[j] < 0) { + return j; + } + cols[k] = cols[hi]; + cols[hi++] = j; + } + } + } + } + *plo = lo; + *phi = hi; + return -1; +} + +/** Single iteration of modified Dijkstra shortest path algorithm as explained in the JV paper. + * + * This is a dense matrix version. + * + * \return The closest free column index. + */ +int_t find_path_dense( + const uint_t n, cost_t * cost[], const int_t start_i, int_t * y, cost_t * v, int_t * pred) +{ + uint_t lo = 0, hi = 0; + int_t final_j = -1; + uint_t n_ready = 0; + int_t * cols; + cost_t * d; + + NEW(cols, int_t, n); + NEW(d, cost_t, n); + + for (uint_t i = 0; i < n; i++) { + cols[i] = i; + pred[i] = start_i; + d[i] = cost[start_i][i] - v[i]; + } + PRINT_COST_ARRAY(d, n); + while (final_j == -1) { + // No columns left on the SCAN list. + if (lo == hi) { + PRINTF("%d..%d -> find\n", lo, hi); + n_ready = lo; + hi = _find_dense(n, lo, d, cols, y); + PRINTF("check %d..%d\n", lo, hi); + PRINT_INDEX_ARRAY(cols, n); + for (uint_t k = lo; k < hi; k++) { + const int_t j = cols[k]; + if (y[j] < 0) { + final_j = j; + } + } + } + if (final_j == -1) { + PRINTF("%d..%d -> scan\n", lo, hi); + final_j = _scan_dense(n, cost, &lo, &hi, d, cols, pred, y, v); + PRINT_COST_ARRAY(d, n); + PRINT_INDEX_ARRAY(cols, n); + PRINT_INDEX_ARRAY(pred, n); + } + } + + PRINTF("found final_j=%d\n", final_j); + PRINT_INDEX_ARRAY(cols, n); + { + const cost_t mind = d[cols[lo]]; + for (uint_t k = 0; k < n_ready; k++) { + const int_t j = cols[k]; + v[j] += d[j] - mind; + } + } + + FREE(cols); + FREE(d); + + return final_j; +} + +/** Augment for a dense cost matrix. + */ +int_t _ca_dense( + const uint_t n, cost_t * cost[], const uint_t n_free_rows, int_t * free_rows, int_t * x, + int_t * y, cost_t * v) +{ + int_t * pred; + + NEW(pred, int_t, n); + + for (int_t * pfree_i = free_rows; pfree_i < free_rows + n_free_rows; pfree_i++) { + int_t i = -1, j; + uint_t k = 0; + + PRINTF("looking at free_i=%d\n", *pfree_i); + j = find_path_dense(n, cost, *pfree_i, y, v, pred); + ASSERT(j >= 0); + ASSERT(j < n); + while (i != *pfree_i) { + PRINTF("augment %d\n", j); + PRINT_INDEX_ARRAY(pred, n); + i = pred[j]; + PRINTF("y[%d]=%d -> %d\n", j, y[j], i); + y[j] = i; + PRINT_INDEX_ARRAY(x, n); + SWAP_INDICES(j, x[i]); + k++; + if (k >= n) { + ASSERT(FALSE); + } + } + } + FREE(pred); + return 0; +} + +/** Solve dense sparse LAP. + */ +int lapjv_internal(const uint_t n, cost_t * cost[], int_t * x, int_t * y) +{ + int ret; + int_t * free_rows; + cost_t * v; + + NEW(free_rows, int_t, n); + NEW(v, cost_t, n); + ret = _ccrrt_dense(n, cost, free_rows, x, y, v); + int i = 0; + while (ret > 0 && i < 2) { + ret = _carr_dense(n, cost, ret, free_rows, x, y, v); + i++; + } + if (ret > 0) { + ret = _ca_dense(n, cost, ret, free_rows, x, y, v); + } + FREE(v); + FREE(free_rows); + return ret; +} diff --git a/perception/autoware_bytetrack3D/lib/src/strack.cpp b/perception/autoware_bytetrack3D/lib/src/strack.cpp new file mode 100644 index 0000000000000..01233d6adf352 --- /dev/null +++ b/perception/autoware_bytetrack3D/lib/src/strack.cpp @@ -0,0 +1,371 @@ +/* + * 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. + */ + +// 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. + +#include "strack.h" + +#include + +#include + +#include + +// init static variable +bool STrack::_parameters_loaded = false; +STrack::KfParams STrack::_kf_parameters; + +STrack::STrack(std::vector in_pose, std::vector in_lwh, float score, int label) +{ + this->pose = std::move(in_pose); + this->lwh = std::move(in_lwh); + velocity.resize(4, 0); + + is_activated = false; + track_id = 0; + state = TrackState::New; + + frame_id = 0; + tracklet_len = 0; + this->score = score; + start_frame = 0; + this->label = label; + + // load static kf parameters: initialized once in program + const std::string package_share_directory = + ament_index_cpp::get_package_share_directory("autoware_bytetrack3d"); + const std::string default_config_path = + package_share_directory + "/config/kalman_filter.param.yaml"; + if (!_parameters_loaded) { + load_parameters(default_config_path); + _parameters_loaded = true; + } + + init_kf_params(); + + init_kalman_filter(); +} + +STrack::~STrack() +{ +} + +void STrack::init_kalman_filter() +{ + // assert parameter is loaded + assert(_parameters_loaded); + + // init kalman filter state + Eigen::MatrixXd X0 = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, 1); + + X0(IDX::X) = this->pose[0]; + X0(IDX::Y) = this->pose[1]; + X0(IDX::Z) = this->pose[2]; + X0(IDX::Yaw) = this->pose[3]; + X0(IDX::L) = this->lwh[0]; + X0(IDX::W) = this->lwh[1]; + X0(IDX::H) = this->lwh[2]; + X0(IDX::VX) = 0.0; + X0(IDX::VY) = 0.0; + X0(IDX::VZ) = 0.0; + X0(IDX::VYaw) = 0.0; + + this->kalman_filter_.init(X0, P0); +} + +/** init a tracklet */ +void STrack::activate(int frame_id) +{ + this->track_id = this->next_id(); + this->unique_id = boost::uuids::random_generator()(); + + this->tracklet_len = 1; + this->state = TrackState::Tracked; + this->is_activated = true; + this->frame_id = frame_id; + this->start_frame = frame_id; +} + +void STrack::re_activate(STrack & new_track, int frame_id, bool new_id) +{ + Eigen::MatrixXd measurement = Eigen::MatrixXd::Zero(_kf_parameters.dim_z, 1); + measurement << new_track.pose[0], new_track.pose[1], new_track.pose[2], new_track.pose[3], + new_track.lwh[0], new_track.lwh[1], new_track.lwh[2]; + + measurement(3) = yaw_correction(kalman_filter_.getXelement(IDX::Yaw), new_track.pose[3]); + + update_kalman_filter(measurement); + + if (std::abs(measurement(3) - this->pose[3]) > M_PI * 0.1) { + this->pose[3] = measurement(3); + } + + reflect_state(); + + this->tracklet_len = 1; + this->state = TrackState::Tracked; + this->is_activated = true; + this->frame_id = frame_id; + this->score = new_track.score; + if (new_id) { + this->track_id = next_id(); + this->unique_id = boost::uuids::random_generator()(); + } +} + +void STrack::mark_lost() +{ + state = TrackState::Lost; +} + +void STrack::mark_removed() +{ + state = TrackState::Removed; +} + +int STrack::next_id() +{ + static int _count = 0; + _count++; + return _count; +} + +int STrack::end_frame() +{ + return this->frame_id; +} + +void STrack::update(STrack & new_track, int frame_id) +{ + this->frame_id = frame_id; + this->tracklet_len++; + + // update + + Eigen::MatrixXd measurement = Eigen::MatrixXd::Zero(_kf_parameters.dim_z, 1); + measurement << new_track.pose[0], new_track.pose[1], new_track.pose[2], new_track.pose[3], + new_track.lwh[0], new_track.lwh[1], new_track.lwh[2]; + + measurement(3) = yaw_correction(kalman_filter_.getXelement(IDX::Yaw), new_track.pose[3]); + + update_kalman_filter(measurement); + + Eigen::MatrixXd state = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, 1); + + reflect_state(); + + if (std::abs(measurement(3) - this->pose[3]) > M_PI * 0.1) { + this->pose[3] = measurement(3); + } + + this->state = TrackState::Tracked; + this->is_activated = true; + + this->score = new_track.score; +} + +/** reflect kalman filter state to current object variables*/ +void STrack::reflect_state() +{ + Eigen::MatrixXd state = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, 1); + this->kalman_filter_.getX(state); + this->pose[0] = state(IDX::X); + this->pose[1] = state(IDX::Y); + this->pose[2] = state(IDX::Z); + + this->pose[3] = state(IDX::Yaw); + + this->lwh[0] = state(IDX::L); + this->lwh[1] = state(IDX::W); + this->lwh[2] = state(IDX::H); + this->velocity[0] = state(IDX::VX); + this->velocity[1] = state(IDX::VY); + this->velocity[2] = state(IDX::VZ); +} + +void STrack::multi_predict(std::vector & stracks) +{ + for (size_t i = 0; i < stracks.size(); i++) { + stracks[i]->predict(); + } +} + +void STrack::update_kalman_filter(const Eigen::MatrixXd & measurement) +{ + // assert parameter is loaded + assert(_parameters_loaded); + + // update + if (!this->kalman_filter_.update(measurement, C, R)) { + std::cerr << "Cannot update" << std::endl; + } +} + +void STrack::predict() +{ + // prediction + if (!this->kalman_filter_.predict(u, A, B, Q)) { + std::cerr << "Cannot predict" << std::endl; + } + reflect_state(); +} + +void STrack::load_parameters(const std::string & path) +{ + YAML::Node config = YAML::LoadFile(path); + // initialize ekf params + _kf_parameters.dim_x = config["dim_x"].as(); + _kf_parameters.dim_z = config["dim_z"].as(); + _kf_parameters.q_cov_p = config["q_cov_p"].as(); + _kf_parameters.q_cov_yaw = config["q_cov_yaw"].as(); + _kf_parameters.q_cov_d = config["q_cov_d"].as(); + _kf_parameters.q_cov_v = config["q_cov_v"].as(); + _kf_parameters.q_cov_vyaw = config["q_cov_vyaw"].as(); + _kf_parameters.r_cov_p = config["r_cov_p"].as(); + _kf_parameters.r_cov_yaw = config["r_cov_yaw"].as(); + _kf_parameters.r_cov_d = config["r_cov_d"].as(); + _kf_parameters.p0_cov_p = config["p0_cov_p"].as(); + _kf_parameters.p0_cov_yaw = config["p0_cov_yaw"].as(); + _kf_parameters.p0_cov_d = config["p0_cov_d"].as(); + _kf_parameters.p0_cov_v = config["p0_cov_v"].as(); + _kf_parameters.p0_cov_vyaw = config["p0_cov_vyaw"].as(); + + _kf_parameters.dt = config["dt"].as(); +} + +void STrack::init_kf_params() +{ + A = Eigen::MatrixXd::Identity(_kf_parameters.dim_x, _kf_parameters.dim_x); + + A(IDX::X, IDX::VX) = _kf_parameters.dt; + A(IDX::Y, IDX::VY) = _kf_parameters.dt; + A(IDX::Z, IDX::VZ) = _kf_parameters.dt; + A(IDX::Yaw, IDX::VYaw) = _kf_parameters.dt; + + u = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, 1); + B = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, _kf_parameters.dim_x); + + C = Eigen::MatrixXd::Zero(_kf_parameters.dim_z, _kf_parameters.dim_x); + C(IDX::X, IDX::X) = 1; + C(IDX::Y, IDX::Y) = 1; + C(IDX::Z, IDX::Z) = 1; + C(IDX::Yaw, IDX::Yaw) = 1; + C(IDX::L, IDX::L) = 1; + C(IDX::W, IDX::W) = 1; + C(IDX::H, IDX::H) = 1; + + R = Eigen::MatrixXd::Zero(_kf_parameters.dim_z, _kf_parameters.dim_z); + R(IDX::X, IDX::X) = _kf_parameters.r_cov_p; + R(IDX::Y, IDX::Y) = _kf_parameters.r_cov_p; + R(IDX::Z, IDX::Z) = _kf_parameters.r_cov_p; + R(IDX::Yaw, IDX::Yaw) = _kf_parameters.r_cov_yaw; + R(IDX::L, IDX::L) = _kf_parameters.r_cov_d; + R(IDX::W, IDX::W) = _kf_parameters.r_cov_d; + R(IDX::H, IDX::H) = _kf_parameters.r_cov_d; + + Q = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, _kf_parameters.dim_x); + Q(IDX::X, IDX::X) = _kf_parameters.q_cov_p; + Q(IDX::Y, IDX::Y) = _kf_parameters.q_cov_p; + Q(IDX::Z, IDX::Z) = _kf_parameters.q_cov_p; + Q(IDX::Yaw, IDX::Yaw) = _kf_parameters.q_cov_yaw; + Q(IDX::L, IDX::L) = _kf_parameters.q_cov_d; + Q(IDX::W, IDX::W) = _kf_parameters.q_cov_d; + Q(IDX::H, IDX::H) = _kf_parameters.q_cov_d; + Q(IDX::VX, IDX::VX) = _kf_parameters.q_cov_v; + Q(IDX::VY, IDX::VY) = _kf_parameters.q_cov_v; + Q(IDX::VZ, IDX::VZ) = _kf_parameters.q_cov_v; + Q(IDX::VYaw, IDX::VYaw) = _kf_parameters.q_cov_vyaw; + + P0 = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, _kf_parameters.dim_x); + P0(IDX::X, IDX::X) = _kf_parameters.p0_cov_p; + P0(IDX::Y, IDX::Y) = _kf_parameters.p0_cov_p; + P0(IDX::Z, IDX::Z) = _kf_parameters.p0_cov_p; + P0(IDX::Yaw, IDX::Yaw) = _kf_parameters.p0_cov_yaw; + P0(IDX::L, IDX::L) = _kf_parameters.p0_cov_d; + P0(IDX::W, IDX::W) = _kf_parameters.p0_cov_d; + P0(IDX::H, IDX::H) = _kf_parameters.p0_cov_d; + P0(IDX::VX, IDX::VX) = _kf_parameters.p0_cov_v; + P0(IDX::VY, IDX::VY) = _kf_parameters.p0_cov_v; + P0(IDX::VZ, IDX::VZ) = _kf_parameters.p0_cov_v; + P0(IDX::VYaw, IDX::VYaw) = _kf_parameters.p0_cov_vyaw; +} + +float STrack::normalize_theta(float theta) +{ + if (theta >= M_PI) theta -= M_PI * 2; + if (theta < -M_PI) theta += M_PI * 2; + + return theta; +} + +float STrack::yaw_correction(float pre_yaw, float obs_yaw) +{ + obs_yaw = normalize_theta(obs_yaw); + pre_yaw = normalize_theta(pre_yaw); + + if (std::abs(obs_yaw - pre_yaw) >= M_PI / 2) { + float ref_vel; + bool wrong_obs = true; + if (std::abs(obs_yaw - M_PI / 2) <= M_PI * 2 / 3) { + ref_vel = this->velocity[0]; + if (std::abs(ref_vel) > 0.2) { + wrong_obs = (std::abs(obs_yaw) - M_PI / 2) > 0.0; + } + + } else { + ref_vel = this->velocity[1]; + if (std::abs(ref_vel) > 1.0) { + wrong_obs = (std::abs(obs_yaw - M_PI / 2) - M_PI / 2.0) > 0.0; + } + } + if (wrong_obs) { + if ( + std::abs(obs_yaw - pre_yaw) > M_PI / 2.0 && std::abs(obs_yaw - pre_yaw) < M_PI * 3 / 2.0) { + if (obs_yaw < pre_yaw) { + obs_yaw += M_PI; + } else { + obs_yaw -= M_PI; + } + } + + if (std::abs(obs_yaw - pre_yaw) >= M_PI * 3 / 2.0) { + obs_yaw = -obs_yaw; + } + } + obs_yaw = normalize_theta(obs_yaw); + } + return obs_yaw; +} diff --git a/perception/autoware_bytetrack3D/lib/src/utils.cpp b/perception/autoware_bytetrack3D/lib/src/utils.cpp new file mode 100644 index 0000000000000..85a34f4c46a50 --- /dev/null +++ b/perception/autoware_bytetrack3D/lib/src/utils.cpp @@ -0,0 +1,349 @@ +/* + * 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. + */ + +// 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. + +#include "byte_tracker.h" +#include "lapjv.h" + +#include + +std::vector ByteTracker::joint_stracks( + std::vector & tlista, std::vector & tlistb) +{ + std::map exists; + std::vector res; + for (size_t i = 0; i < tlista.size(); i++) { + exists.insert(std::pair(tlista[i]->track_id, 1)); + res.push_back(tlista[i]); + } + for (size_t i = 0; i < tlistb.size(); i++) { + int tid = tlistb[i].track_id; + if (!exists[tid] || exists.count(tid) == 0) { + exists[tid] = 1; + res.push_back(&tlistb[i]); + } + } + return res; +} + +std::vector ByteTracker::joint_stracks( + std::vector & tlista, std::vector & tlistb) +{ + std::map exists; + std::vector res; + for (size_t i = 0; i < tlista.size(); i++) { + exists.insert(std::pair(tlista[i].track_id, 1)); + res.push_back(tlista[i]); + } + for (size_t i = 0; i < tlistb.size(); i++) { + int tid = tlistb[i].track_id; + if (!exists[tid] || exists.count(tid) == 0) { + exists[tid] = 1; + res.push_back(tlistb[i]); + } + } + return res; +} + +std::vector ByteTracker::sub_stracks( + std::vector & tlista, std::vector & tlistb) +{ + std::map stracks; + for (size_t i = 0; i < tlista.size(); i++) { + stracks.insert(std::pair(tlista[i].track_id, tlista[i])); + } + for (size_t i = 0; i < tlistb.size(); i++) { + int tid = tlistb[i].track_id; + if (stracks.count(tid) != 0) { + stracks.erase(tid); + } + } + + std::vector res; + std::map::iterator it; + for (it = stracks.begin(); it != stracks.end(); ++it) { + res.push_back(it->second); + } + + return res; +} + +void ByteTracker::remove_duplicate_stracks( + std::vector & resa, std::vector & resb, std::vector & stracksa, + std::vector & stracksb) +{ + std::vector> pdist = distance(stracksa, stracksb); + std::vector> pairs; + for (size_t i = 0; i < pdist.size(); i++) { + for (size_t j = 0; j < pdist[i].size(); j++) { + if (pdist[i][j] < 0.15) { + pairs.push_back(std::pair(i, j)); + } + } + } + + std::vector dupa, dupb; + for (size_t i = 0; i < pairs.size(); i++) { + int timep = stracksa[pairs[i].first].frame_id - stracksa[pairs[i].first].start_frame; + int timeq = stracksb[pairs[i].second].frame_id - stracksb[pairs[i].second].start_frame; + if (timep > timeq) + dupb.push_back(pairs[i].second); + else + dupa.push_back(pairs[i].first); + } + + for (size_t i = 0; i < stracksa.size(); i++) { + std::vector::iterator iter = find(dupa.begin(), dupa.end(), i); + if (iter == dupa.end()) { + resa.push_back(stracksa[i]); + } + } + + for (size_t i = 0; i < stracksb.size(); i++) { + std::vector::iterator iter = find(dupb.begin(), dupb.end(), i); + if (iter == dupb.end()) { + resb.push_back(stracksb[i]); + } + } +} + +void ByteTracker::linear_assignment( + std::vector> & cost_matrix, int cost_matrix_size, int cost_matrix_size_size, + float thresh, std::vector> & matches, std::vector & unmatched_a, + std::vector & unmatched_b) +{ + if (cost_matrix.size() == 0) { + for (int i = 0; i < cost_matrix_size; i++) { + unmatched_a.push_back(i); + } + for (int i = 0; i < cost_matrix_size_size; i++) { + unmatched_b.push_back(i); + } + return; + } + + std::vector rowsol; + std::vector colsol; + [[maybe_unused]] float c = lapjv(cost_matrix, rowsol, colsol, true, thresh); + for (size_t i = 0; i < rowsol.size(); i++) { + if (rowsol[i] >= 0) { + std::vector match; + match.push_back(i); + match.push_back(rowsol[i]); + matches.push_back(match); + } else { + unmatched_a.push_back(i); + } + } + + for (size_t i = 0; i < colsol.size(); i++) { + if (colsol[i] < 0) { + unmatched_b.push_back(i); + } + } +} + +std::vector> ByteTracker::distance( + std::vector & atracks, std::vector & btracks, int & dist_size, + int & dist_size_size) +{ + std::vector> cost_matrix; + if (atracks.size() * btracks.size() == 0) { + dist_size = atracks.size(); + dist_size_size = btracks.size(); + return cost_matrix; + } + + dist_size = atracks.size(); + dist_size_size = btracks.size(); + for (int i = 0; i < dist_size; i++) { + std::vector _iou; + for (int j = 0; j < dist_size_size; j++) { + _iou.push_back(std::sqrt( + std::pow(atracks[i]->pose[0] - btracks[j].pose[0], 2) + + std::pow(atracks[i]->pose[1] - btracks[j].pose[1], 2) + + std::pow(atracks[i]->pose[2] - btracks[j].pose[2], 2))); + } + cost_matrix.push_back(_iou); + } + + return cost_matrix; +} + +std::vector> ByteTracker::distance( + std::vector & atracks, std::vector & btracks) +{ + std::vector> cost_matrix; + for (size_t i = 0; i < atracks.size(); i++) { + std::vector _iou; + for (size_t j = 0; j < btracks.size(); j++) { + _iou.push_back(std::sqrt( + std::pow(atracks[i].pose[0] - btracks[j].pose[0], 2) + + std::pow(atracks[i].pose[1] - btracks[j].pose[1], 2) + + std::pow(atracks[i].pose[2] - btracks[j].pose[2], 2))); + } + cost_matrix.push_back(_iou); + } + + return cost_matrix; +} + +double ByteTracker::lapjv( + const std::vector> & cost, std::vector & rowsol, + std::vector & colsol, bool extend_cost, float cost_limit, bool return_cost) +{ + std::vector> cost_c; + cost_c.assign(cost.begin(), cost.end()); + + int n_rows = cost.size(); + int n_cols = cost[0].size(); + rowsol.resize(n_rows); + colsol.resize(n_cols); + + int n = 0; + if (n_rows == n_cols) { + n = n_rows; + } else { + if (!extend_cost) { + std::cout << "set extend_cost=True" << std::endl; + exit(0); + } + } + + if (extend_cost || cost_limit < LONG_MAX) { + std::vector> cost_c_extended; + + n = n_rows + n_cols; + cost_c_extended.resize(n); + for (size_t i = 0; i < cost_c_extended.size(); i++) cost_c_extended[i].resize(n); + + if (cost_limit < LONG_MAX) { + for (size_t i = 0; i < cost_c_extended.size(); i++) { + for (size_t j = 0; j < cost_c_extended[i].size(); j++) { + cost_c_extended[i][j] = cost_limit / 2.0; + } + } + } else { + float cost_max = -1; + for (size_t i = 0; i < cost_c.size(); i++) { + for (size_t j = 0; j < cost_c[i].size(); j++) { + if (cost_c[i][j] > cost_max) cost_max = cost_c[i][j]; + } + } + for (size_t i = 0; i < cost_c_extended.size(); i++) { + for (size_t j = 0; j < cost_c_extended[i].size(); j++) { + cost_c_extended[i][j] = cost_max + 1; + } + } + } + + for (size_t i = n_rows; i < cost_c_extended.size(); i++) { + for (size_t j = n_cols; j < cost_c_extended[i].size(); j++) { + cost_c_extended[i][j] = 0; + } + } + for (int i = 0; i < n_rows; i++) { + for (int j = 0; j < n_cols; j++) { + cost_c_extended[i][j] = cost_c[i][j]; + } + } + + cost_c.clear(); + cost_c.assign(cost_c_extended.begin(), cost_c_extended.end()); + } + + double ** cost_ptr; + cost_ptr = new double *[sizeof(double *) * n]; + for (int i = 0; i < n; i++) cost_ptr[i] = new double[sizeof(double) * n]; + + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + cost_ptr[i][j] = cost_c[i][j]; + } + } + + int * x_c = new int[sizeof(int) * n]; + int * y_c = new int[sizeof(int) * n]; + + int ret = lapjv_internal(n, cost_ptr, x_c, y_c); + if (ret != 0) { + std::cout << "Calculate Wrong!" << std::endl; + exit(0); + } + + double opt = 0.0; + + if (n != n_rows) { + for (int i = 0; i < n; i++) { + if (x_c[i] >= n_cols) x_c[i] = -1; + if (y_c[i] >= n_rows) y_c[i] = -1; + } + for (int i = 0; i < n_rows; i++) { + rowsol[i] = x_c[i]; + } + for (int i = 0; i < n_cols; i++) { + colsol[i] = y_c[i]; + } + + if (return_cost) { + for (size_t i = 0; i < rowsol.size(); i++) { + if (rowsol[i] != -1) { + opt += cost_ptr[i][rowsol[i]]; + } + } + } + } else if (return_cost) { + for (size_t i = 0; i < rowsol.size(); i++) { + opt += cost_ptr[i][rowsol[i]]; + } + } + + for (int i = 0; i < n; i++) { + delete[] cost_ptr[i]; + } + delete[] cost_ptr; + delete[] x_c; + delete[] y_c; + + return opt; +} + +cv::Scalar ByteTracker::get_color(int idx) +{ + idx += 3; + return cv::Scalar(37 * idx % 255, 17 * idx % 255, 29 * idx % 255); +} diff --git a/perception/autoware_bytetrack3D/lib/src/ykalman_filter.cpp b/perception/autoware_bytetrack3D/lib/src/ykalman_filter.cpp new file mode 100644 index 0000000000000..1cf60b290a778 --- /dev/null +++ b/perception/autoware_bytetrack3D/lib/src/ykalman_filter.cpp @@ -0,0 +1,48 @@ +// 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. + +#include "ykalman_filter.h" + +#include "math.h" + +void yKalmanFilter::setX(Eigen::MatrixXd state) +{ + this->x_ = state; +} + +float yKalmanFilter::normalize_theta(float theta) +{ + if (theta >= M_PI) theta -= M_PI * 2; + if (theta < -M_PI) theta += M_PI * 2; + + return theta; +} + +float yKalmanFilter::yaw_correction(float pre_yaw, float obs_yaw) +{ + obs_yaw = normalize_theta(obs_yaw); + pre_yaw = normalize_theta(pre_yaw); + + if (std::abs(obs_yaw - pre_yaw) > M_PI / 2.0 && std::abs(obs_yaw - pre_yaw) < M_PI * 3 / 2.0) { + obs_yaw += M_PI; + obs_yaw = normalize_theta(obs_yaw); + } + + if (std::abs(obs_yaw - pre_yaw) >= M_PI * 3 / 2.0) { + obs_yaw = -obs_yaw; + obs_yaw = normalize_theta(obs_yaw); + } + + return obs_yaw; +} diff --git a/perception/autoware_bytetrack3D/package.xml b/perception/autoware_bytetrack3D/package.xml new file mode 100644 index 0000000000000..31337f2a8d39a --- /dev/null +++ b/perception/autoware_bytetrack3D/package.xml @@ -0,0 +1,37 @@ + + + + autoware_bytetrack3d + 0.0.1 + ByteTrack3D implementation ported toward Autoware + Manato HIRABAYASHI + Yoshi RI + beginning.fan + cyn-liu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_kalman_filter + autoware_perception_msgs + cv_bridge + eigen + image_transport + libboost-system-dev + libopencv-dev + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_geometry_msgs + tier4_perception_msgs + yaml-cpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/autoware_bytetrack3D/src/bytetrack3D.cpp b/perception/autoware_bytetrack3D/src/bytetrack3D.cpp new file mode 100644 index 0000000000000..7457437f756b5 --- /dev/null +++ b/perception/autoware_bytetrack3D/src/bytetrack3D.cpp @@ -0,0 +1,98 @@ +// 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. + +#include + +#include +#include +#include + +namespace bytetrack3D +{ +ByteTrack3D::ByteTrack3D(const int track_buffer_length) +{ + // Tracker initialization + tracker_ = std::make_unique(track_buffer_length); +} + +bool ByteTrack3D::do_inference(ObjectArray & objects) +{ + // Re-format data + std::vector bytetrack_objects; + for (const auto & obj : objects) { + ByteTrackObject bytetrack_obj; + bytetrack_obj.x = obj.x; + bytetrack_obj.y = obj.y; + bytetrack_obj.z = obj.z; + bytetrack_obj.yaw = obj.yaw; + bytetrack_obj.l = obj.l; + bytetrack_obj.w = obj.w; + bytetrack_obj.h = obj.h; + bytetrack_obj.prob = obj.score; + bytetrack_obj.label = obj.type; + bytetrack_objects.emplace_back(bytetrack_obj); + } + + // cspell: ignore stracks tlwh + // Update tracker + std::vector output_stracks = tracker_->update(bytetrack_objects); + + // Pack results + latest_objects_.clear(); + for (const auto & tracking_result : output_stracks) { + Object object{}; + std::vector pose = tracking_result.pose; + std::vector lwh = tracking_result.lwh; + std::vector velocity = tracking_result.velocity; + object.x = pose[0]; + object.y = pose[1]; + object.z = pose[2]; + object.yaw = pose[3]; + object.l = lwh[0]; + object.w = lwh[1]; + object.h = lwh[2]; + object.vx = velocity[0]; + object.vy = velocity[1]; + object.vz = velocity[2]; + object.vyaw = velocity[3]; + object.score = tracking_result.score; + object.type = tracking_result.label; + object.track_id = tracking_result.track_id; + object.unique_id = tracking_result.unique_id; + latest_objects_.emplace_back(object); + } + + return true; +} + +ObjectArray ByteTrack3D::update_tracker(ObjectArray & input_objects) +{ + do_inference(input_objects); + return latest_objects_; +} +} // namespace bytetrack3D diff --git a/perception/autoware_bytetrack3D/src/bytetrack3D_node.cpp b/perception/autoware_bytetrack3D/src/bytetrack3D_node.cpp new file mode 100644 index 0000000000000..bb8a973ecccc9 --- /dev/null +++ b/perception/autoware_bytetrack3D/src/bytetrack3D_node.cpp @@ -0,0 +1,124 @@ +// 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. + +#include "bytetrack3D/bytetrack3D.hpp" + +#include +#include + +#include "autoware_perception_msgs/msg/object_classification.hpp" + +#include + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +namespace bytetrack3D +{ +ByteTrack3DNode::ByteTrack3DNode(const rclcpp::NodeOptions & node_options) +: Node("bytetrack3D", node_options) +{ + using std::placeholders::_1; + using std::chrono_literals::operator""ms; + + int track_buffer_length = declare_parameter("track_buffer_length", 30); + + this->bytetrack3D_ = std::make_unique(track_buffer_length); + + detection_rect_sub_ = this->create_subscription( + "~/in/objects", 1, std::bind(&ByteTrack3DNode::on_rect, this, _1)); + + objects_pub_ = + this->create_publisher("~/out/objects", 1); +} + +void ByteTrack3DNode::on_rect( + const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; + + autoware_perception_msgs::msg::TrackedObjects out_objects; + + // Unpack detection results + bytetrack3D::ObjectArray object_array; + for (const auto & feat_obj : msg->objects) { + Object obj; + obj.x = feat_obj.kinematics.pose_with_covariance.pose.position.x; + obj.y = feat_obj.kinematics.pose_with_covariance.pose.position.y; + obj.z = feat_obj.kinematics.pose_with_covariance.pose.position.z; + auto q = feat_obj.kinematics.pose_with_covariance.pose.orientation; + obj.yaw = + std::atan2(2.0 * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z); + obj.l = feat_obj.shape.dimensions.x; + obj.w = feat_obj.shape.dimensions.y; + obj.h = feat_obj.shape.dimensions.z; + obj.score = feat_obj.existence_probability; + obj.type = feat_obj.classification.front().label; + object_array.emplace_back(obj); + } + + bytetrack3D::ObjectArray objects = bytetrack3D_->update_tracker(object_array); + for (const auto & tracked_object : objects) { + autoware_perception_msgs::msg::TrackedObject object; + unique_identifier_msgs::msg::UUID uuid_msg; + auto tracked_uuid = tracked_object.unique_id; + std::memcpy(uuid_msg.uuid.data(), &tracked_uuid, tracked_uuid.size()); + object.object_id = uuid_msg; + object.existence_probability = tracked_object.score; + object.classification.emplace_back( + autoware_perception_msgs::build