├── CMakeLists.txt ├── Model_Convert ├── deep_sort.yaml └── exportOnnx.py ├── README.md ├── demo.cpp ├── include ├── datatype.h ├── deepsort.h ├── featuretensor.h ├── hungarianoper.h ├── kalmanfilter.h ├── linear_assignment.h ├── matrix.h ├── model.hpp ├── munkres.h ├── nn_matching.h ├── track.h └── tracker.h ├── resources └── track.txt └── src ├── deepsort.cpp ├── featuretensor.cpp ├── hungarianoper.cpp ├── kalmanfilter.cpp ├── linear_assignment.cpp ├── munkres.cpp ├── nn_matching.cpp ├── track.cpp └── tracker.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | project(deepsort) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 5 | set(CMAKE_BUILD_TYPE Release) 6 | 7 | find_package(CUDA REQUIRED) 8 | find_package(OpenCV REQUIRED) 9 | 10 | 11 | include_directories( 12 | ${CUDA_INCLUDE_DIRS} 13 | ${OpenCV_INCLUDE_DIRS} 14 | ${PROJECT_SOURCE_DIR}/include 15 | ) 16 | link_directories( 17 | /usr/local/cuda/lib64 18 | ) 19 | aux_source_directory(${PROJECT_SOURCE_DIR}/src SRC_DIR) 20 | 21 | # ===== deepsort ===== 22 | add_library(deepsort SHARED ${SRC_DIR}) 23 | target_link_libraries(deepsort 24 | ${CUDA_LIBS} ${OpenCV_LIBS} 25 | ) 26 | 27 | 28 | # ===== demo ===== 29 | add_executable(demo ${PROJECT_SOURCE_DIR}/demo.cpp) 30 | target_link_libraries(demo 31 | ${CUDA_LIBS} ${OpenCV_LIBS} 32 | deepsort 33 | ) 34 | 35 | 36 | -------------------------------------------------------------------------------- /Model_Convert/deep_sort.yaml: -------------------------------------------------------------------------------- 1 | DEEPSORT: 2 | REID_CKPT: "ckpt.t7" 3 | MAX_DIST: 0.2 4 | MIN_CONFIDENCE: 0.3 5 | NMS_MAX_OVERLAP: 0.5 6 | MAX_IOU_DISTANCE: 0.7 7 | MAX_AGE: 70 8 | N_INIT: 3 9 | NN_BUDGET: 100 10 | -------------------------------------------------------------------------------- /Model_Convert/exportOnnx.py: -------------------------------------------------------------------------------- 1 | 2 | import os 3 | import cv2 4 | import time 5 | import argparse 6 | import torch 7 | import numpy as np 8 | 9 | from deep_sort import build_tracker 10 | from utils.draw import draw_boxes 11 | from utils.parser import get_config 12 | 13 | from tqdm import tqdm 14 | 15 | if __name__ == '__main__': 16 | parser = argparse.ArgumentParser() 17 | parser.add_argument("--config_deepsort", type=str, default="./configs/deep_sort.yaml", help='Configure tracker') 18 | parser.add_argument("--cpu", dest="use_cuda", action="store_false", default=True, help='Run in CPU') 19 | args = parser.parse_args() 20 | 21 | cfg = get_config() 22 | cfg.merge_from_file(args.config_deepsort) 23 | use_cuda = args.use_cuda and torch.cuda.is_available() 24 | torch.set_grad_enabled(False) 25 | model = build_tracker(cfg, use_cuda=False) 26 | 27 | model.reid = True 28 | model.extractor.net.eval() 29 | 30 | device = 'cuda' 31 | output_onnx = 'deepsort.onnx' 32 | # ------------------------ export ----------------------------- 33 | print("==> Exporting model to ONNX format at '{}'".format(output_onnx)) 34 | input_names = ['input'] 35 | output_names = ['output'] 36 | 37 | input_tensor = torch.randn(32, 3, 128, 64, device=device) 38 | 39 | torch.onnx.export(model.extractor.net.cuda(), input_tensor, output_onnx, export_params=True, verbose=False, 40 | input_names=input_names, output_names=output_names, opset_version=10, 41 | do_constant_folding=True) 42 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DeepSort CPP Only OpenCV_DNN 2 | 3 | A refactoring version of [DeepSort_TensorRT](https://github.com/GesilaA/deepsort_tensorrt) 4 | 5 | Remove TensorRT from the repository and inference using OpenCV DNN 6 | 7 | # Requirements 8 | 9 | OpenCV>=4.2.0 With CUDA(Tested by 4.5.1) 10 | 11 | Eigen(Tested by 3.3.9) 12 | 13 | # Install 14 | 15 | It was tested based on Visual Studio 2019, but I think it will work well enough with Cmake. 16 | 17 | ~~~ 18 | mkdir build 19 | cd build 20 | cmake .. 21 | make 22 | ~~~ 23 | 24 | # PreTrained Model 25 | reference [DeepSort_PyTorch](https://github.com/ZQPei/deep_sort_pytorch) 26 | 27 | I converted the model(ckpt.t7) downloaded from Github above to Onnx 28 | 29 | [Google Drive Link(deepsort.onnx)](https://drive.google.com/file/d/1eTKZBaSilFZV2z6SjkJzQMrCg7wnaXco/view?usp=sharing) 30 | 31 | # ToDO 32 | - [ ] Detect Model Add 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /demo.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "deepsort.h" 6 | #include 7 | 8 | using std::vector; 9 | 10 | void showDetection(cv::Mat& img, std::vector& boxes) { 11 | cv::Mat temp = img.clone(); 12 | for (auto box : boxes) { 13 | cv::Point lt(box.x1, box.y1); 14 | cv::Point br(box.x2, box.y2); 15 | cv::rectangle(temp, lt, br, cv::Scalar(255, 0, 0), 1); 16 | std::string lbl = cv::format("ID:%d_C:%d_CONF:%.2f", (int)box.trackID, (int)box.classID, box.confidence); 17 | cv::putText(temp, lbl, lt, cv::FONT_HERSHEY_COMPLEX, 0.8, cv::Scalar(0,255,0)); 18 | } 19 | cv::imshow("img", temp); 20 | cv::waitKey(1); 21 | } 22 | 23 | class Tester { 24 | public: 25 | Tester(string modelPath) { 26 | allDetections.clear(); 27 | out.clear(); 28 | DS = new DeepSort(modelPath, 128, 256, 0); 29 | } 30 | ~Tester() { 31 | } 32 | 33 | public: 34 | 35 | void split(const std::string& s, vector& token, char delim=' ') { 36 | token.clear(); 37 | auto string_find_first_not = [s, delim](size_t pos = 0) -> size_t { 38 | for (size_t i = pos; i < s.size(); ++i) 39 | if (s[i] != delim) return i; 40 | return string::npos; 41 | }; 42 | size_t lastPos = string_find_first_not(0); 43 | size_t pos = s.find(delim, lastPos); 44 | while (lastPos != string::npos) { 45 | token.emplace_back(s.substr(lastPos, pos-lastPos)); 46 | lastPos = string_find_first_not(pos); 47 | pos = s.find(delim, lastPos); 48 | } 49 | } 50 | 51 | void loadDetections(std::string txtPath) { 52 | //fstream f(filePath, ios::in); 53 | this->txtPath = txtPath; 54 | ifstream inFile; 55 | inFile.open(txtPath, ios::binary); 56 | std::string temp; 57 | vector token; 58 | while (getline(inFile, temp)) { 59 | // std::cout << temp << std::endl; 60 | split(temp, token, ' '); 61 | int frame = atoi(token[0].c_str()); 62 | int c = atoi(token[1].c_str()); 63 | int x = atoi(token[2].c_str()); 64 | int y = atoi(token[3].c_str()); 65 | int w = atoi(token[4].c_str()); 66 | int h = atoi(token[5].c_str()); 67 | float con= atof(token[6].c_str()); 68 | while (allDetections.size() <= frame) { 69 | vector t; 70 | allDetections.push_back(t); 71 | } 72 | DetectBox dd(x-w/2, y-h/2, x+w/2, y+h/2, con, c); 73 | allDetections[frame].push_back(dd); 74 | } 75 | allDetections.pop_back(); 76 | } 77 | 78 | void run() { 79 | cv::namedWindow("DeepSortTest"); 80 | int i = 1; 81 | cv::Mat whiteBoard(1080, 1920, CV_8UC3, cv::Scalar::all(0)); 82 | for (vector d : allDetections) { 83 | cv::Mat img_rgb; 84 | cv::cvtColor(whiteBoard, img_rgb, cv::COLOR_BGR2RGB); 85 | 86 | DS->sort(img_rgb, d); 87 | 88 | showDetection(whiteBoard, d); 89 | } 90 | cv::destroyAllWindows(); 91 | } 92 | 93 | private: 94 | vector> allDetections; 95 | vector out; 96 | std::string txtPath; 97 | DeepSort* DS; 98 | }; 99 | 100 | int main(int argc, char** argv) { 101 | if (argc < 3) { 102 | std::cout << "./demo [input model path] [input txt path]" << std::endl; 103 | return -1; 104 | } 105 | Tester* test = new Tester(argv[1]); 106 | test->loadDetections(argv[2]); 107 | test->run(); 108 | } 109 | -------------------------------------------------------------------------------- /include/datatype.h: -------------------------------------------------------------------------------- 1 | #ifndef DATATYPE_H 2 | #define DATATYPE_H 3 | #define Feature_Vector_Dim 256 4 | typedef struct DetectBox { 5 | DetectBox(float x1=0, float y1=0, float x2=0, float y2=0, 6 | float confidence=0, float classID=-1, float trackID=-1) { 7 | this->x1 = x1; 8 | this->y1 = y1; 9 | this->x2 = x2; 10 | this->y2 = y2; 11 | this->confidence = confidence; 12 | this->classID = classID; 13 | this->trackID = trackID; 14 | } 15 | float x1, y1, x2, y2; 16 | float confidence; 17 | float classID; 18 | float trackID; 19 | } DetectBox; 20 | 21 | #endif // DATATYPE_H 22 | 23 | #ifndef DEEPSORTDATATYPE_H 24 | #define DEEPSORTDATATYPE_H 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | typedef struct CLSCONF { 31 | CLSCONF() { 32 | this->cls = -1; 33 | this->conf = -1; 34 | } 35 | CLSCONF(int cls, float conf) { 36 | this->cls = cls; 37 | this->conf = conf; 38 | } 39 | int cls; 40 | float conf; 41 | } CLSCONF; 42 | 43 | typedef Eigen::Matrix DETECTBOX; 44 | typedef Eigen::Matrix DETECTBOXSS; 45 | typedef Eigen::Matrix FEATURE; 46 | typedef Eigen::Matrix FEATURESS; 47 | //typedef std::vector FEATURESS; 48 | 49 | //Kalmanfilter 50 | //typedef Eigen::Matrix KAL_FILTER; 51 | typedef Eigen::Matrix KAL_MEAN; 52 | typedef Eigen::Matrix KAL_COVA; 53 | typedef Eigen::Matrix KAL_HMEAN; 54 | typedef Eigen::Matrix KAL_HCOVA; 55 | using KAL_DATA = std::pair; 56 | using KAL_HDATA = std::pair; 57 | 58 | //main 59 | using RESULT_DATA = std::pair; 60 | 61 | //tracker: 62 | using TRACKER_DATA = std::pair; 63 | using MATCH_DATA = std::pair; 64 | typedef struct t{ 65 | std::vector matches; 66 | std::vector unmatched_tracks; 67 | std::vector unmatched_detections; 68 | }TRACHER_MATCHD; 69 | 70 | //linear_assignment: 71 | typedef Eigen::Matrix DYNAMICM; 72 | 73 | #endif //DEEPSORTDATATYPE_H -------------------------------------------------------------------------------- /include/deepsort.h: -------------------------------------------------------------------------------- 1 | //#ifndef DEEPSORT_H 2 | //#define DEEPSORT_H 3 | 4 | #ifdef _DLL_EXPORTS 5 | #define DLL_API _declspec(dllexport) 6 | #else 7 | #define DLL_API _declspec(dllimport) 8 | #endif 9 | 10 | #include 11 | #include 12 | #include "featuretensor.h" 13 | #include "tracker.h" 14 | #include "datatype.h" 15 | #include 16 | 17 | using std::vector; 18 | //using nvinfer1::ILogger; 19 | 20 | class DLL_API DeepSort { 21 | public: 22 | //DeepSort(std::string modelPath, int batchSize, int featureDim, int gpuID, ILogger* gLogger); 23 | DeepSort(std::string modelPath, int batchSize, int featureDim, int gpuID); 24 | 25 | ~DeepSort(); 26 | 27 | public: 28 | void sort(cv::Mat& frame, vector& dets); 29 | 30 | private: 31 | void sort(cv::Mat& frame, DETECTIONS& detections); 32 | void sort(cv::Mat& frame, DETECTIONSV2& detectionsv2); 33 | void sort(vector& dets); 34 | void sort(DETECTIONS& detections); 35 | void init(); 36 | 37 | private: 38 | std::string enginePath; 39 | int batchSize; 40 | int featureDim; 41 | cv::Size imgShape; 42 | float confThres; 43 | float nmsThres; 44 | int maxBudget; 45 | float maxCosineDist; 46 | 47 | private: 48 | vector result; 49 | vector> results; 50 | tracker* objTracker; 51 | FeatureTensor* featureExtractor; 52 | //ILogger* gLogger; 53 | int gpuID; 54 | }; 55 | 56 | //#endif //deepsort.h 57 | -------------------------------------------------------------------------------- /include/featuretensor.h: -------------------------------------------------------------------------------- 1 | #ifndef FEATURETENSOR_H 2 | #define FEATURETENSOR_H 3 | 4 | #include 5 | #include 6 | #include 7 | //#include 8 | //#include 9 | #include "model.hpp" 10 | #include "datatype.h" 11 | //#include "cuda_runtime_api.h" 12 | 13 | using std::vector; 14 | //using nvinfer1::ILogger; 15 | 16 | class FeatureTensor { 17 | public: 18 | //FeatureTensor(const int maxBatchSize, const cv::Size imgShape, const int featureDim, int gpuID, ILogger* gLogger); 19 | FeatureTensor(const int maxBatchSize, const cv::Size imgShape, const int featureDim, int gpuID); 20 | 21 | ~FeatureTensor(); 22 | 23 | public: 24 | bool getRectsFeature(const cv::Mat& img, DETECTIONS& det); 25 | bool getRectsFeature(DETECTIONS& det); 26 | void loadOnnx(std::string onnxPath); 27 | //int getResult(float*& buffer); 28 | cv::Mat doInference(vector& imgMats); 29 | 30 | private: 31 | cv::Mat doInference_run(vector imgMats); 32 | void stream2det(cv::Mat stream, DETECTIONS& det); 33 | 34 | private: 35 | //nvinfer1::IRuntime* runtime; 36 | //nvinfer1::ICudaEngine* engine; 37 | //nvinfer1::IExecutionContext* context; 38 | cv::dnn::Net dnn_engine; 39 | const int maxBatchSize; 40 | const cv::Size imgShape; 41 | const int featureDim; 42 | 43 | private: 44 | int curBatchSize; 45 | //const int inputStreamSize, outputStreamSize; 46 | bool initFlag; 47 | cv::Mat outputBuffer; 48 | //float* const inputBuffer; 49 | //float* const outputBuffer; 50 | int inputIndex, outputIndex; 51 | void* buffers[2]; 52 | //cudaStream_t cudaStream; 53 | // BGR format 54 | float means[3], std[3]; 55 | const std::string inputName, outputName; 56 | //ILogger* gLogger; 57 | }; 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /include/hungarianoper.h: -------------------------------------------------------------------------------- 1 | #ifndef HUNGARIANOPER_H 2 | #define HUNGARIANOPER_H 3 | 4 | #include "munkres.h" 5 | #include "datatype.h" 6 | 7 | 8 | class HungarianOper { 9 | public: 10 | static Eigen::Matrix Solve(const DYNAMICM &cost_matrix); 11 | }; 12 | 13 | #endif // HUNGARIANOPER_H 14 | -------------------------------------------------------------------------------- /include/kalmanfilter.h: -------------------------------------------------------------------------------- 1 | #ifndef KALMANFILTER_H 2 | #define KALMANFILTER_H 3 | 4 | #include "datatype.h" 5 | 6 | class KalmanFilter { 7 | public: 8 | static const double chi2inv95[10]; 9 | KalmanFilter(); 10 | KAL_DATA initiate(const DETECTBOX& measurement); 11 | void predict(KAL_MEAN& mean, KAL_COVA& covariance); 12 | KAL_HDATA project(const KAL_MEAN& mean, const KAL_COVA& covariance); 13 | KAL_DATA update(const KAL_MEAN& mean, 14 | const KAL_COVA& covariance, 15 | const DETECTBOX& measurement); 16 | 17 | Eigen::Matrix gating_distance( 18 | const KAL_MEAN& mean, 19 | const KAL_COVA& covariance, 20 | const std::vector& measurements, 21 | bool only_position = false); 22 | 23 | private: 24 | Eigen::Matrix _motion_mat; 25 | Eigen::Matrix _update_mat; 26 | float _std_weight_position; 27 | float _std_weight_velocity; 28 | }; 29 | 30 | #endif // KALMANFILTER_H 31 | -------------------------------------------------------------------------------- /include/linear_assignment.h: -------------------------------------------------------------------------------- 1 | #ifndef LINEAR_ASSIGNMENT_H 2 | #define LINEAR_ASSIGNMENT_H 3 | 4 | #include "datatype.h" 5 | #include "tracker.h" 6 | 7 | #define INFTY_COST 1e5 8 | class tracker; 9 | //for matching; 10 | class linear_assignment 11 | { 12 | linear_assignment(); 13 | linear_assignment(const linear_assignment& ); 14 | linear_assignment& operator=(const linear_assignment&); 15 | static linear_assignment* instance; 16 | 17 | public: 18 | static linear_assignment* getInstance(); 19 | TRACHER_MATCHD matching_cascade(tracker* distance_metric, 20 | tracker::GATED_METRIC_FUNC distance_metric_func, 21 | float max_distance, 22 | int cascade_depth, 23 | std::vector& tracks, 24 | const DETECTIONS& detections, 25 | std::vector &track_indices, 26 | std::vector detection_indices = std::vector()); 27 | TRACHER_MATCHD min_cost_matching( 28 | tracker* distance_metric, 29 | tracker::GATED_METRIC_FUNC distance_metric_func, 30 | float max_distance, 31 | std::vector& tracks, 32 | const DETECTIONS& detections, 33 | std::vector& track_indices, 34 | std::vector& detection_indices); 35 | DYNAMICM gate_cost_matrix( 36 | KalmanFilter* kf, 37 | DYNAMICM& cost_matrix, 38 | std::vector& tracks, 39 | const DETECTIONS& detections, 40 | const std::vector& track_indices, 41 | const std::vector& detection_indices, 42 | float gated_cost = INFTY_COST, 43 | bool only_position = false); 44 | }; 45 | 46 | #endif // LINEAR_ASSIGNMENT_H 47 | -------------------------------------------------------------------------------- /include/matrix.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2007 John Weaver 3 | * 4 | * This program is free software; you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation; either version 2 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 17 | */ 18 | 19 | #ifndef _MATRIX_H_ 20 | #define _MATRIX_H_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #define XYZMIN(x, y) (x)<(y)?(x):(y) 30 | #define XYZMAX(x, y) (x)>(y)?(x):(y) 31 | 32 | template 33 | class Matrix { 34 | public: 35 | Matrix(){ 36 | m_rows = 0; 37 | m_columns = 0; 38 | m_matrix = nullptr; 39 | } 40 | Matrix(const size_t rows, const size_t columns) { 41 | m_matrix = nullptr; 42 | resize(rows, columns); 43 | } 44 | Matrix(const std::initializer_list> init) { 45 | m_matrix = nullptr; 46 | m_rows = init.size(); 47 | if ( m_rows == 0 ) { 48 | m_columns = 0; 49 | } else { 50 | m_columns = init.begin()->size(); 51 | if ( m_columns > 0 ) { 52 | resize(m_rows, m_columns); 53 | } 54 | } 55 | 56 | size_t i = 0, j; 57 | for ( auto row = init.begin() ; row != init.end() ; ++row, ++i ) { 58 | assert ( row->size() == m_columns && "All rows must have the same number of columns." ); 59 | j = 0; 60 | for ( auto value = row->begin() ; value != row->end() ; ++value, ++j ) { 61 | m_matrix[i][j] = *value; 62 | } 63 | } 64 | } 65 | Matrix(const Matrix &other) { 66 | if ( other.m_matrix != nullptr ) { 67 | // copy arrays 68 | m_matrix = nullptr; 69 | resize(other.m_rows, other.m_columns); 70 | for ( size_t i = 0 ; i < m_rows ; i++ ) { 71 | for ( size_t j = 0 ; j < m_columns ; j++ ) { 72 | m_matrix[i][j] = other.m_matrix[i][j]; 73 | } 74 | } 75 | } else { 76 | m_matrix = nullptr; 77 | m_rows = 0; 78 | m_columns = 0; 79 | } 80 | } 81 | Matrix & operator= (const Matrix &other){ 82 | if ( other.m_matrix != nullptr ) { 83 | // copy arrays 84 | resize(other.m_rows, other.m_columns); 85 | for ( size_t i = 0 ; i < m_rows ; i++ ) { 86 | for ( size_t j = 0 ; j < m_columns ; j++ ) { 87 | m_matrix[i][j] = other.m_matrix[i][j]; 88 | } 89 | } 90 | } else { 91 | // free arrays 92 | for ( size_t i = 0 ; i < m_columns ; i++ ) { 93 | delete [] m_matrix[i]; 94 | } 95 | 96 | delete [] m_matrix; 97 | 98 | m_matrix = nullptr; 99 | m_rows = 0; 100 | m_columns = 0; 101 | } 102 | 103 | return *this; 104 | } 105 | ~Matrix(){ 106 | if ( m_matrix != nullptr ) { 107 | // free arrays 108 | for ( size_t i = 0 ; i < m_rows ; i++ ) { 109 | delete [] m_matrix[i]; 110 | } 111 | 112 | delete [] m_matrix; 113 | } 114 | m_matrix = nullptr; 115 | } 116 | // all operations modify the matrix in-place. 117 | void resize(const size_t rows, const size_t columns, const T default_value = 0) { 118 | assert ( rows > 0 && columns > 0 && "Columns and rows must exist." ); 119 | 120 | if ( m_matrix == nullptr ) { 121 | // alloc arrays 122 | m_matrix = new T*[rows]; // rows 123 | for ( size_t i = 0 ; i < rows ; i++ ) { 124 | m_matrix[i] = new T[columns]; // columns 125 | } 126 | 127 | m_rows = rows; 128 | m_columns = columns; 129 | clear(); 130 | } else { 131 | // save array pointer 132 | T **new_matrix; 133 | // alloc new arrays 134 | new_matrix = new T*[rows]; // rows 135 | for ( size_t i = 0 ; i < rows ; i++ ) { 136 | new_matrix[i] = new T[columns]; // columns 137 | for ( size_t j = 0 ; j < columns ; j++ ) { 138 | new_matrix[i][j] = default_value; 139 | } 140 | } 141 | 142 | // copy data from saved pointer to new arrays 143 | size_t minrows = XYZMIN(rows, m_rows); 144 | size_t mincols = XYZMIN(columns, m_columns); 145 | for ( size_t x = 0 ; x < minrows ; x++ ) { 146 | for ( size_t y = 0 ; y < mincols ; y++ ) { 147 | new_matrix[x][y] = m_matrix[x][y]; 148 | } 149 | } 150 | 151 | // delete old arrays 152 | if ( m_matrix != nullptr ) { 153 | for ( size_t i = 0 ; i < m_rows ; i++ ) { 154 | delete [] m_matrix[i]; 155 | } 156 | 157 | delete [] m_matrix; 158 | } 159 | 160 | m_matrix = new_matrix; 161 | } 162 | 163 | m_rows = rows; 164 | m_columns = columns; 165 | } 166 | void clear() { 167 | assert( m_matrix != nullptr ); 168 | 169 | for ( size_t i = 0 ; i < m_rows ; i++ ) { 170 | for ( size_t j = 0 ; j < m_columns ; j++ ) { 171 | m_matrix[i][j] = 0; 172 | } 173 | } 174 | } 175 | T& operator () (const size_t x, const size_t y) { 176 | assert ( x < m_rows ); 177 | assert ( y < m_columns ); 178 | assert ( m_matrix != nullptr ); 179 | return m_matrix[x][y]; 180 | } 181 | 182 | const T& operator () (const size_t x, const size_t y) const { 183 | assert ( x < m_rows ); 184 | assert ( y < m_columns ); 185 | assert ( m_matrix != nullptr ); 186 | return m_matrix[x][y]; 187 | } 188 | const T mmin() const { 189 | assert( m_matrix != nullptr ); 190 | assert ( m_rows > 0 ); 191 | assert ( m_columns > 0 ); 192 | T min = m_matrix[0][0]; 193 | 194 | for ( size_t i = 0 ; i < m_rows ; i++ ) { 195 | for ( size_t j = 0 ; j < m_columns ; j++ ) { 196 | min = std::min(min, m_matrix[i][j]); 197 | } 198 | } 199 | 200 | return min; 201 | } 202 | 203 | const T mmax() const { 204 | assert( m_matrix != nullptr ); 205 | assert ( m_rows > 0 ); 206 | assert ( m_columns > 0 ); 207 | T max = m_matrix[0][0]; 208 | 209 | for ( size_t i = 0 ; i < m_rows ; i++ ) { 210 | for ( size_t j = 0 ; j < m_columns ; j++ ) { 211 | max = std::max(max, m_matrix[i][j]); 212 | } 213 | } 214 | 215 | return max; 216 | } 217 | inline size_t minsize() { return ((m_rows < m_columns) ? m_rows : m_columns); } 218 | inline size_t columns() const { return m_columns;} 219 | inline size_t rows() const { return m_rows;} 220 | 221 | friend std::ostream& operator<<(std::ostream& os, const Matrix &matrix) 222 | { 223 | os << "Matrix:" << std::endl; 224 | for (size_t row = 0 ; row < matrix.rows() ; row++ ) 225 | { 226 | for (size_t col = 0 ; col < matrix.columns() ; col++ ) 227 | { 228 | os.width(8); 229 | os << matrix(row, col) << ","; 230 | } 231 | os << std::endl; 232 | } 233 | return os; 234 | } 235 | 236 | private: 237 | T **m_matrix; 238 | size_t m_rows; 239 | size_t m_columns; 240 | }; 241 | 242 | //#ifndef USE_EXPORT_KEYWORD 243 | //#include "matrix.cpp" 244 | ////#define export /*export*/ 245 | //#endif 246 | 247 | #endif /* !defined(_MATRIX_H_) */ 248 | 249 | -------------------------------------------------------------------------------- /include/model.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MODEL_HPP 2 | #define MODEL_HPP 3 | 4 | #include 5 | #include "datatype.h" 6 | 7 | 8 | // * Each rect's data structure. 9 | // * tlwh: topleft point & (w,h) 10 | // * confidence: detection confidence. 11 | // * feature: the rect's 256d feature. 12 | // */ 13 | 14 | const float kRatio=0.5; 15 | enum DETECTBOX_IDX {IDX_X = 0, IDX_Y, IDX_W, IDX_H }; 16 | 17 | class DETECTION_ROW { 18 | public: 19 | DETECTBOX tlwh; 20 | float confidence; 21 | FEATURE feature; 22 | DETECTBOX to_xyah() const { 23 | //(centerx, centery, ration, h) 24 | DETECTBOX ret = tlwh; 25 | ret(0, IDX_X) += (ret(0, IDX_W)*kRatio); 26 | ret(0, IDX_Y) += (ret(0, IDX_H)*kRatio); 27 | ret(0, IDX_W) /= ret(0, IDX_H); 28 | return ret; 29 | } 30 | DETECTBOX to_tlbr() const { 31 | //(x,y,xx,yy) 32 | DETECTBOX ret = tlwh; 33 | ret(0, IDX_X) += ret(0, IDX_W); 34 | ret(0, IDX_Y) += ret(0, IDX_H); 35 | return ret; 36 | } 37 | }; 38 | 39 | typedef std::vector DETECTIONS; 40 | typedef std::pair, DETECTIONS> DETECTIONSV2; 41 | 42 | #endif // MODEL_HPP 43 | -------------------------------------------------------------------------------- /include/munkres.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2007 John Weaver 3 | * Copyright (c) 2015 Miroslav Krajicek 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 2 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program; if not, write to the Free Software 17 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 18 | */ 19 | 20 | #if !defined(_MUNKRES_H_) 21 | #define _MUNKRES_H_ 22 | 23 | #include "matrix.h" 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | 32 | template class Munkres 33 | { 34 | static constexpr int NORMAL = 0; 35 | static constexpr int STAR = 1; 36 | static constexpr int PRIME = 2; 37 | public: 38 | 39 | /* 40 | * 41 | * Linear assignment problem solution 42 | * [modifies matrix in-place.] 43 | * matrix(row,col): row major format assumed. 44 | * 45 | * Assignments are remaining 0 values 46 | * (extra 0 values are replaced with -1) 47 | * 48 | */ 49 | void solve(Matrix &m) { 50 | const size_t rows = m.rows(), 51 | columns = m.columns(), 52 | size = XYZMAX(rows, columns); 53 | 54 | #ifdef DEBUG 55 | std::cout << "Munkres input: " << m << std::endl; 56 | #endif 57 | 58 | // Copy input matrix 59 | this->matrix = m; 60 | 61 | if ( rows != columns ) { 62 | // If the input matrix isn't square, make it square 63 | // and fill the empty values with the largest value present 64 | // in the matrix. 65 | matrix.resize(size, size, matrix.mmax()); 66 | } 67 | 68 | 69 | // STAR == 1 == starred, PRIME == 2 == primed 70 | mask_matrix.resize(size, size); 71 | 72 | row_mask = new bool[size]; 73 | col_mask = new bool[size]; 74 | for ( size_t i = 0 ; i < size ; i++ ) { 75 | row_mask[i] = false; 76 | } 77 | 78 | for ( size_t i = 0 ; i < size ; i++ ) { 79 | col_mask[i] = false; 80 | } 81 | 82 | // Prepare the matrix values... 83 | 84 | // If there were any infinities, replace them with a value greater 85 | // than the maximum value in the matrix. 86 | replace_infinites(matrix); 87 | 88 | minimize_along_direction(matrix, rows >= columns); 89 | minimize_along_direction(matrix, rows < columns); 90 | 91 | // Follow the steps 92 | int step = 1; 93 | while ( step ) { 94 | switch ( step ) { 95 | case 1: 96 | step = step1(); 97 | // step is always 2 98 | break; 99 | case 2: 100 | step = step2(); 101 | // step is always either 0 or 3 102 | break; 103 | case 3: 104 | step = step3(); 105 | // step in [3, 4, 5] 106 | break; 107 | case 4: 108 | step = step4(); 109 | // step is always 2 110 | break; 111 | case 5: 112 | step = step5(); 113 | // step is always 3 114 | break; 115 | } 116 | } 117 | 118 | // Store results 119 | for ( size_t row = 0 ; row < size ; row++ ) { 120 | for ( size_t col = 0 ; col < size ; col++ ) { 121 | if ( mask_matrix(row, col) == STAR ) { 122 | matrix(row, col) = 0; 123 | } else { 124 | matrix(row, col) = -1; 125 | } 126 | } 127 | } 128 | 129 | #ifdef DEBUG 130 | std::cout << "Munkres output: " << matrix << std::endl; 131 | #endif 132 | // Remove the excess rows or columns that we added to fit the 133 | // input to a square matrix. 134 | matrix.resize(rows, columns); 135 | 136 | m = matrix; 137 | 138 | delete [] row_mask; 139 | delete [] col_mask; 140 | } 141 | 142 | static void replace_infinites(Matrix &matrix) { 143 | const size_t rows = matrix.rows(), 144 | columns = matrix.columns(); 145 | //assert( rows > 0 && columns > 0 ); 146 | double max = matrix(0, 0); 147 | constexpr auto infinity = std::numeric_limits::infinity(); 148 | 149 | // Find the greatest value in the matrix that isn't infinity. 150 | for ( size_t row = 0 ; row < rows ; row++ ) { 151 | for ( size_t col = 0 ; col < columns ; col++ ) { 152 | if ( matrix(row, col) != infinity ) { 153 | if ( max == infinity ) { 154 | max = matrix(row, col); 155 | } else { 156 | max = XYZMAX(max, matrix(row, col)); 157 | } 158 | } 159 | } 160 | } 161 | 162 | // a value higher than the maximum value present in the matrix. 163 | if ( max == infinity ) { 164 | // This case only occurs when all values are infinite. 165 | max = 0; 166 | } else { 167 | max++; 168 | } 169 | 170 | for ( size_t row = 0 ; row < rows ; row++ ) { 171 | for ( size_t col = 0 ; col < columns ; col++ ) { 172 | if ( matrix(row, col) == infinity ) { 173 | matrix(row, col) = max; 174 | } 175 | } 176 | } 177 | 178 | } 179 | static void minimize_along_direction(Matrix &matrix, const bool over_columns) { 180 | const size_t outer_size = over_columns ? matrix.columns() : matrix.rows(), 181 | inner_size = over_columns ? matrix.rows() : matrix.columns(); 182 | 183 | // Look for a minimum value to subtract from all values along 184 | // the "outer" direction. 185 | for ( size_t i = 0 ; i < outer_size ; i++ ) { 186 | double min = over_columns ? matrix(0, i) : matrix(i, 0); 187 | 188 | // As long as the current minimum is greater than zero, 189 | // keep looking for the minimum. 190 | // Start at one because we already have the 0th value in min. 191 | for ( size_t j = 1 ; j < inner_size && min > 0 ; j++ ) { 192 | min = XYZMIN( 193 | min, 194 | over_columns ? matrix(j, i) : matrix(i, j)); 195 | } 196 | 197 | if ( min > 0 ) { 198 | for ( size_t j = 0 ; j < inner_size ; j++ ) { 199 | if ( over_columns ) { 200 | matrix(j, i) -= min; 201 | } else { 202 | matrix(i, j) -= min; 203 | } 204 | } 205 | } 206 | } 207 | } 208 | 209 | private: 210 | 211 | inline bool find_uncovered_in_matrix(const double item, size_t &row, size_t &col) const { 212 | const size_t rows = matrix.rows(), 213 | columns = matrix.columns(); 214 | 215 | for ( row = 0 ; row < rows ; row++ ) { 216 | if ( !row_mask[row] ) { 217 | for ( col = 0 ; col < columns ; col++ ) { 218 | if ( !col_mask[col] ) { 219 | if ( matrix(row,col) == item ) { 220 | return true; 221 | } 222 | } 223 | } 224 | } 225 | } 226 | 227 | return false; 228 | } 229 | 230 | bool pair_in_list(const std::pair &needle, const std::list > &haystack) { 231 | for ( std::list >::const_iterator i = haystack.begin() ; i != haystack.end() ; i++ ) { 232 | if ( needle == *i ) { 233 | return true; 234 | } 235 | } 236 | 237 | return false; 238 | } 239 | 240 | int step1() { 241 | const size_t rows = matrix.rows(), 242 | columns = matrix.columns(); 243 | 244 | for ( size_t row = 0 ; row < rows ; row++ ) { 245 | for ( size_t col = 0 ; col < columns ; col++ ) { 246 | if ( 0 == matrix(row, col) ) { 247 | for ( size_t nrow = 0 ; nrow < row ; nrow++ ) 248 | if ( STAR == mask_matrix(nrow,col) ) 249 | goto next_column; 250 | 251 | mask_matrix(row,col) = STAR; 252 | goto next_row; 253 | } 254 | next_column:; 255 | } 256 | next_row:; 257 | } 258 | 259 | return 2; 260 | } 261 | 262 | int step2() { 263 | const size_t rows = matrix.rows(), 264 | columns = matrix.columns(); 265 | size_t covercount = 0; 266 | 267 | for ( size_t row = 0 ; row < rows ; row++ ) 268 | for ( size_t col = 0 ; col < columns ; col++ ) 269 | if ( STAR == mask_matrix(row, col) ) { 270 | col_mask[col] = true; 271 | covercount++; 272 | } 273 | 274 | if ( covercount >= matrix.minsize() ) { 275 | #ifdef DEBUG 276 | std::cout << "Final cover count: " << covercount << std::endl; 277 | #endif 278 | return 0; 279 | } 280 | 281 | #ifdef DEBUG 282 | std::cout << "Munkres matrix has " << covercount << " of " << matrix.minsize() << " Columns covered:" << std::endl; 283 | std::cout << matrix << std::endl; 284 | #endif 285 | 286 | 287 | return 3; 288 | } 289 | 290 | int step3() { 291 | /* 292 | Main Zero Search 293 | 294 | 1. Find an uncovered Z in the distance matrix and prime it. If no such zero exists, go to Step 5 295 | 2. If No Z* exists in the row of the Z', go to Step 4. 296 | 3. If a Z* exists, cover this row and uncover the column of the Z*. Return to Step 3.1 to find a new Z 297 | */ 298 | if ( find_uncovered_in_matrix(0, saverow, savecol) ) { 299 | mask_matrix(saverow,savecol) = PRIME; // prime it. 300 | } else { 301 | return 5; 302 | } 303 | 304 | for ( size_t ncol = 0 ; ncol < matrix.columns() ; ncol++ ) { 305 | if ( mask_matrix(saverow,ncol) == STAR ) { 306 | row_mask[saverow] = true; //cover this row and 307 | col_mask[ncol] = false; // uncover the column containing the starred zero 308 | return 3; // repeat 309 | } 310 | } 311 | 312 | return 4; // no starred zero in the row containing this primed zero 313 | } 314 | 315 | int step4() { 316 | const size_t rows = matrix.rows(), 317 | columns = matrix.columns(); 318 | 319 | // seq contains pairs of row/column values where we have found 320 | // either a star or a prime that is part of the ``alternating sequence``. 321 | std::list > seq; 322 | // use saverow, savecol from step 3. 323 | std::pair z0(saverow, savecol); 324 | seq.insert(seq.end(), z0); 325 | 326 | // We have to find these two pairs: 327 | std::pair z1(-1, -1); 328 | std::pair z2n(-1, -1); 329 | 330 | size_t row, col = savecol; 331 | /* 332 | Increment Set of Starred Zeros 333 | 334 | 1. Construct the ``alternating sequence'' of primed and starred zeros: 335 | 336 | Z0 : Unpaired Z' from Step 4.2 337 | Z1 : The Z* in the column of Z0 338 | Z[2N] : The Z' in the row of Z[2N-1], if such a zero exists 339 | Z[2N+1] : The Z* in the column of Z[2N] 340 | 341 | The sequence eventually terminates with an unpaired Z' = Z[2N] for some N. 342 | */ 343 | bool madepair; 344 | do { 345 | madepair = false; 346 | for ( row = 0 ; row < rows ; row++ ) { 347 | if ( mask_matrix(row,col) == STAR ) { 348 | z1.first = row; 349 | z1.second = col; 350 | if ( pair_in_list(z1, seq) ) { 351 | continue; 352 | } 353 | 354 | madepair = true; 355 | seq.insert(seq.end(), z1); 356 | break; 357 | } 358 | } 359 | 360 | if ( !madepair ) 361 | break; 362 | 363 | madepair = false; 364 | 365 | for ( col = 0 ; col < columns ; col++ ) { 366 | if ( mask_matrix(row, col) == PRIME ) { 367 | z2n.first = row; 368 | z2n.second = col; 369 | if ( pair_in_list(z2n, seq) ) { 370 | continue; 371 | } 372 | madepair = true; 373 | seq.insert(seq.end(), z2n); 374 | break; 375 | } 376 | } 377 | } while ( madepair ); 378 | 379 | for ( std::list >::iterator i = seq.begin() ; 380 | i != seq.end() ; 381 | i++ ) { 382 | // 2. Unstar each starred zero of the sequence. 383 | if ( mask_matrix(i->first,i->second) == STAR ) 384 | mask_matrix(i->first,i->second) = NORMAL; 385 | 386 | // 3. Star each primed zero of the sequence, 387 | // thus increasing the number of starred zeros by one. 388 | if ( mask_matrix(i->first,i->second) == PRIME ) 389 | mask_matrix(i->first,i->second) = STAR; 390 | } 391 | 392 | // 4. Erase all primes, uncover all columns and rows, 393 | for ( size_t row = 0 ; row < mask_matrix.rows() ; row++ ) { 394 | for ( size_t col = 0 ; col < mask_matrix.columns() ; col++ ) { 395 | if ( mask_matrix(row,col) == PRIME ) { 396 | mask_matrix(row,col) = NORMAL; 397 | } 398 | } 399 | } 400 | 401 | for ( size_t i = 0 ; i < rows ; i++ ) { 402 | row_mask[i] = false; 403 | } 404 | 405 | for ( size_t i = 0 ; i < columns ; i++ ) { 406 | col_mask[i] = false; 407 | } 408 | 409 | // and return to Step 2. 410 | return 2; 411 | } 412 | 413 | int step5() { 414 | const size_t rows = matrix.rows(), 415 | columns = matrix.columns(); 416 | /* 417 | New Zero Manufactures 418 | 419 | 1. Let h be the smallest uncovered entry in the (modified) distance matrix. 420 | 2. Add h to all covered rows. 421 | 3. Subtract h from all uncovered columns 422 | 4. Return to Step 3, without altering stars, primes, or covers. 423 | */ 424 | double h = 100000;//xyzoylz std::numeric_limits::max(); 425 | for ( size_t row = 0 ; row < rows ; row++ ) { 426 | if ( !row_mask[row] ) { 427 | for ( size_t col = 0 ; col < columns ; col++ ) { 428 | if ( !col_mask[col] ) { 429 | if ( h > matrix(row, col) && matrix(row, col) != 0 ) { 430 | h = matrix(row, col); 431 | } 432 | } 433 | } 434 | } 435 | } 436 | 437 | for ( size_t row = 0 ; row < rows ; row++ ) { 438 | if ( row_mask[row] ) { 439 | for ( size_t col = 0 ; col < columns ; col++ ) { 440 | matrix(row, col) += h; 441 | } 442 | } 443 | } 444 | 445 | for ( size_t col = 0 ; col < columns ; col++ ) { 446 | if ( !col_mask[col] ) { 447 | for ( size_t row = 0 ; row < rows ; row++ ) { 448 | matrix(row, col) -= h; 449 | } 450 | } 451 | } 452 | 453 | return 3; 454 | } 455 | 456 | Matrix mask_matrix; 457 | Matrix matrix; 458 | bool *row_mask; 459 | bool *col_mask; 460 | size_t saverow = 0, savecol = 0; 461 | }; 462 | 463 | 464 | #endif /* !defined(_MUNKRES_H_) */ 465 | -------------------------------------------------------------------------------- /include/nn_matching.h: -------------------------------------------------------------------------------- 1 | #ifndef NN_MATCHING_H 2 | #define NN_MATCHING_H 3 | 4 | #include "datatype.h" 5 | 6 | #include 7 | 8 | //A tool to calculate distance; 9 | class NearNeighborDisMetric{ 10 | public: 11 | enum METRIC_TYPE{euclidean=1, cosine}; 12 | NearNeighborDisMetric(METRIC_TYPE metric, 13 | float matching_threshold, 14 | int budget); 15 | DYNAMICM distance(const FEATURESS& features, const std::vector &targets); 16 | // void partial_fit(FEATURESS& features, std::vector targets, std::vector active_targets); 17 | void partial_fit(std::vector& tid_feats, std::vector& active_targets); 18 | float mating_threshold; 19 | 20 | private: 21 | typedef Eigen::VectorXf (NearNeighborDisMetric::*PTRFUN)(const FEATURESS&, const FEATURESS&); 22 | Eigen::VectorXf _nncosine_distance(const FEATURESS& x, const FEATURESS& y); 23 | Eigen::VectorXf _nneuclidean_distance(const FEATURESS& x, const FEATURESS& y); 24 | 25 | Eigen::MatrixXf _pdist(const FEATURESS& x, const FEATURESS& y); 26 | Eigen::MatrixXf _cosine_distance(const FEATURESS & a, const FEATURESS& b, bool data_is_normalized = false); 27 | private: 28 | PTRFUN _metric; 29 | int budget; 30 | std::map samples; 31 | }; 32 | 33 | #endif // NN_MATCHING_H 34 | -------------------------------------------------------------------------------- /include/track.h: -------------------------------------------------------------------------------- 1 | #ifndef TRACK_H 2 | #define TRACK_H 3 | 4 | 5 | #include "kalmanfilter.h" 6 | #include "datatype.h" 7 | #include "model.hpp" 8 | 9 | class Track 10 | { 11 | /*""" 12 | A single target track with state space `(x, y, a, h)` and associated 13 | velocities, where `(x, y)` is the center of the bounding box, `a` is the 14 | aspect ratio and `h` is the height. 15 | 16 | Parameters 17 | ---------- 18 | mean : ndarray 19 | Mean vector of the initial state distribution. 20 | covariance : ndarray 21 | Covariance matrix of the initial state distribution. 22 | track_id : int 23 | A unique track identifier. 24 | n_init : int 25 | Number of consecutive detections before the track is confirmed. The 26 | track state is set to `Deleted` if a miss occurs within the first 27 | `n_init` frames. 28 | max_age : int 29 | The maximum number of consecutive misses before the track state is 30 | set to `Deleted`. 31 | feature : Optional[ndarray] 32 | Feature vector of the detection this track originates from. If not None, 33 | this feature is added to the `features` cache. 34 | 35 | Attributes 36 | ---------- 37 | mean : ndarray 38 | Mean vector of the initial state distribution. 39 | covariance : ndarray 40 | Covariance matrix of the initial state distribution. 41 | track_id : int 42 | A unique track identifier. 43 | hits : int 44 | Total number of measurement updates. 45 | age : int 46 | Total number of frames since first occurance. 47 | time_since_update : int 48 | Total number of frames since last measurement update. 49 | state : TrackState 50 | The current track state. 51 | features : List[ndarray] 52 | A cache of features. On each measurement update, the associated feature 53 | vector is added to this list. 54 | 55 | """*/ 56 | enum TrackState {Tentative = 1, Confirmed, Deleted}; 57 | 58 | public: 59 | Track(KAL_MEAN& mean, KAL_COVA& covariance, int track_id, 60 | int n_init, int max_age, const FEATURE& feature); 61 | Track(KAL_MEAN& mean, KAL_COVA& covariance, int track_id, 62 | int n_init, int max_age, const FEATURE& feature, int cls, float conf); 63 | void predit(KalmanFilter* kf); 64 | void update(KalmanFilter* const kf, const DETECTION_ROW &detection); 65 | void update(KalmanFilter* const kf, const DETECTION_ROW & detection, CLSCONF pair_det); 66 | void mark_missed(); 67 | bool is_confirmed(); 68 | bool is_deleted(); 69 | bool is_tentative(); 70 | DETECTBOX to_tlwh(); 71 | int time_since_update; 72 | int track_id; 73 | FEATURESS features; 74 | KAL_MEAN mean; 75 | KAL_COVA covariance; 76 | 77 | int hits; 78 | int age; 79 | int _n_init; 80 | int _max_age; 81 | TrackState state; 82 | 83 | int cls; 84 | float conf; 85 | private: 86 | void featuresAppendOne(const FEATURE& f); 87 | }; 88 | 89 | #endif // TRACK_H 90 | -------------------------------------------------------------------------------- /include/tracker.h: -------------------------------------------------------------------------------- 1 | #ifndef TRACKER_H 2 | #define TRACKER_H 3 | 4 | 5 | #include 6 | 7 | #include "kalmanfilter.h" 8 | #include "track.h" 9 | #include "model.hpp" 10 | 11 | using namespace std; 12 | 13 | class NearNeighborDisMetric; 14 | 15 | class tracker 16 | { 17 | public: 18 | NearNeighborDisMetric* metric; 19 | float max_iou_distance; 20 | int max_age; 21 | int n_init; 22 | 23 | KalmanFilter* kf; 24 | 25 | int _next_idx; 26 | public: 27 | std::vector tracks; 28 | tracker(/*NearNeighborDisMetric* metric,*/ 29 | float max_cosine_distance, int nn_budget, 30 | float max_iou_distance = 0.7, 31 | int max_age = 70, int n_init=3); 32 | void predict(); 33 | void update(const DETECTIONS& detections); 34 | void update(const DETECTIONSV2& detectionsv2); 35 | typedef DYNAMICM (tracker::* GATED_METRIC_FUNC)( 36 | std::vector& tracks, 37 | const DETECTIONS& dets, 38 | const std::vector& track_indices, 39 | const std::vector& detection_indices); 40 | private: 41 | void _match(const DETECTIONS& detections, TRACHER_MATCHD& res); 42 | void _initiate_track(const DETECTION_ROW& detection); 43 | void _initiate_track(const DETECTION_ROW& detection, CLSCONF clsConf); 44 | public: 45 | DYNAMICM gated_matric( 46 | std::vector& tracks, 47 | const DETECTIONS& dets, 48 | const std::vector& track_indices, 49 | const std::vector& detection_indices); 50 | DYNAMICM iou_cost( 51 | std::vector& tracks, 52 | const DETECTIONS& dets, 53 | const std::vector& track_indices, 54 | const std::vector& detection_indices); 55 | Eigen::VectorXf iou(DETECTBOX& bbox, 56 | DETECTBOXSS &candidates); 57 | }; 58 | 59 | #endif // TRACKER_H 60 | -------------------------------------------------------------------------------- /src/deepsort.cpp: -------------------------------------------------------------------------------- 1 | #define _DLL_EXPORTS 2 | 3 | #include "deepsort.h" 4 | 5 | //DeepSort::DeepSort(std::string modelPath, int batchSize, int featureDim, int gpuID, ILogger* gLogger) { 6 | // this->gpuID = gpuID; 7 | // this->enginePath = modelPath; 8 | // this->batchSize = batchSize; 9 | // this->featureDim = featureDim; 10 | // this->imgShape = cv::Size(64, 128); 11 | // this->maxBudget = 100; 12 | // this->maxCosineDist = 0.2; 13 | // this->gLogger = gLogger; 14 | // init(); 15 | //} 16 | 17 | DeepSort::DeepSort(std::string modelPath, int batchSize, int featureDim, int gpuID) { 18 | this->gpuID = gpuID; 19 | this->enginePath = modelPath; 20 | this->batchSize = batchSize; 21 | this->featureDim = featureDim; 22 | this->imgShape = cv::Size(64, 128); 23 | this->maxBudget = 100; 24 | this->maxCosineDist = 0.2; 25 | init(); 26 | } 27 | 28 | 29 | void DeepSort::init() { 30 | objTracker = new tracker(maxCosineDist, maxBudget); 31 | //featureExtractor = new FeatureTensor(batchSize, imgShape, featureDim, gpuID, gLogger); 32 | featureExtractor = new FeatureTensor(batchSize, imgShape, featureDim, gpuID); 33 | 34 | featureExtractor->loadOnnx(enginePath); 35 | } 36 | 37 | DeepSort::~DeepSort() { 38 | delete objTracker; 39 | } 40 | 41 | void DeepSort::sort(cv::Mat& frame, vector& dets) { 42 | // preprocess Mat -> DETECTION 43 | DETECTIONS detections; 44 | vector clsConf; 45 | 46 | for (DetectBox i : dets) { 47 | DETECTBOX box(i.x1, i.y1, i.x2 - i.x1, i.y2 - i.y1); 48 | DETECTION_ROW d; 49 | d.tlwh = box; 50 | d.confidence = i.confidence; 51 | detections.push_back(d); 52 | clsConf.push_back(CLSCONF((int)i.classID, i.confidence)); 53 | } 54 | result.clear(); 55 | results.clear(); 56 | if (detections.size() > 0) { 57 | DETECTIONSV2 detectionsv2 = make_pair(clsConf, detections); 58 | sort(frame, detectionsv2); 59 | } 60 | // postprocess DETECTION -> Mat 61 | dets.clear(); 62 | for (auto r : result) { 63 | DETECTBOX i = r.second; 64 | DetectBox b(i(0), i(1), i(2) + i(0), i(3) + i(1), 1.); 65 | b.trackID = (float)r.first; 66 | dets.push_back(b); 67 | } 68 | for (int i = 0; i < results.size(); ++i) { 69 | CLSCONF c = results[i].first; 70 | dets[i].classID = c.cls; 71 | dets[i].confidence = c.conf; 72 | } 73 | } 74 | 75 | 76 | void DeepSort::sort(cv::Mat& frame, DETECTIONS& detections) { 77 | bool flag = featureExtractor->getRectsFeature(frame, detections); 78 | if (flag) { 79 | objTracker->predict(); 80 | objTracker->update(detections); 81 | //result.clear(); 82 | for (Track& track : objTracker->tracks) { 83 | if (!track.is_confirmed() || track.time_since_update > 1) 84 | continue; 85 | result.push_back(make_pair(track.track_id, track.to_tlwh())); 86 | } 87 | } 88 | } 89 | 90 | void DeepSort::sort(cv::Mat& frame, DETECTIONSV2& detectionsv2) { 91 | std::vector& clsConf = detectionsv2.first; 92 | DETECTIONS& detections = detectionsv2.second; 93 | bool flag = featureExtractor->getRectsFeature(frame, detections); 94 | if (flag) { 95 | objTracker->predict(); 96 | objTracker->update(detectionsv2); 97 | result.clear(); 98 | results.clear(); 99 | for (Track& track : objTracker->tracks) { 100 | if (!track.is_confirmed() || track.time_since_update > 1) 101 | continue; 102 | result.push_back(make_pair(track.track_id, track.to_tlwh())); 103 | results.push_back(make_pair(CLSCONF(track.cls, track.conf), track.to_tlwh())); 104 | } 105 | } 106 | } 107 | 108 | void DeepSort::sort(vector& dets) { 109 | DETECTIONS detections; 110 | for (DetectBox i : dets) { 111 | DETECTBOX box(i.x1, i.y1, i.x2 - i.x1, i.y2 - i.y1); 112 | DETECTION_ROW d; 113 | d.tlwh = box; 114 | d.confidence = i.confidence; 115 | detections.push_back(d); 116 | } 117 | if (detections.size() > 0) 118 | sort(detections); 119 | dets.clear(); 120 | for (auto r : result) { 121 | DETECTBOX i = r.second; 122 | DetectBox b(i(0), i(1), i(2), i(3), 1.); 123 | b.trackID = r.first; 124 | dets.push_back(b); 125 | } 126 | } 127 | 128 | void DeepSort::sort(DETECTIONS& detections) { 129 | bool flag = featureExtractor->getRectsFeature(detections); 130 | if (flag) { 131 | objTracker->predict(); 132 | objTracker->update(detections); 133 | result.clear(); 134 | for (Track& track : objTracker->tracks) { 135 | if (!track.is_confirmed() || track.time_since_update > 1) 136 | continue; 137 | result.push_back(make_pair(track.track_id, track.to_tlwh())); 138 | } 139 | } 140 | } 141 | -------------------------------------------------------------------------------- /src/featuretensor.cpp: -------------------------------------------------------------------------------- 1 | #include "featuretensor.h" 2 | #include 3 | 4 | //using namespace nvinfer1; 5 | 6 | #define INPUTSTREAM_SIZE (maxBatchSize*3*imgShape.area()) 7 | #define OUTPUTSTREAM_SIZE (maxBatchSize*featureDim) 8 | 9 | FeatureTensor::FeatureTensor(const int maxBatchSize, const cv::Size imgShape, const int featureDim, int gpuID) 10 | : maxBatchSize(maxBatchSize), imgShape(imgShape), featureDim(featureDim), 11 | inputName("input"), outputName("output") { 12 | //cudaSetDevice(gpuID); 13 | //this->gLogger = gLogger; 14 | //runtime = nullptr; 15 | //engine = nullptr; 16 | //context = nullptr; 17 | 18 | means[0] = 0.485, means[1] = 0.456, means[2] = 0.406; 19 | std[0] = 0.229, std[1] = 0.224, std[2] = 0.225; 20 | 21 | initFlag = false; 22 | } 23 | 24 | FeatureTensor::~FeatureTensor() { 25 | if (initFlag) { 26 | // cudaStreamSynchronize(cudaStream); 27 | //cudaStreamDestroy(cudaStream); 28 | //cudaFree(buffers[inputIndex]); 29 | //cudaFree(buffers[outputIndex]); 30 | } 31 | } 32 | 33 | bool FeatureTensor::getRectsFeature(const cv::Mat& img, DETECTIONS& det) { 34 | std::vector mats; 35 | for (auto& dbox : det) { 36 | cv::Rect rect = cv::Rect(int(dbox.tlwh(0)), int(dbox.tlwh(1)), 37 | int(dbox.tlwh(2)), int(dbox.tlwh(3))); 38 | rect.x -= (rect.height * 0.5 - rect.width) * 0.5; 39 | rect.width = rect.height * 0.5; 40 | rect.x = (rect.x >= 0 ? rect.x : 0); 41 | rect.y = (rect.y >= 0 ? rect.y : 0); 42 | rect.width = (rect.x + rect.width <= img.cols ? rect.width : (img.cols - rect.x)); 43 | rect.height = (rect.y + rect.height <= img.rows ? rect.height : (img.rows - rect.y)); 44 | cv::Mat tempMat = img(rect).clone(); 45 | cv::resize(tempMat, tempMat, imgShape); 46 | mats.push_back(tempMat); 47 | } 48 | cv::Mat out = doInference(mats); 49 | // decode output to det 50 | stream2det(out, det); 51 | return true; 52 | } 53 | 54 | bool FeatureTensor::getRectsFeature(DETECTIONS& det) { 55 | return true; 56 | } 57 | 58 | void FeatureTensor::loadOnnx(std::string onnxPath) { 59 | //dnn_engine = new cv::dnn::Net(); 60 | 61 | //dnn_engine.setPreferableBackend(cv::dnn::DNN_BACKEND_CUDA); 62 | //dnn_engine.setPreferableTarget(cv::dnn::DNN_TARGET_CUDA); 63 | //static auto _engine = cv::dnn::readNetFromONNX(onnxPath); 64 | dnn_engine = cv::dnn::readNetFromONNX(onnxPath); 65 | } 66 | 67 | //int FeatureTensor::getResult(float*& buffer) { 68 | // if (buffer != nullptr) 69 | // delete buffer; 70 | // int curStreamSize = curBatchSize*featureDim; 71 | // buffer = new float[curStreamSize]; 72 | // for (int i = 0; i < curStreamSize; ++i) { 73 | // buffer[i] = outputBuffer[i]; 74 | // } 75 | // return curStreamSize; 76 | //} 77 | 78 | cv::Mat FeatureTensor::doInference(vector& imgMats) { 79 | cv::Mat out; 80 | int mat_size = imgMats.size(); 81 | if (mat_size > 0) { 82 | if (mat_size != 32) { 83 | for (size_t i = 0; i < 32 - mat_size; ++i) 84 | imgMats.push_back(cv::Mat(cv::Size(64, 128), CV_8UC3, cv::Scalar(255, 255, 255))); 85 | } 86 | 87 | out = doInference_run(imgMats); 88 | } 89 | 90 | return out; 91 | } 92 | 93 | 94 | cv::Mat FeatureTensor::doInference_run(vector imgMats) { 95 | //cudaMemcpyAsync(buffers[inputIndex], inputBuffer, inputStreamSize * sizeof(float), cudaMemcpyHostToDevice, cudaStream); 96 | //Dims4 inputDims{curBatchSize, 3, imgShape.height, imgShape.width}; 97 | //context->setBindingDimensions(0, inputDims); 98 | // 99 | //context->enqueueV2(buffers, cudaStream, nullptr); 100 | //cudaMemcpyAsync(outputBuffer, buffers[outputIndex], outputStreamSize * sizeof(float), cudaMemcpyDeviceToHost, cudaStream); 101 | // cudaStreamSynchronize(cudaStream); 102 | auto blob = cv::dnn::blobFromImages(imgMats, 0.225, cv::Size(64, 128), cv::Scalar(0.485, 0.456, 0.406), true); 103 | 104 | dnn_engine.setInput(blob, "input"); 105 | cv::Mat out = dnn_engine.forward(this->outputName); 106 | 107 | return out; 108 | } 109 | 110 | void FeatureTensor::stream2det(cv::Mat stream, DETECTIONS& det) { 111 | int i = 0; 112 | for (DETECTION_ROW& dbox : det) { 113 | for (int j = 0; j < featureDim; ++j) { 114 | float data = stream.at(j, i); 115 | dbox.feature[j] = data; 116 | } 117 | i++; 118 | } 119 | } 120 | -------------------------------------------------------------------------------- /src/hungarianoper.cpp: -------------------------------------------------------------------------------- 1 | #include "hungarianoper.h" 2 | 3 | Eigen::Matrix HungarianOper::Solve(const DYNAMICM &cost_matrix) { 4 | int rows = cost_matrix.rows(); 5 | int cols = cost_matrix.cols(); 6 | Matrix matrix(rows, cols); 7 | for (int row = 0; row < rows; row++) { 8 | for (int col = 0; col < cols; col++) { 9 | matrix(row, col) = cost_matrix(row, col); 10 | } 11 | } 12 | //Munkres get matrix; 13 | Munkres m; 14 | m.solve(matrix); 15 | 16 | // 17 | std::vector> pairs; 18 | for (int row = 0; row < rows; row++) { 19 | for (int col = 0; col < cols; col++) { 20 | int tmp = (int)matrix(row, col); 21 | if (tmp == 0) pairs.push_back(std::make_pair(row, col)); 22 | } 23 | } 24 | // 25 | int count = pairs.size(); 26 | Eigen::Matrix re(count, 2); 27 | for (int i = 0; i < count; i++) { 28 | re(i, 0) = pairs[i].first; 29 | re(i, 1) = pairs[i].second; 30 | } 31 | return re; 32 | }//end Solve; 33 | -------------------------------------------------------------------------------- /src/kalmanfilter.cpp: -------------------------------------------------------------------------------- 1 | #include "kalmanfilter.h" 2 | #include 3 | #include 4 | 5 | const double KalmanFilter::chi2inv95[10] = { 6 | 0, 7 | 3.8415, 8 | 5.9915, 9 | 7.8147, 10 | 9.4877, 11 | 11.070, 12 | 12.592, 13 | 14.067, 14 | 15.507, 15 | 16.919 16 | }; 17 | KalmanFilter::KalmanFilter() { 18 | int ndim = 4; 19 | double dt = 1.; 20 | 21 | _motion_mat = Eigen::MatrixXf::Identity(8, 8); 22 | for(int i = 0; i < ndim; i++) { 23 | _motion_mat(i, ndim+i) = dt; 24 | } 25 | _update_mat = Eigen::MatrixXf::Identity(4, 8); 26 | 27 | this->_std_weight_position = 1. / 20; 28 | this->_std_weight_velocity = 1. / 160; 29 | } 30 | 31 | KAL_DATA KalmanFilter::initiate(const DETECTBOX& measurement) { 32 | DETECTBOX mean_pos = measurement; 33 | DETECTBOX mean_vel; 34 | for(int i = 0; i < 4; i++) mean_vel(i) = 0; 35 | 36 | KAL_MEAN mean; 37 | for(int i = 0; i < 8; i++){ 38 | if(i < 4) mean(i) = mean_pos(i); 39 | else mean(i) = mean_vel(i - 4); 40 | } 41 | 42 | KAL_MEAN std; 43 | std(0) = 2 * _std_weight_position * measurement[3]; 44 | std(1) = 2 * _std_weight_position * measurement[3]; 45 | std(2) = 1e-2; 46 | std(3) = 2 * _std_weight_position * measurement[3]; 47 | std(4) = 10 * _std_weight_velocity * measurement[3]; 48 | std(5) = 10 * _std_weight_velocity * measurement[3]; 49 | std(6) = 1e-5; 50 | std(7) = 10 * _std_weight_velocity * measurement[3]; 51 | 52 | KAL_MEAN tmp = std.array().square(); 53 | KAL_COVA var = tmp.asDiagonal(); 54 | return std::make_pair(mean, var); 55 | } 56 | 57 | void KalmanFilter::predict(KAL_MEAN &mean, KAL_COVA &covariance) { 58 | //revise the data; 59 | DETECTBOX std_pos; 60 | std_pos << _std_weight_position * mean(3), 61 | _std_weight_position * mean(3), 62 | 1e-2, 63 | _std_weight_position * mean(3); 64 | DETECTBOX std_vel; 65 | std_vel << _std_weight_velocity * mean(3), 66 | _std_weight_velocity * mean(3), 67 | 1e-5, 68 | _std_weight_velocity * mean(3); 69 | KAL_MEAN tmp; 70 | tmp.block<1,4>(0,0) = std_pos; 71 | tmp.block<1,4>(0,4) = std_vel; 72 | tmp = tmp.array().square(); 73 | KAL_COVA motion_cov = tmp.asDiagonal(); 74 | KAL_MEAN mean1 = this->_motion_mat * mean.transpose(); 75 | KAL_COVA covariance1 = this->_motion_mat * covariance *(_motion_mat.transpose()); 76 | covariance1 += motion_cov; 77 | 78 | mean = mean1; 79 | covariance = covariance1; 80 | } 81 | 82 | KAL_HDATA KalmanFilter::project(const KAL_MEAN &mean, const KAL_COVA &covariance) { 83 | DETECTBOX std; 84 | std << _std_weight_position * mean(3), _std_weight_position * mean(3), 85 | 1e-1, _std_weight_position * mean(3); 86 | KAL_HMEAN mean1 = _update_mat * mean.transpose(); 87 | KAL_HCOVA covariance1 = _update_mat * covariance * (_update_mat.transpose()); 88 | Eigen::Matrix diag = std.asDiagonal(); 89 | diag = diag.array().square().matrix(); 90 | covariance1 += diag; 91 | // covariance1.diagonal() << diag; 92 | return std::make_pair(mean1, covariance1); 93 | } 94 | 95 | KAL_DATA 96 | KalmanFilter::update( 97 | const KAL_MEAN &mean, 98 | const KAL_COVA &covariance, 99 | const DETECTBOX &measurement) { 100 | KAL_HDATA pa = project(mean, covariance); 101 | KAL_HMEAN projected_mean = pa.first; 102 | KAL_HCOVA projected_cov = pa.second; 103 | 104 | //chol_factor, lower = 105 | //scipy.linalg.cho_factor(projected_cov, lower=True, check_finite=False) 106 | //kalmain_gain = 107 | //scipy.linalg.cho_solve((cho_factor, lower), 108 | //np.dot(covariance, self._upadte_mat.T).T, 109 | //check_finite=False).T 110 | Eigen::Matrix B = (covariance * (_update_mat.transpose())).transpose(); 111 | Eigen::Matrix kalman_gain = (projected_cov.llt().solve(B)).transpose(); // eg.8x4 112 | Eigen::Matrix innovation = measurement - projected_mean; //eg.1x4 113 | auto tmp = innovation*(kalman_gain.transpose()); 114 | KAL_MEAN new_mean = (mean.array() + tmp.array()).matrix(); 115 | KAL_COVA new_covariance = covariance - kalman_gain*projected_cov*(kalman_gain.transpose()); 116 | return std::make_pair(new_mean, new_covariance); 117 | } 118 | 119 | Eigen::Matrix 120 | KalmanFilter::gating_distance( 121 | const KAL_MEAN &mean, 122 | const KAL_COVA &covariance, 123 | const std::vector &measurements, 124 | bool only_position) { 125 | KAL_HDATA pa = this->project(mean, covariance); 126 | if(only_position) { 127 | printf("not implement!"); 128 | exit(0); 129 | } 130 | KAL_HMEAN mean1 = pa.first; 131 | KAL_HCOVA covariance1 = pa.second; 132 | 133 | // Eigen::Matrix d(size, 4); 134 | DETECTBOXSS d(measurements.size(), 4); 135 | int pos = 0; 136 | for(DETECTBOX box:measurements) { 137 | d.row(pos++) = box - mean1; 138 | } 139 | Eigen::Matrix factor = covariance1.llt().matrixL(); 140 | Eigen::Matrix z = factor.triangularView().solve(d).transpose(); 141 | auto zz = ((z.array())*(z.array())).matrix(); 142 | auto square_maha = zz.colwise().sum(); 143 | return square_maha; 144 | } 145 | 146 | -------------------------------------------------------------------------------- /src/linear_assignment.cpp: -------------------------------------------------------------------------------- 1 | #include "linear_assignment.h" 2 | #include "hungarianoper.h" 3 | #include 4 | 5 | linear_assignment *linear_assignment::instance = NULL; 6 | linear_assignment::linear_assignment() 7 | { 8 | } 9 | 10 | linear_assignment *linear_assignment::getInstance() 11 | { 12 | if(instance == NULL) instance = new linear_assignment(); 13 | return instance; 14 | } 15 | 16 | TRACHER_MATCHD 17 | linear_assignment::matching_cascade( 18 | tracker *distance_metric, 19 | tracker::GATED_METRIC_FUNC distance_metric_func, 20 | float max_distance, 21 | int cascade_depth, 22 | std::vector &tracks, 23 | const DETECTIONS &detections, 24 | std::vector& track_indices, 25 | std::vector detection_indices) 26 | { 27 | TRACHER_MATCHD res; 28 | // std::cout << "distance_metric" << distance_metric << std::endl; 29 | // std::cout << "max_distance" << max_distance << std::endl; 30 | // std::cout << "cascade_depth" << cascade_depth << std::endl; 31 | // std::cout << "tracks [" << std::endl; 32 | // for (auto i : tracks) 33 | // std::cout << i.hits << ", "; 34 | // std::cout << "]" << endl; 35 | // std::cout << "detections [" << std::endl; 36 | // for (auto i : detections) 37 | // std::cout << i.confidence << ", "; 38 | // std::cout << "]" << endl; 39 | // std::cout << "track_indices [" << std::endl; 40 | // for (auto i : track_indices) 41 | // std::cout << i << ", "; 42 | // std::cout << "]" << endl; 43 | // std::cout << "detection_indices [" << std::endl; 44 | // for (auto i : detection_indices) 45 | // std::cout << i << ", "; 46 | // std::cout << "]" << endl; 47 | // !!!python diff: track_indices will never be None. 48 | // if(track_indices.empty() == true) { 49 | // for(size_t i = 0; i < tracks.size(); i++) { 50 | // track_indices.push_back(i); 51 | // } 52 | // } 53 | 54 | //!!!python diff: detection_indices will always be None. 55 | for(size_t i = 0; i < detections.size(); i++) { 56 | detection_indices.push_back(int(i)); 57 | } 58 | 59 | std::vector unmatched_detections; 60 | unmatched_detections.assign(detection_indices.begin(), detection_indices.end()); 61 | res.matches.clear(); 62 | std::vector track_indices_l; 63 | 64 | std::map matches_trackid; 65 | for(int level = 0; level < cascade_depth; level++) { 66 | if(unmatched_detections.size() == 0) break; //No detections left; 67 | 68 | track_indices_l.clear(); 69 | for(int k:track_indices) { 70 | if(tracks[k].time_since_update == 1+level) 71 | track_indices_l.push_back(k); 72 | } 73 | if(track_indices_l.size() == 0) continue; //Nothing to match at this level. 74 | 75 | TRACHER_MATCHD tmp = min_cost_matching( 76 | distance_metric, distance_metric_func, 77 | max_distance, tracks, detections, track_indices_l, 78 | unmatched_detections); 79 | unmatched_detections.assign(tmp.unmatched_detections.begin(), tmp.unmatched_detections.end()); 80 | for(size_t i = 0; i < tmp.matches.size(); i++) { 81 | MATCH_DATA pa = tmp.matches[i]; 82 | res.matches.push_back(pa); 83 | matches_trackid.insert(pa); 84 | } 85 | } 86 | res.unmatched_detections.assign(unmatched_detections.begin(), unmatched_detections.end()); 87 | for(size_t i = 0; i < track_indices.size(); i++) { 88 | int tid = track_indices[i]; 89 | if(matches_trackid.find(tid) == matches_trackid.end()) 90 | res.unmatched_tracks.push_back(tid); 91 | } 92 | return res; 93 | } 94 | 95 | TRACHER_MATCHD 96 | linear_assignment::min_cost_matching(tracker *distance_metric, 97 | tracker::GATED_METRIC_FUNC distance_metric_func, 98 | float max_distance, 99 | std::vector &tracks, 100 | const DETECTIONS &detections, 101 | std::vector &track_indices, 102 | std::vector &detection_indices) 103 | { 104 | TRACHER_MATCHD res; 105 | //!!!python diff: track_indices && detection_indices will never be None. 106 | // if(track_indices.empty() == true) { 107 | // for(size_t i = 0; i < tracks.size(); i++) { 108 | // track_indices.push_back(i); 109 | // } 110 | // } 111 | // if(detection_indices.empty() == true) { 112 | // for(size_t i = 0; i < detections.size(); i++) { 113 | // detection_indices.push_back(int(i)); 114 | // } 115 | // } 116 | if((detection_indices.size() == 0) || (track_indices.size() == 0)) { 117 | res.matches.clear(); 118 | res.unmatched_tracks.assign(track_indices.begin(), track_indices.end()); 119 | res.unmatched_detections.assign(detection_indices.begin(), detection_indices.end()); 120 | return res; 121 | } 122 | DYNAMICM cost_matrix = (distance_metric->*(distance_metric_func))( 123 | tracks, detections, track_indices, detection_indices); 124 | for(int i = 0; i < cost_matrix.rows(); i++) { 125 | for(int j = 0; j < cost_matrix.cols(); j++) { 126 | float tmp = cost_matrix(i,j); 127 | if(tmp > max_distance) cost_matrix(i,j) = max_distance + 1e-5; 128 | } 129 | } 130 | Eigen::Matrix indices = HungarianOper::Solve(cost_matrix); 131 | res.matches.clear(); 132 | res.unmatched_tracks.clear(); 133 | res.unmatched_detections.clear(); 134 | for(size_t col = 0; col < detection_indices.size(); col++) { 135 | bool flag = false; 136 | for(int i = 0; i < indices.rows(); i++) 137 | if(indices(i, 1) == col) { flag = true; break;} 138 | if(flag == false)res.unmatched_detections.push_back(detection_indices[col]); 139 | } 140 | for(size_t row = 0; row < track_indices.size(); row++) { 141 | bool flag = false; 142 | for(int i = 0; i < indices.rows(); i++) 143 | if(indices(i, 0) == row) { flag = true; break; } 144 | if(flag == false) res.unmatched_tracks.push_back(track_indices[row]); 145 | } 146 | for(int i = 0; i < indices.rows(); i++) { 147 | int row = indices(i, 0); 148 | int col = indices(i, 1); 149 | 150 | int track_idx = track_indices[row]; 151 | int detection_idx = detection_indices[col]; 152 | if(cost_matrix(row, col) > max_distance) { 153 | res.unmatched_tracks.push_back(track_idx); 154 | res.unmatched_detections.push_back(detection_idx); 155 | } else res.matches.push_back(std::make_pair(track_idx, detection_idx)); 156 | } 157 | return res; 158 | } 159 | 160 | DYNAMICM 161 | linear_assignment::gate_cost_matrix( 162 | KalmanFilter *kf, 163 | DYNAMICM &cost_matrix, 164 | std::vector &tracks, 165 | const DETECTIONS &detections, 166 | const std::vector &track_indices, 167 | const std::vector &detection_indices, 168 | float gated_cost, bool only_position) 169 | { 170 | // std::cout << "input cost matric" << cost_matrix << std::endl; 171 | int gating_dim = (only_position == true?2:4); 172 | double gating_threshold = KalmanFilter::chi2inv95[gating_dim]; 173 | std::vector measurements; 174 | for(int i:detection_indices) { 175 | DETECTION_ROW t = detections[i]; 176 | measurements.push_back(t.to_xyah()); 177 | } 178 | for(size_t i = 0; i < track_indices.size(); i++) { 179 | Track& track = tracks[track_indices[i]]; 180 | Eigen::Matrix gating_distance = kf->gating_distance( 181 | track.mean, track.covariance, measurements, only_position); 182 | for (int j = 0; j < gating_distance.cols(); j++) { 183 | if (gating_distance(0, j) > gating_threshold) cost_matrix(i, j) = gated_cost; 184 | } 185 | } 186 | // std::cout << "out cost matrix" << cost_matrix << std::endl; 187 | return cost_matrix; 188 | } 189 | 190 | -------------------------------------------------------------------------------- /src/munkres.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2007 John Weaver 3 | * Copyright (c) 2015 Miroslav Krajicek 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 2 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program; if not, write to the Free Software 17 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 18 | */ 19 | 20 | #include "munkres.h" 21 | 22 | template class Munkres; 23 | template class Munkres; 24 | template class Munkres; 25 | 26 | -------------------------------------------------------------------------------- /src/nn_matching.cpp: -------------------------------------------------------------------------------- 1 | #include "nn_matching.h" 2 | #include 3 | 4 | using namespace Eigen; 5 | 6 | NearNeighborDisMetric::NearNeighborDisMetric( 7 | NearNeighborDisMetric::METRIC_TYPE metric, 8 | float matching_threshold, int budget) 9 | { 10 | if (metric == euclidean) { 11 | _metric = 12 | &NearNeighborDisMetric::_nneuclidean_distance; 13 | } else if (metric == cosine) { 14 | _metric = 15 | &NearNeighborDisMetric::_nncosine_distance; 16 | } 17 | 18 | this->mating_threshold = matching_threshold; 19 | this->budget = budget; 20 | this->samples.clear(); 21 | } 22 | 23 | DYNAMICM 24 | NearNeighborDisMetric::distance( 25 | const FEATURESS & features, 26 | const std::vector < int >&targets) 27 | { 28 | DYNAMICM cost_matrix = Eigen::MatrixXf::Zero(targets.size(), features.rows()); 29 | int idx = 0; 30 | for (int target:targets) { 31 | cost_matrix.row(idx) = (this->*_metric) (this->samples[target], features); 32 | idx++; 33 | } 34 | return cost_matrix; 35 | } 36 | 37 | void 38 | NearNeighborDisMetric::partial_fit( 39 | std::vector &tid_feats, 40 | std::vector < int >&active_targets) 41 | { 42 | /*python code: 43 | * let feature(target_id) append to samples; 44 | * && delete not comfirmed target_id from samples. 45 | * update samples; 46 | */ 47 | for (TRACKER_DATA & data:tid_feats) { 48 | int track_id = data.first; 49 | FEATURESS newFeatOne = data.second; 50 | 51 | if (samples.find(track_id) != samples.end()) { //append 52 | int oldSize = samples[track_id].rows(); 53 | int addSize = newFeatOne.rows(); 54 | int newSize = oldSize + addSize; 55 | 56 | if (newSize <= this->budget) { 57 | FEATURESS newSampleFeatures(newSize, Feature_Vector_Dim); 58 | newSampleFeatures.block(0, 0, oldSize, Feature_Vector_Dim) = samples[track_id]; 59 | newSampleFeatures.block(oldSize, 0, addSize, Feature_Vector_Dim) = newFeatOne; 60 | samples[track_id] = newSampleFeatures; 61 | } else { 62 | if (oldSize < this->budget) { //original space is not enough; 63 | FEATURESS newSampleFeatures(this->budget, Feature_Vector_Dim); 64 | if (addSize >= this->budget) { 65 | newSampleFeatures = newFeatOne.block(0, 0, this->budget, Feature_Vector_Dim); 66 | } else { 67 | newSampleFeatures.block(0, 0, this->budget - addSize, Feature_Vector_Dim) = 68 | samples[track_id].block(addSize - 1, 0, this->budget - addSize, Feature_Vector_Dim).eval(); 69 | newSampleFeatures.block(this->budget - addSize, 0, addSize, Feature_Vector_Dim) = newFeatOne; 70 | } 71 | samples[track_id] = newSampleFeatures; 72 | } else { //original space is ok; 73 | if (addSize >= this->budget) { 74 | samples[track_id] = newFeatOne.block(0, 0, this->budget, Feature_Vector_Dim); 75 | } else { 76 | samples[track_id].block(0, 0, this->budget - addSize, Feature_Vector_Dim) = 77 | samples[track_id].block(addSize - 1, 0, this->budget - addSize, Feature_Vector_Dim).eval(); 78 | samples[track_id].block(this->budget - addSize, 0, addSize, Feature_Vector_Dim) = newFeatOne; 79 | } 80 | } 81 | } 82 | } else { //not exit, create new one; 83 | samples[track_id] = newFeatOne; 84 | } 85 | } //add features; 86 | 87 | //erase the samples which not in active_targets; 88 | for (std::map < int, FEATURESS >::iterator i = samples.begin(); i != samples.end();) { 89 | bool flag = false; 90 | for (int j:active_targets) if (j == i->first) { flag = true; break; } 91 | if (flag == false)samples.erase(i++); 92 | else i++; 93 | } 94 | } 95 | 96 | Eigen::VectorXf 97 | NearNeighborDisMetric::_nncosine_distance( 98 | const FEATURESS & x, const FEATURESS & y) 99 | { 100 | MatrixXf distances = _cosine_distance(x, y); 101 | VectorXf res = distances.colwise().minCoeff().transpose(); 102 | return res; 103 | } 104 | 105 | Eigen::VectorXf 106 | NearNeighborDisMetric::_nneuclidean_distance( 107 | const FEATURESS & x, const FEATURESS & y) 108 | { 109 | MatrixXf distances = _pdist(x, y); 110 | VectorXf res = distances.colwise().maxCoeff().transpose(); 111 | res = res.array().max(VectorXf::Zero(res.rows()).array()); 112 | return res; 113 | } 114 | 115 | Eigen::MatrixXf 116 | NearNeighborDisMetric::_pdist(const FEATURESS & x, const FEATURESS & y) 117 | { 118 | int len1 = x.rows(), len2 = y.rows(); 119 | if (len1 == 0 || len2 == 0) { 120 | return Eigen::MatrixXf::Zero(len1, len2); 121 | } 122 | MatrixXf res = -2.0 * x * y.transpose(); 123 | res = res.colwise() + x.rowwise().squaredNorm(); 124 | res = res.rowwise() + y.rowwise().squaredNorm().transpose(); 125 | res = res.array().max(MatrixXf::Zero(res.rows(), res.cols()).array()); 126 | return res; 127 | } 128 | 129 | Eigen::MatrixXf 130 | NearNeighborDisMetric::_cosine_distance( 131 | const FEATURESS & a, const FEATURESS & b, bool data_is_normalized) 132 | { 133 | FEATURESS aa = a; 134 | FEATURESS bb = b; 135 | if (!data_is_normalized) { 136 | //undo: 137 | for (int i = 0; i < a.rows(); ++i) { 138 | aa.row(i) = a.row(i) / sqrt(a.row(i).squaredNorm()); 139 | } 140 | for (int i = 0; i < b.rows(); ++i) { 141 | bb.row(i) = b.row(i) / sqrt(b.row(i).squaredNorm()); 142 | } 143 | } 144 | MatrixXf res = 1. - (aa * bb.transpose()).array(); 145 | return res; 146 | } 147 | -------------------------------------------------------------------------------- /src/track.cpp: -------------------------------------------------------------------------------- 1 | #include "track.h" 2 | #include 3 | 4 | Track::Track(KAL_MEAN & mean, KAL_COVA & covariance, int track_id, int n_init, int max_age, const FEATURE & feature) 5 | { 6 | this->mean = mean; 7 | this->covariance = covariance; 8 | this->track_id = track_id; 9 | this->hits = 1; 10 | this->age = 1; 11 | this->time_since_update = 0; 12 | this->state = TrackState::Tentative; 13 | features = FEATURESS(1, Feature_Vector_Dim); 14 | features.row(0) = feature; //features.rows() must = 0; 15 | 16 | this->_n_init = n_init; 17 | this->_max_age = max_age; 18 | } 19 | 20 | Track::Track(KAL_MEAN & mean, KAL_COVA & covariance, int track_id, int n_init, int max_age, const FEATURE & feature, int cls, float conf) 21 | { 22 | this->mean = mean; 23 | this->covariance = covariance; 24 | this->track_id = track_id; 25 | this->hits = 1; 26 | this->age = 1; 27 | this->time_since_update = 0; 28 | this->state = TrackState::Tentative; 29 | features = FEATURESS(1, Feature_Vector_Dim); 30 | features.row(0) = feature; //features.rows() must = 0; 31 | 32 | this->_n_init = n_init; 33 | this->_max_age = max_age; 34 | 35 | this->cls = cls; 36 | this->conf = conf; 37 | } 38 | 39 | void Track::predit(KalmanFilter * kf) 40 | { 41 | /*Propagate the state distribution to the current time step using a 42 | Kalman filter prediction step. 43 | 44 | Parameters 45 | ---------- 46 | kf : kalman_filter.KalmanFilterd 47 | The Kalman filter. 48 | */ 49 | 50 | kf->predict(this->mean, this->covariance); 51 | 52 | 53 | this->age += 1; 54 | this->time_since_update += 1; 55 | } 56 | 57 | void Track::update(KalmanFilter * const kf, const DETECTION_ROW & detection) 58 | { 59 | KAL_DATA pa = kf->update(this->mean, this->covariance, detection.to_xyah()); 60 | this->mean = pa.first; 61 | this->covariance = pa.second; 62 | 63 | featuresAppendOne(detection.feature); 64 | // this->features.row(features.rows()) = detection.feature; 65 | this->hits += 1; 66 | this->time_since_update = 0; 67 | if (this->state == TrackState::Tentative && this->hits >= this->_n_init) { 68 | this->state = TrackState::Confirmed; 69 | } 70 | } 71 | 72 | void Track::update(KalmanFilter * const kf, const DETECTION_ROW & detection, CLSCONF pair_det) 73 | { 74 | KAL_DATA pa = kf->update(this->mean, this->covariance, detection.to_xyah()); 75 | this->mean = pa.first; 76 | this->covariance = pa.second; 77 | 78 | featuresAppendOne(detection.feature); 79 | // this->features.row(features.rows()) = detection.feature; 80 | this->hits += 1; 81 | this->time_since_update = 0; 82 | if (this->state == TrackState::Tentative && this->hits >= this->_n_init) { 83 | this->state = TrackState::Confirmed; 84 | } 85 | this->cls = pair_det.cls; 86 | this->conf = pair_det.conf; 87 | } 88 | 89 | void Track::mark_missed() 90 | { 91 | if (this->state == TrackState::Tentative) { 92 | this->state = TrackState::Deleted; 93 | } else if (this->time_since_update > this->_max_age) { 94 | this->state = TrackState::Deleted; 95 | } 96 | } 97 | 98 | bool Track::is_confirmed() 99 | { 100 | return this->state == TrackState::Confirmed; 101 | } 102 | 103 | bool Track::is_deleted() 104 | { 105 | return this->state == TrackState::Deleted; 106 | } 107 | 108 | bool Track::is_tentative() 109 | { 110 | return this->state == TrackState::Tentative; 111 | } 112 | 113 | DETECTBOX Track::to_tlwh() 114 | { 115 | DETECTBOX ret = mean.leftCols(4); 116 | ret(2) *= ret(3); 117 | ret.leftCols(2) -= (ret.rightCols(2) / 2); 118 | return ret; 119 | } 120 | 121 | void Track::featuresAppendOne(const FEATURE & f) 122 | { 123 | int size = this->features.rows(); 124 | FEATURESS newfeatures = FEATURESS(size + 1, Feature_Vector_Dim); 125 | newfeatures.block(0, 0, size, Feature_Vector_Dim) = this->features; 126 | newfeatures.row(size) = f; 127 | features = newfeatures; 128 | } 129 | -------------------------------------------------------------------------------- /src/tracker.cpp: -------------------------------------------------------------------------------- 1 | #include "tracker.h" 2 | #include "nn_matching.h" 3 | #include "linear_assignment.h" 4 | using namespace std; 5 | 6 | #define MY_inner_DEBUG 7 | #ifdef MY_inner_DEBUG 8 | #include 9 | #include 10 | #endif 11 | using namespace std; 12 | tracker::tracker( /*NearNeighborDisMetric *metric, */ 13 | float max_cosine_distance, int nn_budget, 14 | float max_iou_distance, int max_age, int n_init) 15 | { 16 | this->metric = new NearNeighborDisMetric( 17 | NearNeighborDisMetric::METRIC_TYPE::cosine, 18 | max_cosine_distance, nn_budget); 19 | this->max_iou_distance = max_iou_distance; 20 | this->max_age = max_age; 21 | this->n_init = n_init; 22 | 23 | this->kf = new KalmanFilter(); 24 | this->tracks.clear(); 25 | this->_next_idx = 1; 26 | } 27 | 28 | void tracker::predict() 29 | { 30 | for (Track & track:tracks) { 31 | track.predit(kf); 32 | } 33 | } 34 | 35 | void tracker::update(const DETECTIONS & detections) 36 | { 37 | TRACHER_MATCHD res; 38 | _match(detections, res); 39 | 40 | vector < MATCH_DATA > &matches = res.matches; 41 | for (MATCH_DATA & data:matches) { 42 | int track_idx = data.first; 43 | int detection_idx = data.second; 44 | tracks[track_idx].update(this->kf, detections[detection_idx]); 45 | } 46 | vector < int >&unmatched_tracks = res.unmatched_tracks; 47 | for (int &track_idx:unmatched_tracks) { 48 | this->tracks[track_idx].mark_missed(); 49 | } 50 | vector < int >&unmatched_detections = res.unmatched_detections; 51 | for (int &detection_idx:unmatched_detections) { 52 | this->_initiate_track(detections[detection_idx]); 53 | } 54 | vector < Track >::iterator it; 55 | for (it = tracks.begin(); it != tracks.end();) { 56 | if ((*it).is_deleted()) it = tracks.erase(it); 57 | else ++it; 58 | } 59 | vector < int >active_targets; 60 | vector < TRACKER_DATA > tid_features; 61 | for (Track & track:tracks) { 62 | if (track.is_confirmed() == false) continue; 63 | active_targets.push_back(track.track_id); 64 | tid_features.push_back(std::make_pair(track. track_id, track.features)); 65 | FEATURESS t = FEATURESS(0, Feature_Vector_Dim); 66 | track.features = t; 67 | } 68 | this->metric->partial_fit(tid_features, active_targets); 69 | } 70 | 71 | void tracker::update(const DETECTIONSV2 & detectionsv2) 72 | { 73 | const vector& clsConf = detectionsv2.first; 74 | const DETECTIONS& detections = detectionsv2.second; 75 | TRACHER_MATCHD res; 76 | _match(detections, res); 77 | 78 | vector < MATCH_DATA > &matches = res.matches; 79 | for (MATCH_DATA & data:matches) { 80 | int track_idx = data.first; 81 | int detection_idx = data.second; 82 | tracks[track_idx].update(this->kf, detections[detection_idx], clsConf[detection_idx]); 83 | } 84 | vector < int >&unmatched_tracks = res.unmatched_tracks; 85 | for (int &track_idx:unmatched_tracks) { 86 | this->tracks[track_idx].mark_missed(); 87 | } 88 | vector < int >&unmatched_detections = res.unmatched_detections; 89 | for (int &detection_idx:unmatched_detections) { 90 | this->_initiate_track(detections[detection_idx], clsConf[detection_idx]); 91 | } 92 | vector < Track >::iterator it; 93 | for (it = tracks.begin(); it != tracks.end();) { 94 | if ((*it).is_deleted()) it = tracks.erase(it); 95 | else ++it; 96 | } 97 | vector < int >active_targets; 98 | vector < TRACKER_DATA > tid_features; 99 | for (Track & track:tracks) { 100 | if (track.is_confirmed() == false) continue; 101 | active_targets.push_back(track.track_id); 102 | tid_features.push_back(std::make_pair(track. track_id, track.features)); 103 | FEATURESS t = FEATURESS(0, Feature_Vector_Dim); 104 | track.features = t; 105 | } 106 | this->metric->partial_fit(tid_features, active_targets); 107 | } 108 | 109 | void tracker::_match(const DETECTIONS & detections, TRACHER_MATCHD & res) 110 | { 111 | vector < int >confirmed_tracks; 112 | vector < int >unconfirmed_tracks; 113 | int idx = 0; 114 | for (Track & t:tracks) { 115 | if (t.is_confirmed()) confirmed_tracks.push_back(idx); 116 | else unconfirmed_tracks.push_back(idx); 117 | idx++; 118 | } 119 | 120 | TRACHER_MATCHD matcha = linear_assignment::getInstance()-> matching_cascade( 121 | this, &tracker::gated_matric, 122 | this->metric->mating_threshold, 123 | this->max_age, 124 | this->tracks, 125 | detections, 126 | confirmed_tracks); 127 | vector < int >iou_track_candidates; 128 | iou_track_candidates.assign(unconfirmed_tracks.begin(), unconfirmed_tracks.end()); 129 | vector < int >::iterator it; 130 | for (it = matcha.unmatched_tracks.begin(); it != matcha.unmatched_tracks.end();) { 131 | int idx = *it; 132 | if (tracks[idx].time_since_update == 1) { //push into unconfirmed 133 | iou_track_candidates.push_back(idx); 134 | it = matcha.unmatched_tracks.erase(it); 135 | continue; 136 | } 137 | ++it; 138 | } 139 | TRACHER_MATCHD matchb = linear_assignment::getInstance()->min_cost_matching( 140 | this, &tracker::iou_cost, 141 | this->max_iou_distance, 142 | this->tracks, 143 | detections, 144 | iou_track_candidates, 145 | matcha.unmatched_detections); 146 | //get result: 147 | res.matches.assign(matcha.matches.begin(), matcha.matches.end()); 148 | res.matches.insert(res.matches.end(), matchb.matches.begin(), matchb.matches.end()); 149 | //unmatched_tracks; 150 | res.unmatched_tracks.assign( 151 | matcha.unmatched_tracks.begin(), 152 | matcha.unmatched_tracks.end()); 153 | res.unmatched_tracks.insert( 154 | res.unmatched_tracks.end(), 155 | matchb.unmatched_tracks.begin(), 156 | matchb.unmatched_tracks.end()); 157 | res.unmatched_detections.assign( 158 | matchb.unmatched_detections.begin(), 159 | matchb.unmatched_detections.end()); 160 | } 161 | 162 | void tracker::_initiate_track(const DETECTION_ROW & detection) 163 | { 164 | KAL_DATA data = kf->initiate(detection.to_xyah()); 165 | KAL_MEAN mean = data.first; 166 | KAL_COVA covariance = data.second; 167 | 168 | this->tracks.push_back(Track(mean, covariance, this->_next_idx, this->n_init, 169 | this->max_age, detection.feature)); 170 | _next_idx += 1; 171 | } 172 | void tracker::_initiate_track(const DETECTION_ROW& detection, CLSCONF clsConf) 173 | { 174 | KAL_DATA data = kf->initiate(detection.to_xyah()); 175 | KAL_MEAN mean = data.first; 176 | KAL_COVA covariance = data.second; 177 | 178 | this->tracks.push_back(Track(mean, covariance, this->_next_idx, this->n_init, 179 | this->max_age, detection.feature, clsConf.cls, clsConf.conf)); 180 | _next_idx += 1; 181 | } 182 | 183 | DYNAMICM tracker::gated_matric( 184 | std::vector < Track > &tracks, 185 | const DETECTIONS & dets, 186 | const std::vector < int >&track_indices, 187 | const std::vector < int >&detection_indices) 188 | { 189 | FEATURESS features(detection_indices.size(), 512); 190 | int pos = 0; 191 | for (int i:detection_indices) { 192 | features.row(pos++) = dets[i].feature; 193 | } 194 | vector < int >targets; 195 | for (int i:track_indices) { 196 | targets.push_back(tracks[i].track_id); 197 | } 198 | DYNAMICM cost_matrix = this->metric->distance(features, targets); 199 | DYNAMICM res = linear_assignment::getInstance()->gate_cost_matrix( 200 | this->kf, cost_matrix, tracks, dets, track_indices, 201 | detection_indices); 202 | return res; 203 | } 204 | 205 | DYNAMICM 206 | tracker::iou_cost( 207 | std::vector < Track > &tracks, 208 | const DETECTIONS & dets, 209 | const std::vector < int >&track_indices, 210 | const std::vector < int >&detection_indices) 211 | { 212 | //!!!python diff: track_indices && detection_indices will never be None. 213 | // if(track_indices.empty() == true) { 214 | // for(size_t i = 0; i < tracks.size(); i++) { 215 | // track_indices.push_back(i); 216 | // } 217 | // } 218 | // if(detection_indices.empty() == true) { 219 | // for(size_t i = 0; i < dets.size(); i++) { 220 | // detection_indices.push_back(i); 221 | // } 222 | // } 223 | int rows = track_indices.size(); 224 | int cols = detection_indices.size(); 225 | DYNAMICM cost_matrix = Eigen::MatrixXf::Zero(rows, cols); 226 | for (int i = 0; i < rows; i++) { 227 | int track_idx = track_indices[i]; 228 | if (tracks[track_idx].time_since_update > 1) { 229 | cost_matrix.row(i) = Eigen::RowVectorXf::Constant(cols, INFTY_COST); 230 | continue; 231 | } 232 | DETECTBOX bbox = tracks[track_idx].to_tlwh(); 233 | int csize = detection_indices.size(); 234 | DETECTBOXSS candidates(csize, 4); 235 | for (int k = 0; k < csize; k++) candidates.row(k) = dets[detection_indices[k]].tlwh; 236 | Eigen::RowVectorXf rowV = (1. - iou(bbox, candidates).array()).matrix().transpose(); 237 | cost_matrix.row(i) = rowV; 238 | } 239 | return cost_matrix; 240 | } 241 | 242 | Eigen::VectorXf 243 | tracker::iou(DETECTBOX & bbox, DETECTBOXSS & candidates) 244 | { 245 | float bbox_tl_1 = bbox[0]; 246 | float bbox_tl_2 = bbox[1]; 247 | float bbox_br_1 = bbox[0] + bbox[2]; 248 | float bbox_br_2 = bbox[1] + bbox[3]; 249 | float area_bbox = bbox[2] * bbox[3]; 250 | 251 | Eigen::Matrix < float, -1, 2 > candidates_tl; 252 | Eigen::Matrix < float, -1, 2 > candidates_br; 253 | candidates_tl = candidates.leftCols(2); 254 | candidates_br = candidates.rightCols(2) + candidates_tl; 255 | 256 | int size = int (candidates.rows()); 257 | // Eigen::VectorXf area_intersection(size); 258 | // Eigen::VectorXf area_candidates(size); 259 | Eigen::VectorXf res(size); 260 | for (int i = 0; i < size; i++) { 261 | float tl_1 = std::max(bbox_tl_1, candidates_tl(i, 0)); 262 | float tl_2 = std::max(bbox_tl_2, candidates_tl(i, 1)); 263 | float br_1 = std::min(bbox_br_1, candidates_br(i, 0)); 264 | float br_2 = std::min(bbox_br_2, candidates_br(i, 1)); 265 | 266 | float w = br_1 - tl_1; w = (w < 0 ? 0 : w); 267 | float h = br_2 - tl_2; h = (h < 0 ? 0 : h); 268 | float area_intersection = w * h; 269 | float area_candidates = candidates(i, 2) * candidates(i, 3); 270 | res[i] = area_intersection / (area_bbox + area_candidates - area_intersection); 271 | } 272 | return res; 273 | } 274 | --------------------------------------------------------------------------------