├── .gitignore ├── CMakeLists.txt ├── Dockerfile ├── LICENSE ├── README.md ├── data └── YOLOX_ncnn_palace │ ├── detection_results.json │ └── tracking_results.json ├── include └── ByteTrack │ ├── BYTETracker.h │ ├── KalmanFilter.h │ ├── Object.h │ ├── Rect.h │ ├── STrack.h │ └── lapjv.h ├── src ├── BYTETracker.cpp ├── KalmanFilter.cpp ├── Object.cpp ├── Rect.cpp ├── STrack.cpp └── lapjv.cpp └── test └── test_BYTETracker.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | build/* 2 | 3 | # Prerequisites 4 | *.d 5 | 6 | # Compiled Object files 7 | *.slo 8 | *.lo 9 | *.o 10 | *.obj 11 | 12 | # Precompiled Headers 13 | *.gch 14 | *.pch 15 | 16 | # Compiled Dynamic libraries 17 | *.so 18 | *.dylib 19 | *.dll 20 | 21 | # Fortran module files 22 | *.mod 23 | *.smod 24 | 25 | # Compiled Static libraries 26 | *.lai 27 | *.la 28 | *.a 29 | *.lib 30 | 31 | # Executables 32 | *.exe 33 | *.out 34 | *.app 35 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | project(bytetrack) 3 | 4 | IF(NOT CMAKE_BUILD_TYPE) 5 | SET(CMAKE_BUILD_TYPE Release) 6 | ENDIF() 7 | 8 | MESSAGE("Build type: " ${CMAKE_BUILD_TYPE}) 9 | 10 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O2") 11 | set(CMAKE_CXX_FLAGS_DEBUG "-O0 -g -MMD -Wall -Wextra -Winit-self") 12 | 13 | # Check C++17 support 14 | include(CheckCXXCompilerFlag) 15 | CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17) 16 | if(COMPILER_SUPPORTS_CXX17) 17 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") 18 | add_definitions(-DCOMPILEDWITHC17) 19 | message(STATUS "Using flag -std=c++17.") 20 | else() 21 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++17 support. Please use a different C++ compiler.") 22 | endif() 23 | 24 | set(EIGEN_VERSION 3.3) 25 | find_package(Eigen3 ${EIGEN_VERSION} QUIET) 26 | if(NOT EIGEN3_FOUND) 27 | set(BUILD_TESTING OFF CACHE INTERNAL "") 28 | FetchContent_Declare(eigen 29 | GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git 30 | GIT_TAG ${EIGEN_VERSION} 31 | GIT_SHALLOW ON) 32 | FetchContent_MakeAvailable(eigen) 33 | unset(BUILD_TESTING CACHE) 34 | endif() 35 | 36 | add_library(${PROJECT_NAME} SHARED 37 | ${CMAKE_CURRENT_SOURCE_DIR}/src/BYTETracker.cpp 38 | ${CMAKE_CURRENT_SOURCE_DIR}/src/KalmanFilter.cpp 39 | ${CMAKE_CURRENT_SOURCE_DIR}/src/lapjv.cpp 40 | ${CMAKE_CURRENT_SOURCE_DIR}/src/Object.cpp 41 | ${CMAKE_CURRENT_SOURCE_DIR}/src/Rect.cpp 42 | ${CMAKE_CURRENT_SOURCE_DIR}/src/STrack.cpp 43 | ) 44 | target_include_directories(${PROJECT_NAME} PUBLIC 45 | ${CMAKE_CURRENT_SOURCE_DIR}/include 46 | ) 47 | target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) 48 | 49 | # Build the tests if the 'BUILD_BYTETRACK_TEST' variable is set to 'ON' 50 | set(BUILD_BYTETRACK_TEST OFF CACHE BOOL "The flag whether to build the tests or not") 51 | if(BUILD_BYTETRACK_TEST) 52 | enable_testing() 53 | 54 | find_package(GTest QUIET) 55 | if(NOT GTest_FOUND) 56 | message(FATAL_ERROR "GTest not found.") 57 | endif() 58 | 59 | add_executable(${PROJECT_NAME}_test 60 | ${CMAKE_CURRENT_SOURCE_DIR}/test/test_BYTETracker.cpp 61 | ) 62 | 63 | target_link_libraries(${PROJECT_NAME}_test 64 | ${PROJECT_NAME} 65 | GTest::GTest 66 | GTest::Main 67 | ) 68 | 69 | gtest_discover_tests(${PROJECT_NAME}_test 70 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/data/YOLOX_ncnn_palace) 71 | endif() 72 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:20.04 2 | ENV DEBIAN_FRONTEND noninteractive 3 | 4 | RUN apt update && apt install -y --no-install-recommends \ 5 | build-essential \ 6 | gdb \ 7 | git \ 8 | ca-certificates \ 9 | libssl-dev \ 10 | pkg-config \ 11 | wget \ 12 | unzip \ 13 | vim \ 14 | tmux && \ 15 | apt clean && \ 16 | rm -rf /var/lib/apt/lists/* 17 | 18 | # Install GTest 19 | RUN apt update && apt install -y --no-install-recommends \ 20 | libgtest-dev && \ 21 | apt clean && \ 22 | rm -rf /var/lib/apt/lists/* 23 | 24 | # Install Boost 25 | RUN apt update && apt install -y --no-install-recommends \ 26 | libboost-dev && \ 27 | apt clean && \ 28 | rm -rf /var/lib/apt/lists/* 29 | 30 | # Install latest CMake 31 | RUN git clone -b release --depth=1 https://github.com/Kitware/CMake.git && cd CMake && \ 32 | ./bootstrap && make -j "$(nproc)" && make install && \ 33 | cd ../ && rm -rf CMake 34 | 35 | # Install Eigen 3.3.9 36 | ENV EIGEN_VERSION="3.3.9" 37 | RUN mkdir -p /tmp/eigen && cd /tmp/eigen && \ 38 | wget https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.zip && \ 39 | unzip eigen-${EIGEN_VERSION}.zip -d . && \ 40 | mkdir /tmp/eigen/eigen-${EIGEN_VERSION}/build && cd /tmp/eigen/eigen-${EIGEN_VERSION}/build/ && \ 41 | cmake .. && \ 42 | make install && \ 43 | cd /tmp && rm -rf eigen 44 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Vertical Beach (lp6m, medalotte) 4 | Copyright (c) 2021 Yifu Zhang 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ByteTrack-cpp 2 | 3 | C++ implementation of ByteTrack that does not include an object detection algorithm. 4 | 5 | ## Overview 6 | 7 | - The implementation is based on [ByteTrack-CPP-ncnn](https://github.com/ifzhang/ByteTrack/tree/3434c5e8bc6a5ae8ad530528ba8d9a431967f237/deploy/ncnn/cpp) that is the official C++ implementation using ncnn 8 | - Only tracking algorithm are implemented in this repository 9 | - Any object detection algorithm can be easily combined 10 | - Provided as a shared library usable in C++17 or higher 11 | - The output of the implementation has been verified to be equivalent to the output of the [ByteTrack-CPP-ncnn](https://github.com/ifzhang/ByteTrack/tree/3434c5e8bc6a5ae8ad530528ba8d9a431967f237/deploy/ncnn/cpp) 12 | - [Verification data](data/YOLOX_ncnn_palace) are generated by [ByteTrack-CPP-ncnn](https://github.com/ifzhang/ByteTrack/tree/3434c5e8bc6a5ae8ad530528ba8d9a431967f237/deploy/ncnn/cpp) using YOLOX 13 | 14 | ## Dependencies 15 | 16 | - Eigen 3.3 17 | - C++ compiler with C++17 or higher support 18 | - CMake 3.14 or higher 19 | - GoogleTest 1.10 or higher (Only tests) 20 | 21 | ## Build and Test 22 | 23 | The shared library (libbytetrack.so) can be build with following commands: 24 | 25 | ```shell 26 | mkdir build && cd build 27 | cmake .. 28 | make 29 | ``` 30 | 31 | The implementation can be test with following commands: 32 | 33 | ```shell 34 | mkdir build && cd build 35 | cmake .. -DBUILD_BYTETRACK_TEST=ON 36 | make 37 | ctest --verbose 38 | ``` 39 | 40 | ## Tips 41 | 42 | You can use docker container to build and test the implementation. 43 | 44 | ```shell 45 | docker build . -t bytetrack-cpp:latest 46 | docker run -ti --rm \ 47 | -v ${PWD}:/usr/src/app \ 48 | -w /usr/src/app \ 49 | bytetrack-cpp:latest 50 | ``` 51 | 52 | ## License 53 | 54 | MIT 55 | -------------------------------------------------------------------------------- /include/ByteTrack/BYTETracker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "ByteTrack/STrack.h" 4 | #include "ByteTrack/lapjv.h" 5 | #include "ByteTrack/Object.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace byte_track 14 | { 15 | class BYTETracker 16 | { 17 | public: 18 | using STrackPtr = std::shared_ptr; 19 | 20 | BYTETracker(const int& frame_rate = 30, 21 | const int& track_buffer = 30, 22 | const float& track_thresh = 0.5, 23 | const float& high_thresh = 0.6, 24 | const float& match_thresh = 0.8); 25 | ~BYTETracker(); 26 | 27 | std::vector update(const std::vector& objects); 28 | 29 | private: 30 | std::vector jointStracks(const std::vector &a_tlist, 31 | const std::vector &b_tlist) const; 32 | 33 | std::vector subStracks(const std::vector &a_tlist, 34 | const std::vector &b_tlist) const; 35 | 36 | void removeDuplicateStracks(const std::vector &a_stracks, 37 | const std::vector &b_stracks, 38 | std::vector &a_res, 39 | std::vector &b_res) const; 40 | 41 | void linearAssignment(const std::vector> &cost_matrix, 42 | const int &cost_matrix_size, 43 | const int &cost_matrix_size_size, 44 | const float &thresh, 45 | std::vector> &matches, 46 | std::vector &b_unmatched, 47 | std::vector &a_unmatched) const; 48 | 49 | std::vector> calcIouDistance(const std::vector &a_tracks, 50 | const std::vector &b_tracks) const; 51 | 52 | std::vector> calcIous(const std::vector> &a_rect, 53 | const std::vector> &b_rect) const; 54 | 55 | double execLapjv(const std::vector > &cost, 56 | std::vector &rowsol, 57 | std::vector &colsol, 58 | bool extend_cost = false, 59 | float cost_limit = std::numeric_limits::max(), 60 | bool return_cost = true) const; 61 | 62 | private: 63 | const float track_thresh_; 64 | const float high_thresh_; 65 | const float match_thresh_; 66 | const size_t max_time_lost_; 67 | 68 | size_t frame_id_; 69 | size_t track_id_count_; 70 | 71 | std::vector tracked_stracks_; 72 | std::vector lost_stracks_; 73 | std::vector removed_stracks_; 74 | }; 75 | } -------------------------------------------------------------------------------- /include/ByteTrack/KalmanFilter.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Eigen/Dense" 4 | 5 | #include "ByteTrack/Rect.h" 6 | 7 | namespace byte_track 8 | { 9 | class KalmanFilter 10 | { 11 | public: 12 | using DetectBox = Xyah; 13 | 14 | using StateMean = Eigen::Matrix; 15 | using StateCov = Eigen::Matrix; 16 | 17 | using StateHMean = Eigen::Matrix; 18 | using StateHCov = Eigen::Matrix; 19 | 20 | KalmanFilter(const float& std_weight_position = 1. / 20, 21 | const float& std_weight_velocity = 1. / 160); 22 | 23 | void initiate(StateMean& mean, StateCov& covariance, const DetectBox& measurement); 24 | 25 | void predict(StateMean& mean, StateCov& covariance); 26 | 27 | void update(StateMean& mean, StateCov& covariance, const DetectBox& measurement); 28 | 29 | private: 30 | float std_weight_position_; 31 | float std_weight_velocity_; 32 | 33 | Eigen::Matrix motion_mat_; 34 | Eigen::Matrix update_mat_; 35 | 36 | void project(StateHMean &projected_mean, StateHCov &projected_covariance, 37 | const StateMean& mean, const StateCov& covariance); 38 | }; 39 | } -------------------------------------------------------------------------------- /include/ByteTrack/Object.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "ByteTrack/Rect.h" 4 | 5 | namespace byte_track 6 | { 7 | struct Object 8 | { 9 | Rect rect; 10 | int label; 11 | float prob; 12 | 13 | Object(const Rect &_rect, 14 | const int &_label, 15 | const float &_prob); 16 | }; 17 | } -------------------------------------------------------------------------------- /include/ByteTrack/Rect.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Eigen/Dense" 4 | 5 | namespace byte_track 6 | { 7 | template 8 | using Tlwh = Eigen::Matrix; 9 | 10 | template 11 | using Tlbr = Eigen::Matrix; 12 | 13 | template 14 | using Xyah = Eigen::Matrix; 15 | 16 | template 17 | class Rect 18 | { 19 | public: 20 | Tlwh tlwh; 21 | 22 | Rect() = default; 23 | Rect(const T &x, const T &y, const T &width, const T &height); 24 | 25 | ~Rect(); 26 | 27 | const T &x() const; 28 | const T &y() const; 29 | const T &width() const; 30 | const T &height() const; 31 | 32 | T &x(); 33 | T &y(); 34 | T &width(); 35 | T &height(); 36 | 37 | const T &tl_x() const; 38 | const T &tl_y() const; 39 | T br_x() const; 40 | T br_y() const; 41 | 42 | Tlbr getTlbr() const; 43 | Xyah getXyah() const; 44 | 45 | float calcIoU(const Rect& other) const; 46 | }; 47 | 48 | template 49 | Rect generate_rect_by_tlbr(const Tlbr& tlbr); 50 | 51 | template 52 | Rect generate_rect_by_xyah(const Xyah& xyah); 53 | 54 | } -------------------------------------------------------------------------------- /include/ByteTrack/STrack.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "ByteTrack/Rect.h" 4 | #include "ByteTrack/KalmanFilter.h" 5 | 6 | #include 7 | 8 | namespace byte_track 9 | { 10 | enum class STrackState { 11 | New = 0, 12 | Tracked = 1, 13 | Lost = 2, 14 | Removed = 3, 15 | }; 16 | 17 | class STrack 18 | { 19 | public: 20 | STrack(const Rect& rect, const float& score); 21 | ~STrack(); 22 | 23 | const Rect& getRect() const; 24 | const STrackState& getSTrackState() const; 25 | 26 | const bool& isActivated() const; 27 | const float& getScore() const; 28 | const size_t& getTrackId() const; 29 | const size_t& getFrameId() const; 30 | const size_t& getStartFrameId() const; 31 | const size_t& getTrackletLength() const; 32 | 33 | void activate(const size_t& frame_id, const size_t& track_id); 34 | void reActivate(const STrack &new_track, const size_t &frame_id, const int &new_track_id = -1); 35 | 36 | void predict(); 37 | void update(const STrack &new_track, const size_t &frame_id); 38 | 39 | void markAsLost(); 40 | void markAsRemoved(); 41 | 42 | private: 43 | KalmanFilter kalman_filter_; 44 | KalmanFilter::StateMean mean_; 45 | KalmanFilter::StateCov covariance_; 46 | 47 | Rect rect_; 48 | STrackState state_; 49 | 50 | bool is_activated_; 51 | float score_; 52 | size_t track_id_; 53 | size_t frame_id_; 54 | size_t start_frame_id_; 55 | size_t tracklet_len_; 56 | 57 | void updateRect(); 58 | }; 59 | } -------------------------------------------------------------------------------- /include/ByteTrack/lapjv.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace byte_track 6 | { 7 | int lapjv_internal(const size_t n, double *cost[], int *x, int *y); 8 | } -------------------------------------------------------------------------------- /src/BYTETracker.cpp: -------------------------------------------------------------------------------- 1 | #include "ByteTrack/BYTETracker.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | byte_track::BYTETracker::BYTETracker(const int& frame_rate, 12 | const int& track_buffer, 13 | const float& track_thresh, 14 | const float& high_thresh, 15 | const float& match_thresh) : 16 | track_thresh_(track_thresh), 17 | high_thresh_(high_thresh), 18 | match_thresh_(match_thresh), 19 | max_time_lost_(static_cast(frame_rate / 30.0 * track_buffer)), 20 | frame_id_(0), 21 | track_id_count_(0) 22 | { 23 | } 24 | 25 | byte_track::BYTETracker::~BYTETracker() 26 | { 27 | } 28 | 29 | std::vector byte_track::BYTETracker::update(const std::vector& objects) 30 | { 31 | ////////////////// Step 1: Get detections ////////////////// 32 | frame_id_++; 33 | 34 | // Create new STracks using the result of object detection 35 | std::vector det_stracks; 36 | std::vector det_low_stracks; 37 | 38 | for (const auto &object : objects) 39 | { 40 | const auto strack = std::make_shared(object.rect, object.prob); 41 | if (object.prob >= track_thresh_) 42 | { 43 | det_stracks.push_back(strack); 44 | } 45 | else 46 | { 47 | det_low_stracks.push_back(strack); 48 | } 49 | } 50 | 51 | // Create lists of existing STrack 52 | std::vector active_stracks; 53 | std::vector non_active_stracks; 54 | std::vector strack_pool; 55 | 56 | for (const auto& tracked_strack : tracked_stracks_) 57 | { 58 | if (!tracked_strack->isActivated()) 59 | { 60 | non_active_stracks.push_back(tracked_strack); 61 | } 62 | else 63 | { 64 | active_stracks.push_back(tracked_strack); 65 | } 66 | } 67 | 68 | strack_pool = jointStracks(active_stracks, lost_stracks_); 69 | 70 | // Predict current pose by KF 71 | for (auto &strack : strack_pool) 72 | { 73 | strack->predict(); 74 | } 75 | 76 | ////////////////// Step 2: First association, with IoU ////////////////// 77 | std::vector current_tracked_stracks; 78 | std::vector remain_tracked_stracks; 79 | std::vector remain_det_stracks; 80 | std::vector refind_stracks; 81 | 82 | { 83 | std::vector> matches_idx; 84 | std::vector unmatch_detection_idx, unmatch_track_idx; 85 | 86 | const auto dists = calcIouDistance(strack_pool, det_stracks); 87 | linearAssignment(dists, strack_pool.size(), det_stracks.size(), match_thresh_, 88 | matches_idx, unmatch_track_idx, unmatch_detection_idx); 89 | 90 | for (const auto &match_idx : matches_idx) 91 | { 92 | const auto track = strack_pool[match_idx[0]]; 93 | const auto det = det_stracks[match_idx[1]]; 94 | if (track->getSTrackState() == STrackState::Tracked) 95 | { 96 | track->update(*det, frame_id_); 97 | current_tracked_stracks.push_back(track); 98 | } 99 | else 100 | { 101 | track->reActivate(*det, frame_id_); 102 | refind_stracks.push_back(track); 103 | } 104 | } 105 | 106 | for (const auto &unmatch_idx : unmatch_detection_idx) 107 | { 108 | remain_det_stracks.push_back(det_stracks[unmatch_idx]); 109 | } 110 | 111 | for (const auto &unmatch_idx : unmatch_track_idx) 112 | { 113 | if (strack_pool[unmatch_idx]->getSTrackState() == STrackState::Tracked) 114 | { 115 | remain_tracked_stracks.push_back(strack_pool[unmatch_idx]); 116 | } 117 | } 118 | } 119 | 120 | ////////////////// Step 3: Second association, using low score dets ////////////////// 121 | std::vector current_lost_stracks; 122 | 123 | { 124 | std::vector> matches_idx; 125 | std::vector unmatch_track_idx, unmatch_detection_idx; 126 | 127 | const auto dists = calcIouDistance(remain_tracked_stracks, det_low_stracks); 128 | linearAssignment(dists, remain_tracked_stracks.size(), det_low_stracks.size(), 0.5, 129 | matches_idx, unmatch_track_idx, unmatch_detection_idx); 130 | 131 | for (const auto &match_idx : matches_idx) 132 | { 133 | const auto track = remain_tracked_stracks[match_idx[0]]; 134 | const auto det = det_low_stracks[match_idx[1]]; 135 | if (track->getSTrackState() == STrackState::Tracked) 136 | { 137 | track->update(*det, frame_id_); 138 | current_tracked_stracks.push_back(track); 139 | } 140 | else 141 | { 142 | track->reActivate(*det, frame_id_); 143 | refind_stracks.push_back(track); 144 | } 145 | } 146 | 147 | for (const auto &unmatch_track : unmatch_track_idx) 148 | { 149 | const auto track = remain_tracked_stracks[unmatch_track]; 150 | if (track->getSTrackState() != STrackState::Lost) 151 | { 152 | track->markAsLost(); 153 | current_lost_stracks.push_back(track); 154 | } 155 | } 156 | } 157 | 158 | ////////////////// Step 4: Init new stracks ////////////////// 159 | std::vector current_removed_stracks; 160 | 161 | { 162 | std::vector unmatch_detection_idx; 163 | std::vector unmatch_unconfirmed_idx; 164 | std::vector> matches_idx; 165 | 166 | // Deal with unconfirmed tracks, usually tracks with only one beginning frame 167 | const auto dists = calcIouDistance(non_active_stracks, remain_det_stracks); 168 | linearAssignment(dists, non_active_stracks.size(), remain_det_stracks.size(), 0.7, 169 | matches_idx, unmatch_unconfirmed_idx, unmatch_detection_idx); 170 | 171 | for (const auto &match_idx : matches_idx) 172 | { 173 | non_active_stracks[match_idx[0]]->update(*remain_det_stracks[match_idx[1]], frame_id_); 174 | current_tracked_stracks.push_back(non_active_stracks[match_idx[0]]); 175 | } 176 | 177 | for (const auto &unmatch_idx : unmatch_unconfirmed_idx) 178 | { 179 | const auto track = non_active_stracks[unmatch_idx]; 180 | track->markAsRemoved(); 181 | current_removed_stracks.push_back(track); 182 | } 183 | 184 | // Add new stracks 185 | for (const auto &unmatch_idx : unmatch_detection_idx) 186 | { 187 | const auto track = remain_det_stracks[unmatch_idx]; 188 | if (track->getScore() < high_thresh_) 189 | { 190 | continue; 191 | } 192 | track_id_count_++; 193 | track->activate(frame_id_, track_id_count_); 194 | current_tracked_stracks.push_back(track); 195 | } 196 | } 197 | 198 | ////////////////// Step 5: Update state ////////////////// 199 | for (const auto &lost_strack : lost_stracks_) 200 | { 201 | if (frame_id_ - lost_strack->getFrameId() > max_time_lost_) 202 | { 203 | lost_strack->markAsRemoved(); 204 | current_removed_stracks.push_back(lost_strack); 205 | } 206 | } 207 | 208 | tracked_stracks_ = jointStracks(current_tracked_stracks, refind_stracks); 209 | lost_stracks_ = subStracks(jointStracks(subStracks(lost_stracks_, tracked_stracks_), current_lost_stracks), removed_stracks_); 210 | removed_stracks_ = jointStracks(removed_stracks_, current_removed_stracks); 211 | 212 | std::vector tracked_stracks_out, lost_stracks_out; 213 | removeDuplicateStracks(tracked_stracks_, lost_stracks_, tracked_stracks_out, lost_stracks_out); 214 | tracked_stracks_ = tracked_stracks_out; 215 | lost_stracks_ = lost_stracks_out; 216 | 217 | std::vector output_stracks; 218 | for (const auto &track : tracked_stracks_) 219 | { 220 | if (track->isActivated()) 221 | { 222 | output_stracks.push_back(track); 223 | } 224 | } 225 | 226 | return output_stracks; 227 | } 228 | std::vector byte_track::BYTETracker::jointStracks(const std::vector &a_tlist, 229 | const std::vector &b_tlist) const 230 | { 231 | std::map exists; 232 | std::vector res; 233 | for (size_t i = 0; i < a_tlist.size(); i++) 234 | { 235 | exists.emplace(a_tlist[i]->getTrackId(), 1); 236 | res.push_back(a_tlist[i]); 237 | } 238 | for (size_t i = 0; i < b_tlist.size(); i++) 239 | { 240 | const int &tid = b_tlist[i]->getTrackId(); 241 | if (!exists[tid] || exists.count(tid) == 0) 242 | { 243 | exists[tid] = 1; 244 | res.push_back(b_tlist[i]); 245 | } 246 | } 247 | return res; 248 | } 249 | 250 | std::vector byte_track::BYTETracker::subStracks(const std::vector &a_tlist, 251 | const std::vector &b_tlist) const 252 | { 253 | std::map stracks; 254 | for (size_t i = 0; i < a_tlist.size(); i++) 255 | { 256 | stracks.emplace(a_tlist[i]->getTrackId(), a_tlist[i]); 257 | } 258 | 259 | for (size_t i = 0; i < b_tlist.size(); i++) 260 | { 261 | const int &tid = b_tlist[i]->getTrackId(); 262 | if (stracks.count(tid) != 0) 263 | { 264 | stracks.erase(tid); 265 | } 266 | } 267 | 268 | std::vector res; 269 | std::map::iterator it; 270 | for (it = stracks.begin(); it != stracks.end(); ++it) 271 | { 272 | res.push_back(it->second); 273 | } 274 | 275 | return res; 276 | } 277 | 278 | void byte_track::BYTETracker::removeDuplicateStracks(const std::vector &a_stracks, 279 | const std::vector &b_stracks, 280 | std::vector &a_res, 281 | std::vector &b_res) const 282 | { 283 | const auto ious = calcIouDistance(a_stracks, b_stracks); 284 | 285 | std::vector> overlapping_combinations; 286 | for (size_t i = 0; i < ious.size(); i++) 287 | { 288 | for (size_t j = 0; j < ious[i].size(); j++) 289 | { 290 | if (ious[i][j] < 0.15) 291 | { 292 | overlapping_combinations.emplace_back(i, j); 293 | } 294 | } 295 | } 296 | 297 | std::vector a_overlapping(a_stracks.size(), false), b_overlapping(b_stracks.size(), false); 298 | for (const auto &[a_idx, b_idx] : overlapping_combinations) 299 | { 300 | const int timep = a_stracks[a_idx]->getFrameId() - a_stracks[a_idx]->getStartFrameId(); 301 | const int timeq = b_stracks[b_idx]->getFrameId() - b_stracks[b_idx]->getStartFrameId(); 302 | if (timep > timeq) 303 | { 304 | b_overlapping[b_idx] = true; 305 | } 306 | else 307 | { 308 | a_overlapping[a_idx] = true; 309 | } 310 | } 311 | 312 | for (size_t ai = 0; ai < a_stracks.size(); ai++) 313 | { 314 | if (!a_overlapping[ai]) 315 | { 316 | a_res.push_back(a_stracks[ai]); 317 | } 318 | } 319 | 320 | for (size_t bi = 0; bi < b_stracks.size(); bi++) 321 | { 322 | if (!b_overlapping[bi]) 323 | { 324 | b_res.push_back(b_stracks[bi]); 325 | } 326 | } 327 | } 328 | 329 | void byte_track::BYTETracker::linearAssignment(const std::vector> &cost_matrix, 330 | const int &cost_matrix_size, 331 | const int &cost_matrix_size_size, 332 | const float &thresh, 333 | std::vector> &matches, 334 | std::vector &a_unmatched, 335 | std::vector &b_unmatched) const 336 | { 337 | if (cost_matrix.size() == 0) 338 | { 339 | for (int i = 0; i < cost_matrix_size; i++) 340 | { 341 | a_unmatched.push_back(i); 342 | } 343 | for (int i = 0; i < cost_matrix_size_size; i++) 344 | { 345 | b_unmatched.push_back(i); 346 | } 347 | return; 348 | } 349 | 350 | std::vector rowsol; std::vector colsol; 351 | execLapjv(cost_matrix, rowsol, colsol, true, thresh); 352 | for (size_t i = 0; i < rowsol.size(); i++) 353 | { 354 | if (rowsol[i] >= 0) 355 | { 356 | std::vector match; 357 | match.push_back(i); 358 | match.push_back(rowsol[i]); 359 | matches.push_back(match); 360 | } 361 | else 362 | { 363 | a_unmatched.push_back(i); 364 | } 365 | } 366 | 367 | for (size_t i = 0; i < colsol.size(); i++) 368 | { 369 | if (colsol[i] < 0) 370 | { 371 | b_unmatched.push_back(i); 372 | } 373 | } 374 | } 375 | 376 | std::vector> byte_track::BYTETracker::calcIous(const std::vector> &a_rect, 377 | const std::vector> &b_rect) const 378 | { 379 | std::vector> ious; 380 | if (a_rect.size() * b_rect.size() == 0) 381 | { 382 | return ious; 383 | } 384 | 385 | ious.resize(a_rect.size()); 386 | for (size_t i = 0; i < ious.size(); i++) 387 | { 388 | ious[i].resize(b_rect.size()); 389 | } 390 | 391 | for (size_t bi = 0; bi < b_rect.size(); bi++) 392 | { 393 | for (size_t ai = 0; ai < a_rect.size(); ai++) 394 | { 395 | ious[ai][bi] = b_rect[bi].calcIoU(a_rect[ai]); 396 | } 397 | } 398 | return ious; 399 | } 400 | 401 | std::vector > byte_track::BYTETracker::calcIouDistance(const std::vector &a_tracks, 402 | const std::vector &b_tracks) const 403 | { 404 | std::vector> a_rects, b_rects; 405 | for (size_t i = 0; i < a_tracks.size(); i++) 406 | { 407 | a_rects.push_back(a_tracks[i]->getRect()); 408 | } 409 | 410 | for (size_t i = 0; i < b_tracks.size(); i++) 411 | { 412 | b_rects.push_back(b_tracks[i]->getRect()); 413 | } 414 | 415 | const auto ious = calcIous(a_rects, b_rects); 416 | 417 | std::vector> cost_matrix; 418 | for (size_t i = 0; i < ious.size(); i++) 419 | { 420 | std::vector iou; 421 | for (size_t j = 0; j < ious[i].size(); j++) 422 | { 423 | iou.push_back(1 - ious[i][j]); 424 | } 425 | cost_matrix.push_back(iou); 426 | } 427 | 428 | return cost_matrix; 429 | } 430 | 431 | double byte_track::BYTETracker::execLapjv(const std::vector> &cost, 432 | std::vector &rowsol, 433 | std::vector &colsol, 434 | bool extend_cost, 435 | float cost_limit, 436 | bool return_cost) const 437 | { 438 | std::vector > cost_c; 439 | cost_c.assign(cost.begin(), cost.end()); 440 | 441 | std::vector > cost_c_extended; 442 | 443 | int n_rows = cost.size(); 444 | int n_cols = cost[0].size(); 445 | rowsol.resize(n_rows); 446 | colsol.resize(n_cols); 447 | 448 | int n = 0; 449 | if (n_rows == n_cols) 450 | { 451 | n = n_rows; 452 | } 453 | else 454 | { 455 | if (!extend_cost) 456 | { 457 | throw std::runtime_error("The `extend_cost` variable should set True"); 458 | } 459 | } 460 | 461 | if (extend_cost || cost_limit < std::numeric_limits::max()) 462 | { 463 | n = n_rows + n_cols; 464 | cost_c_extended.resize(n); 465 | for (size_t i = 0; i < cost_c_extended.size(); i++) 466 | cost_c_extended[i].resize(n); 467 | 468 | if (cost_limit < std::numeric_limits::max()) 469 | { 470 | for (size_t i = 0; i < cost_c_extended.size(); i++) 471 | { 472 | for (size_t j = 0; j < cost_c_extended[i].size(); j++) 473 | { 474 | cost_c_extended[i][j] = cost_limit / 2.0; 475 | } 476 | } 477 | } 478 | else 479 | { 480 | float cost_max = -1; 481 | for (size_t i = 0; i < cost_c.size(); i++) 482 | { 483 | for (size_t j = 0; j < cost_c[i].size(); j++) 484 | { 485 | if (cost_c[i][j] > cost_max) 486 | cost_max = cost_c[i][j]; 487 | } 488 | } 489 | for (size_t i = 0; i < cost_c_extended.size(); i++) 490 | { 491 | for (size_t j = 0; j < cost_c_extended[i].size(); j++) 492 | { 493 | cost_c_extended[i][j] = cost_max + 1; 494 | } 495 | } 496 | } 497 | 498 | for (size_t i = n_rows; i < cost_c_extended.size(); i++) 499 | { 500 | for (size_t j = n_cols; j < cost_c_extended[i].size(); j++) 501 | { 502 | cost_c_extended[i][j] = 0; 503 | } 504 | } 505 | for (int i = 0; i < n_rows; i++) 506 | { 507 | for (int j = 0; j < n_cols; j++) 508 | { 509 | cost_c_extended[i][j] = cost_c[i][j]; 510 | } 511 | } 512 | 513 | cost_c.clear(); 514 | cost_c.assign(cost_c_extended.begin(), cost_c_extended.end()); 515 | } 516 | 517 | double **cost_ptr; 518 | cost_ptr = new double *[sizeof(double *) * n]; 519 | for (int i = 0; i < n; i++) 520 | cost_ptr[i] = new double[sizeof(double) * n]; 521 | 522 | for (int i = 0; i < n; i++) 523 | { 524 | for (int j = 0; j < n; j++) 525 | { 526 | cost_ptr[i][j] = cost_c[i][j]; 527 | } 528 | } 529 | 530 | int* x_c = new int[sizeof(int) * n]; 531 | int *y_c = new int[sizeof(int) * n]; 532 | 533 | int ret = lapjv_internal(n, cost_ptr, x_c, y_c); 534 | if (ret != 0) 535 | { 536 | throw std::runtime_error("The result of lapjv_internal() is invalid."); 537 | } 538 | 539 | double opt = 0.0; 540 | 541 | if (n != n_rows) 542 | { 543 | for (int i = 0; i < n; i++) 544 | { 545 | if (x_c[i] >= n_cols) 546 | x_c[i] = -1; 547 | if (y_c[i] >= n_rows) 548 | y_c[i] = -1; 549 | } 550 | for (int i = 0; i < n_rows; i++) 551 | { 552 | rowsol[i] = x_c[i]; 553 | } 554 | for (int i = 0; i < n_cols; i++) 555 | { 556 | colsol[i] = y_c[i]; 557 | } 558 | 559 | if (return_cost) 560 | { 561 | for (size_t i = 0; i < rowsol.size(); i++) 562 | { 563 | if (rowsol[i] != -1) 564 | { 565 | opt += cost_ptr[i][rowsol[i]]; 566 | } 567 | } 568 | } 569 | } 570 | else if (return_cost) 571 | { 572 | for (size_t i = 0; i < rowsol.size(); i++) 573 | { 574 | opt += cost_ptr[i][rowsol[i]]; 575 | } 576 | } 577 | 578 | for (int i = 0; i < n; i++) 579 | { 580 | delete[]cost_ptr[i]; 581 | } 582 | delete[]cost_ptr; 583 | delete[]x_c; 584 | delete[]y_c; 585 | 586 | return opt; 587 | } 588 | -------------------------------------------------------------------------------- /src/KalmanFilter.cpp: -------------------------------------------------------------------------------- 1 | #include "ByteTrack/KalmanFilter.h" 2 | 3 | #include 4 | 5 | byte_track::KalmanFilter::KalmanFilter(const float& std_weight_position, 6 | const float& std_weight_velocity) : 7 | std_weight_position_(std_weight_position), 8 | std_weight_velocity_(std_weight_velocity) 9 | { 10 | constexpr size_t ndim = 4; 11 | constexpr float dt = 1; 12 | 13 | motion_mat_ = Eigen::MatrixXf::Identity(8, 8); 14 | update_mat_ = Eigen::MatrixXf::Identity(4, 8); 15 | 16 | for (size_t i = 0; i < ndim; i++) 17 | { 18 | motion_mat_(i, ndim + i) = dt; 19 | } 20 | } 21 | 22 | void byte_track::KalmanFilter::initiate(StateMean &mean, StateCov &covariance, const DetectBox &measurement) 23 | { 24 | mean.block<1, 4>(0, 0) = measurement.block<1, 4>(0, 0); 25 | mean.block<1, 4>(0, 4) = Eigen::Vector4f::Zero(); 26 | 27 | StateMean std; 28 | std(0) = 2 * std_weight_position_ * measurement[3]; 29 | std(1) = 2 * std_weight_position_ * measurement[3]; 30 | std(2) = 1e-2; 31 | std(3) = 2 * std_weight_position_ * measurement[3]; 32 | std(4) = 10 * std_weight_velocity_ * measurement[3]; 33 | std(5) = 10 * std_weight_velocity_ * measurement[3]; 34 | std(6) = 1e-5; 35 | std(7) = 10 * std_weight_velocity_ * measurement[3]; 36 | 37 | StateMean tmp = std.array().square(); 38 | covariance = tmp.asDiagonal(); 39 | } 40 | 41 | void byte_track::KalmanFilter::predict(StateMean &mean, StateCov &covariance) 42 | { 43 | StateMean std; 44 | std(0) = std_weight_position_ * mean(3); 45 | std(1) = std_weight_position_ * mean(3); 46 | std(2) = 1e-2; 47 | std(3) = std_weight_position_ * mean(3); 48 | std(4) = std_weight_velocity_ * mean(3); 49 | std(5) = std_weight_velocity_ * mean(3); 50 | std(6) = 1e-5; 51 | std(7) = std_weight_velocity_ * mean(3); 52 | 53 | StateMean tmp = std.array().square(); 54 | StateCov motion_cov = tmp.asDiagonal(); 55 | 56 | mean = motion_mat_ * mean.transpose(); 57 | covariance = motion_mat_ * covariance * (motion_mat_.transpose()) + motion_cov; 58 | } 59 | 60 | void byte_track::KalmanFilter::update(StateMean &mean, StateCov &covariance, const DetectBox &measurement) 61 | { 62 | StateHMean projected_mean; 63 | StateHCov projected_cov; 64 | project(projected_mean, projected_cov, mean, covariance); 65 | 66 | Eigen::Matrix B = (covariance * (update_mat_.transpose())).transpose(); 67 | Eigen::Matrix kalman_gain = (projected_cov.llt().solve(B)).transpose(); 68 | Eigen::Matrix innovation = measurement - projected_mean; 69 | 70 | const auto tmp = innovation * (kalman_gain.transpose()); 71 | mean = (mean.array() + tmp.array()).matrix(); 72 | covariance = covariance - kalman_gain * projected_cov * (kalman_gain.transpose()); 73 | } 74 | 75 | void byte_track::KalmanFilter::project(StateHMean &projected_mean, StateHCov &projected_covariance, 76 | const StateMean& mean, const StateCov& covariance) 77 | { 78 | DetectBox std; 79 | std << std_weight_position_ * mean(3), 80 | std_weight_position_ * mean(3), 81 | 1e-1, 82 | std_weight_position_ * mean(3); 83 | 84 | projected_mean = update_mat_ * mean.transpose(); 85 | projected_covariance = update_mat_ * covariance * (update_mat_.transpose()); 86 | 87 | Eigen::Matrix diag = std.asDiagonal(); 88 | projected_covariance += diag.array().square().matrix(); 89 | } 90 | -------------------------------------------------------------------------------- /src/Object.cpp: -------------------------------------------------------------------------------- 1 | #include "ByteTrack/Object.h" 2 | 3 | byte_track::Object::Object(const Rect &_rect, 4 | const int &_label, 5 | const float &_prob) : rect(_rect), label(_label), prob(_prob) 6 | { 7 | } -------------------------------------------------------------------------------- /src/Rect.cpp: -------------------------------------------------------------------------------- 1 | #include "ByteTrack/Rect.h" 2 | 3 | #include 4 | 5 | template 6 | byte_track::Rect::Rect(const T &x, const T &y, const T &width, const T &height) : 7 | tlwh({x, y, width, height}) 8 | { 9 | } 10 | 11 | template 12 | byte_track::Rect::~Rect() 13 | { 14 | } 15 | 16 | template 17 | const T& byte_track::Rect::x() const 18 | { 19 | return tlwh[0]; 20 | } 21 | 22 | template 23 | const T& byte_track::Rect::y() const 24 | { 25 | return tlwh[1]; 26 | } 27 | 28 | template 29 | const T& byte_track::Rect::width() const 30 | { 31 | return tlwh[2]; 32 | } 33 | 34 | template 35 | const T& byte_track::Rect::height() const 36 | { 37 | return tlwh[3]; 38 | } 39 | 40 | template 41 | T& byte_track::Rect::x() 42 | { 43 | return tlwh[0]; 44 | } 45 | 46 | template 47 | T& byte_track::Rect::y() 48 | { 49 | return tlwh[1]; 50 | } 51 | 52 | template 53 | T& byte_track::Rect::width() 54 | { 55 | return tlwh[2]; 56 | } 57 | 58 | template 59 | T& byte_track::Rect::height() 60 | { 61 | return tlwh[3]; 62 | } 63 | 64 | template 65 | const T& byte_track::Rect::tl_x() const 66 | { 67 | return tlwh[0]; 68 | } 69 | 70 | template 71 | const T& byte_track::Rect::tl_y() const 72 | { 73 | return tlwh[1]; 74 | } 75 | 76 | template 77 | T byte_track::Rect::br_x() const 78 | { 79 | return tlwh[0] + tlwh[2]; 80 | } 81 | 82 | template 83 | T byte_track::Rect::br_y() const 84 | { 85 | return tlwh[1] + tlwh[3]; 86 | } 87 | 88 | template 89 | byte_track::Tlbr byte_track::Rect::getTlbr() const 90 | { 91 | return { 92 | tlwh[0], 93 | tlwh[1], 94 | tlwh[0] + tlwh[2], 95 | tlwh[1] + tlwh[3], 96 | }; 97 | } 98 | 99 | template 100 | byte_track::Xyah byte_track::Rect::getXyah() const 101 | { 102 | return { 103 | tlwh[0] + tlwh[2] / 2, 104 | tlwh[1] + tlwh[3] / 2, 105 | tlwh[2] / tlwh[3], 106 | tlwh[3], 107 | }; 108 | } 109 | 110 | template 111 | float byte_track::Rect::calcIoU(const Rect& other) const 112 | { 113 | const float box_area = (other.tlwh[2] + 1) * (other.tlwh[3] + 1); 114 | const float iw = std::min(tlwh[0] + tlwh[2], other.tlwh[0] + other.tlwh[2]) - std::max(tlwh[0], other.tlwh[0]) + 1; 115 | float iou = 0; 116 | if (iw > 0) 117 | { 118 | const float ih = std::min(tlwh[1] + tlwh[3], other.tlwh[1] + other.tlwh[3]) - std::max(tlwh[1], other.tlwh[1]) + 1; 119 | if (ih > 0) 120 | { 121 | const float ua = (tlwh[0] + tlwh[2] - tlwh[0] + 1) * (tlwh[1] + tlwh[3] - tlwh[1] + 1) + box_area - iw * ih; 122 | iou = iw * ih / ua; 123 | } 124 | } 125 | return iou; 126 | } 127 | 128 | template 129 | byte_track::Rect byte_track::generate_rect_by_tlbr(const byte_track::Tlbr& tlbr) 130 | { 131 | return byte_track::Rect(tlbr[0], tlbr[1], tlbr[2] - tlbr[0], tlbr[3] - tlbr[1]); 132 | } 133 | 134 | template 135 | byte_track::Rect byte_track::generate_rect_by_xyah(const byte_track::Xyah& xyah) 136 | { 137 | const auto width = xyah[2] * xyah[3]; 138 | return byte_track::Rect(xyah[0] - width / 2, xyah[1] - xyah[3] / 2, width, xyah[3]); 139 | } 140 | 141 | // explicit instantiation 142 | template class byte_track::Rect; 143 | template class byte_track::Rect; 144 | 145 | template byte_track::Rect byte_track::generate_rect_by_tlbr(const byte_track::Tlbr&); 146 | template byte_track::Rect byte_track::generate_rect_by_tlbr(const byte_track::Tlbr&); 147 | 148 | template byte_track::Rect byte_track::generate_rect_by_xyah(const byte_track::Xyah&); 149 | template byte_track::Rect byte_track::generate_rect_by_xyah(const byte_track::Xyah&); 150 | -------------------------------------------------------------------------------- /src/STrack.cpp: -------------------------------------------------------------------------------- 1 | #include "ByteTrack/STrack.h" 2 | 3 | #include 4 | 5 | byte_track::STrack::STrack(const Rect& rect, const float& score) : 6 | kalman_filter_(), 7 | mean_(), 8 | covariance_(), 9 | rect_(rect), 10 | state_(STrackState::New), 11 | is_activated_(false), 12 | score_(score), 13 | track_id_(0), 14 | frame_id_(0), 15 | start_frame_id_(0), 16 | tracklet_len_(0) 17 | { 18 | } 19 | 20 | byte_track::STrack::~STrack() 21 | { 22 | } 23 | 24 | const byte_track::Rect& byte_track::STrack::getRect() const 25 | { 26 | return rect_; 27 | } 28 | 29 | const byte_track::STrackState& byte_track::STrack::getSTrackState() const 30 | { 31 | return state_; 32 | } 33 | 34 | const bool& byte_track::STrack::isActivated() const 35 | { 36 | return is_activated_; 37 | } 38 | const float& byte_track::STrack::getScore() const 39 | { 40 | return score_; 41 | } 42 | 43 | const size_t& byte_track::STrack::getTrackId() const 44 | { 45 | return track_id_; 46 | } 47 | 48 | const size_t& byte_track::STrack::getFrameId() const 49 | { 50 | return frame_id_; 51 | } 52 | 53 | const size_t& byte_track::STrack::getStartFrameId() const 54 | { 55 | return start_frame_id_; 56 | } 57 | 58 | const size_t& byte_track::STrack::getTrackletLength() const 59 | { 60 | return tracklet_len_; 61 | } 62 | 63 | void byte_track::STrack::activate(const size_t& frame_id, const size_t& track_id) 64 | { 65 | kalman_filter_.initiate(mean_, covariance_, rect_.getXyah()); 66 | 67 | updateRect(); 68 | 69 | state_ = STrackState::Tracked; 70 | if (frame_id == 1) 71 | { 72 | is_activated_ = true; 73 | } 74 | track_id_ = track_id; 75 | frame_id_ = frame_id; 76 | start_frame_id_ = frame_id; 77 | tracklet_len_ = 0; 78 | } 79 | 80 | void byte_track::STrack::reActivate(const STrack &new_track, const size_t &frame_id, const int &new_track_id) 81 | { 82 | kalman_filter_.update(mean_, covariance_, new_track.getRect().getXyah()); 83 | 84 | updateRect(); 85 | 86 | state_ = STrackState::Tracked; 87 | is_activated_ = true; 88 | score_ = new_track.getScore(); 89 | if (0 <= new_track_id) 90 | { 91 | track_id_ = new_track_id; 92 | } 93 | frame_id_ = frame_id; 94 | tracklet_len_ = 0; 95 | } 96 | 97 | void byte_track::STrack::predict() 98 | { 99 | if (state_ != STrackState::Tracked) 100 | { 101 | mean_[7] = 0; 102 | } 103 | kalman_filter_.predict(mean_, covariance_); 104 | } 105 | 106 | void byte_track::STrack::update(const STrack &new_track, const size_t &frame_id) 107 | { 108 | kalman_filter_.update(mean_, covariance_, new_track.getRect().getXyah()); 109 | 110 | updateRect(); 111 | 112 | state_ = STrackState::Tracked; 113 | is_activated_ = true; 114 | score_ = new_track.getScore(); 115 | frame_id_ = frame_id; 116 | tracklet_len_++; 117 | } 118 | 119 | void byte_track::STrack::markAsLost() 120 | { 121 | state_ = STrackState::Lost; 122 | } 123 | 124 | void byte_track::STrack::markAsRemoved() 125 | { 126 | state_ = STrackState::Removed; 127 | } 128 | 129 | void byte_track::STrack::updateRect() 130 | { 131 | rect_.width() = mean_[2] * mean_[3]; 132 | rect_.height() = mean_[3]; 133 | rect_.x() = mean_[0] - rect_.width() / 2; 134 | rect_.y() = mean_[1] - rect_.height() / 2; 135 | } 136 | -------------------------------------------------------------------------------- /src/lapjv.cpp: -------------------------------------------------------------------------------- 1 | #include "ByteTrack/lapjv.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #define LAPJV_CPP_NEW(x, t, n) if ((x = (t *)malloc(sizeof(t) * (n))) == 0) { return -1; } 8 | #define LAPJV_CPP_FREE(x) if (x != 0) { free(x); x = 0; } 9 | #define LAPJV_CPP_SWAP_INDICES(a, b) { int _temp_index = a; a = b; b = _temp_index; } 10 | 11 | namespace 12 | { 13 | constexpr size_t LARGE = 1000000; 14 | 15 | enum class fp_t { 16 | FP_1 = 1, 17 | FP_2 = 2, 18 | FP_DYNAMIC = 3, 19 | }; 20 | 21 | /** Column-reduction and reduction transfer for a dense cost matrix. 22 | */ 23 | int _ccrrt_dense(const size_t n, double *cost[], 24 | int *free_rows, int *x, int *y, double *v) 25 | { 26 | int n_free_rows; 27 | bool *unique; 28 | 29 | for (size_t i = 0; i < n; i++) { 30 | x[i] = -1; 31 | v[i] = LARGE; 32 | y[i] = 0; 33 | } 34 | for (size_t i = 0; i < n; i++) { 35 | for (size_t j = 0; j < n; j++) { 36 | const double c = cost[i][j]; 37 | if (c < v[j]) { 38 | v[j] = c; 39 | y[j] = i; 40 | } 41 | } 42 | } 43 | LAPJV_CPP_NEW(unique, bool, n); 44 | memset(unique, true, n); 45 | { 46 | int j = n; 47 | do { 48 | j--; 49 | const int i = y[j]; 50 | if (x[i] < 0) { 51 | x[i] = j; 52 | } 53 | else { 54 | unique[i] = false; 55 | y[j] = -1; 56 | } 57 | } while (j > 0); 58 | } 59 | n_free_rows = 0; 60 | for (size_t i = 0; i < n; i++) { 61 | if (x[i] < 0) { 62 | free_rows[n_free_rows++] = i; 63 | } 64 | else if (unique[i]) { 65 | const int j = x[i]; 66 | double min = LARGE; 67 | for (size_t j2 = 0; j2 < n; j2++) { 68 | if (j2 == (size_t)j) { 69 | continue; 70 | } 71 | const double c = cost[i][j2] - v[j2]; 72 | if (c < min) { 73 | min = c; 74 | } 75 | } 76 | v[j] -= min; 77 | } 78 | } 79 | LAPJV_CPP_FREE(unique); 80 | return n_free_rows; 81 | } 82 | 83 | 84 | /** Augmenting row reduction for a dense cost matrix. 85 | */ 86 | int _carr_dense( 87 | const size_t n, double *cost[], 88 | const size_t n_free_rows, 89 | int *free_rows, int *x, int *y, double *v) 90 | { 91 | size_t current = 0; 92 | int new_free_rows = 0; 93 | size_t rr_cnt = 0; 94 | while (current < n_free_rows) { 95 | int i0; 96 | int j1, j2; 97 | double v1, v2, v1_new; 98 | bool v1_lowers; 99 | 100 | rr_cnt++; 101 | const int free_i = free_rows[current++]; 102 | j1 = 0; 103 | v1 = cost[free_i][0] - v[0]; 104 | j2 = -1; 105 | v2 = LARGE; 106 | for (size_t j = 1; j < n; j++) { 107 | const double c = cost[free_i][j] - v[j]; 108 | if (c < v2) { 109 | if (c >= v1) { 110 | v2 = c; 111 | j2 = j; 112 | } 113 | else { 114 | v2 = v1; 115 | v1 = c; 116 | j2 = j1; 117 | j1 = j; 118 | } 119 | } 120 | } 121 | i0 = y[j1]; 122 | v1_new = v[j1] - (v2 - v1); 123 | v1_lowers = v1_new < v[j1]; 124 | if (rr_cnt < current * n) { 125 | if (v1_lowers) { 126 | v[j1] = v1_new; 127 | } 128 | else if (i0 >= 0 && j2 >= 0) { 129 | j1 = j2; 130 | i0 = y[j2]; 131 | } 132 | if (i0 >= 0) { 133 | if (v1_lowers) { 134 | free_rows[--current] = i0; 135 | } 136 | else { 137 | free_rows[new_free_rows++] = i0; 138 | } 139 | } 140 | } 141 | else { 142 | if (i0 >= 0) { 143 | free_rows[new_free_rows++] = i0; 144 | } 145 | } 146 | x[free_i] = j1; 147 | y[j1] = free_i; 148 | } 149 | return new_free_rows; 150 | } 151 | 152 | 153 | /** Find columns with minimum d[j] and put them on the SCAN list. 154 | */ 155 | size_t _find_dense(const size_t n, size_t lo, double *d, int *cols, int *y) 156 | { 157 | size_t hi = lo + 1; 158 | double mind = d[cols[lo]]; 159 | for (size_t k = hi; k < n; k++) { 160 | int j = cols[k]; 161 | if (d[j] <= mind) { 162 | if (d[j] < mind) { 163 | hi = lo; 164 | mind = d[j]; 165 | } 166 | cols[k] = cols[hi]; 167 | cols[hi++] = j; 168 | } 169 | } 170 | return hi; 171 | } 172 | 173 | 174 | // Scan all columns in TODO starting from arbitrary column in SCAN 175 | // and try to decrease d of the TODO columns using the SCAN column. 176 | int _scan_dense(const size_t n, double *cost[], 177 | size_t *plo, size_t*phi, 178 | double *d, int *cols, int *pred, 179 | int *y, double *v) 180 | { 181 | size_t lo = *plo; 182 | size_t hi = *phi; 183 | double h, cred_ij; 184 | 185 | while (lo != hi) { 186 | int j = cols[lo++]; 187 | const int i = y[j]; 188 | const double mind = d[j]; 189 | h = cost[i][j] - v[j] - mind; 190 | // For all columns in TODO 191 | for (size_t k = hi; k < n; k++) { 192 | j = cols[k]; 193 | cred_ij = cost[i][j] - v[j] - h; 194 | if (cred_ij < d[j]) { 195 | d[j] = cred_ij; 196 | pred[j] = i; 197 | if (cred_ij == mind) { 198 | if (y[j] < 0) { 199 | return j; 200 | } 201 | cols[k] = cols[hi]; 202 | cols[hi++] = j; 203 | } 204 | } 205 | } 206 | } 207 | *plo = lo; 208 | *phi = hi; 209 | return -1; 210 | } 211 | 212 | 213 | /** Single iteration of modified Dijkstra shortest path algorithm as explained in the JV paper. 214 | * 215 | * This is a dense matrix version. 216 | * 217 | * \return The closest free column index. 218 | */ 219 | int find_path_dense( 220 | const size_t n, double *cost[], 221 | const int start_i, 222 | int *y, double *v, 223 | int *pred) 224 | { 225 | size_t lo = 0, hi = 0; 226 | int final_j = -1; 227 | size_t n_ready = 0; 228 | int *cols; 229 | double *d; 230 | 231 | LAPJV_CPP_NEW(cols, int, n); 232 | LAPJV_CPP_NEW(d, double, n); 233 | 234 | for (size_t i = 0; i < n; i++) { 235 | cols[i] = i; 236 | pred[i] = start_i; 237 | d[i] = cost[start_i][i] - v[i]; 238 | } 239 | 240 | while (final_j == -1) { 241 | // No columns left on the SCAN list. 242 | if (lo == hi) { 243 | n_ready = lo; 244 | hi = _find_dense(n, lo, d, cols, y); 245 | for (size_t k = lo; k < hi; k++) { 246 | const int j = cols[k]; 247 | if (y[j] < 0) { 248 | final_j = j; 249 | } 250 | } 251 | } 252 | if (final_j == -1) { 253 | final_j = _scan_dense( 254 | n, cost, &lo, &hi, d, cols, pred, y, v); 255 | } 256 | } 257 | 258 | { 259 | const double mind = d[cols[lo]]; 260 | for (size_t k = 0; k < n_ready; k++) { 261 | const int j = cols[k]; 262 | v[j] += d[j] - mind; 263 | } 264 | } 265 | 266 | LAPJV_CPP_FREE(cols); 267 | LAPJV_CPP_FREE(d); 268 | 269 | return final_j; 270 | } 271 | 272 | 273 | /** Augment for a dense cost matrix. 274 | */ 275 | int _ca_dense( 276 | const size_t n, double *cost[], 277 | const size_t n_free_rows, 278 | int *free_rows, int *x, int *y, double *v) 279 | { 280 | int *pred; 281 | 282 | LAPJV_CPP_NEW(pred, int, n); 283 | 284 | for (int *pfree_i = free_rows; pfree_i < free_rows + n_free_rows; pfree_i++) { 285 | int i = -1, j; 286 | size_t k = 0; 287 | 288 | j = find_path_dense(n, cost, *pfree_i, y, v, pred); 289 | if (j < 0) 290 | { 291 | throw std::runtime_error("Error occured in _ca_dense(): j < 0"); 292 | } 293 | if (j >= static_cast(n)) 294 | { 295 | throw std::runtime_error("Error occured in _ca_dense(): j >= n"); 296 | } 297 | while (i != *pfree_i) { 298 | i = pred[j]; 299 | y[j] = i; 300 | LAPJV_CPP_SWAP_INDICES(j, x[i]); 301 | k++; 302 | if (k >= n) { 303 | throw std::runtime_error("Error occured in _ca_dense(): k >= n"); 304 | } 305 | } 306 | } 307 | LAPJV_CPP_FREE(pred); 308 | return 0; 309 | } 310 | } 311 | 312 | /** Solve dense sparse LAP. */ 313 | int byte_track::lapjv_internal( 314 | const size_t n, double *cost[], 315 | int *x, int *y) 316 | { 317 | int ret; 318 | int *free_rows; 319 | double *v; 320 | 321 | LAPJV_CPP_NEW(free_rows, int, n); 322 | LAPJV_CPP_NEW(v, double, n); 323 | ret = _ccrrt_dense(n, cost, free_rows, x, y, v); 324 | int i = 0; 325 | while (ret > 0 && i < 2) { 326 | ret = _carr_dense(n, cost, ret, free_rows, x, y, v); 327 | i++; 328 | } 329 | if (ret > 0) { 330 | ret = _ca_dense(n, cost, ret, free_rows, x, y, v); 331 | } 332 | LAPJV_CPP_FREE(v); 333 | LAPJV_CPP_FREE(free_rows); 334 | return ret; 335 | } 336 | 337 | #undef LAPJV_CPP_NEW 338 | #undef LAPJV_CPP_FREE 339 | #undef LAPJV_CPP_SWAP_INDICES 340 | -------------------------------------------------------------------------------- /test/test_BYTETracker.cpp: -------------------------------------------------------------------------------- 1 | #include "ByteTrack/BYTETracker.h" 2 | 3 | #include "gtest/gtest.h" 4 | 5 | #include "boost/property_tree/ptree.hpp" 6 | #include "boost/property_tree/json_parser.hpp" 7 | #include "boost/foreach.hpp" 8 | #include "boost/optional.hpp" 9 | 10 | #include 11 | 12 | namespace 13 | { 14 | constexpr double EPS = 1e-2; 15 | 16 | const std::string D_RESULTS_FILE = "detection_results.json"; 17 | const std::string T_RESULTS_FILE = "tracking_results.json"; 18 | 19 | // key: track_id, value: rect of tracking object 20 | using BYTETrackerOut = std::map>; 21 | 22 | template 23 | T get_data(const boost::property_tree::ptree &pt, const std::string &key) 24 | { 25 | T ret; 26 | if (boost::optional data = pt.get_optional(key)) 27 | { 28 | ret = data.get(); 29 | } 30 | else 31 | { 32 | throw std::runtime_error("Could not read the data from ptree: [key: " + key + "]"); 33 | } 34 | return ret; 35 | } 36 | 37 | std::map> get_inputs_ref(const boost::property_tree::ptree &pt) 38 | { 39 | std::map> inputs_ref; 40 | BOOST_FOREACH (const boost::property_tree::ptree::value_type &child, pt.get_child("results")) 41 | { 42 | const boost::property_tree::ptree &result = child.second; 43 | const auto frame_id = get_data(result, "frame_id"); 44 | const auto prob = get_data(result, "prob"); 45 | const auto x = get_data(result, "x"); 46 | const auto y = get_data(result, "y"); 47 | const auto width = get_data(result, "width"); 48 | const auto height = get_data(result, "height"); 49 | 50 | decltype(inputs_ref)::iterator itr = inputs_ref.find(frame_id); 51 | if (itr != inputs_ref.end()) 52 | { 53 | itr->second.emplace_back(byte_track::Rect(x, y, width, height), 0, prob); 54 | } 55 | else 56 | { 57 | std::vector v(1, {byte_track::Rect(x, y, width, height), 0, prob}); 58 | inputs_ref.emplace_hint(inputs_ref.end(), frame_id, v); 59 | } 60 | } 61 | return inputs_ref; 62 | } 63 | 64 | std::map get_outputs_ref(const boost::property_tree::ptree &pt) 65 | { 66 | std::map outputs_ref; 67 | BOOST_FOREACH (const boost::property_tree::ptree::value_type &child, pt.get_child("results")) 68 | { 69 | const boost::property_tree::ptree &result = child.second; 70 | const auto frame_id = get_data(result, "frame_id"); 71 | const auto track_id = get_data(result, "track_id"); 72 | const auto x = get_data(result, "x"); 73 | const auto y = get_data(result, "y"); 74 | const auto width = get_data(result, "width"); 75 | const auto height = get_data(result, "height"); 76 | 77 | decltype(outputs_ref)::iterator itr = outputs_ref.find(frame_id); 78 | if (itr != outputs_ref.end()) 79 | { 80 | itr->second.emplace(track_id, byte_track::Rect(x, y, width, height)); 81 | } 82 | else 83 | { 84 | BYTETrackerOut v{ 85 | {track_id, byte_track::Rect(x, y, width, height)}, 86 | }; 87 | outputs_ref.emplace_hint(outputs_ref.end(), frame_id, v); 88 | } 89 | } 90 | return outputs_ref; 91 | } 92 | } 93 | 94 | TEST(ByteTrack, BYTETracker) 95 | { 96 | boost::property_tree::ptree pt_d_results; 97 | boost::property_tree::read_json(D_RESULTS_FILE, pt_d_results); 98 | 99 | boost::property_tree::ptree pt_t_results; 100 | boost::property_tree::read_json(T_RESULTS_FILE, pt_t_results); 101 | 102 | try 103 | { 104 | // Get infomation of reference data 105 | const auto detection_results_name = get_data(pt_d_results, "name"); 106 | const auto tracking_results_name = get_data(pt_t_results, "name"); 107 | const auto fps = get_data(pt_d_results, "fps"); 108 | const auto track_buffer = get_data(pt_d_results, "track_buffer"); 109 | 110 | if (detection_results_name != tracking_results_name) 111 | { 112 | throw std::runtime_error("The name of the tests are different: [detection_results_name: " + detection_results_name + 113 | ", tracking_results_name: " + tracking_results_name + "]"); 114 | } 115 | 116 | // Get input reference data from D_RESULTS_FILE 117 | const auto inputs_ref = get_inputs_ref(pt_d_results); 118 | 119 | // Get output reference data from T_RESULTS_FILE 120 | auto outputs_ref = get_outputs_ref(pt_t_results); 121 | 122 | // Test BYTETracker::update() 123 | byte_track::BYTETracker tracker(fps, track_buffer); 124 | for (const auto &[frame_id, objects] : inputs_ref) 125 | { 126 | const auto outputs = tracker.update(objects); 127 | 128 | // Verify between the reference data and the output of the BYTETracker impl 129 | EXPECT_EQ(outputs.size(), outputs_ref[frame_id].size()); 130 | for (const auto &outputs_per_frame : outputs) 131 | { 132 | const auto &rect = outputs_per_frame->getRect(); 133 | const auto &track_id = outputs_per_frame->getTrackId(); 134 | const auto &ref = outputs_ref[frame_id][track_id]; 135 | EXPECT_NEAR(ref.x(), rect.x(), EPS); 136 | EXPECT_NEAR(ref.y(), rect.y(), EPS); 137 | EXPECT_NEAR(ref.width(), rect.width(), EPS); 138 | EXPECT_NEAR(ref.height(), rect.height(), EPS); 139 | } 140 | } 141 | } 142 | catch (const std::exception &e) 143 | { 144 | FAIL() << e.what(); 145 | } 146 | } 147 | 148 | int main(int argc, char **argv) 149 | { 150 | testing::InitGoogleTest(&argc, argv); 151 | return (RUN_ALL_TESTS()); 152 | } 153 | --------------------------------------------------------------------------------