diff --git a/deploy/DeepStream/CMakeLists.txt b/deploy/DeepStream/CMakeLists.txt new file mode 100644 index 00000000..866c695f --- /dev/null +++ b/deploy/DeepStream/CMakeLists.txt @@ -0,0 +1,25 @@ +set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) +cmake_minimum_required(VERSION 3.16) +set(CMAKE_CUDA_COMPILER /usr/local/cuda/bin/nvcc) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/lib) +set(DS_ROOT_DIR /opt/nvidia/deepstream/deepstream) +set(DS_LIBS_DIR ${DS_ROOT_DIR}/lib) +set(DS_INCLUDES_DIR ${DS_ROOT_DIR}/sources/includes) + +find_package(GLib) +find_package(OpenCV) + +include_directories(${GLIB2_INCLUDE_DIR}) +include_directories(${OpenCV_INCLUDE_DIRS}) +include_directories(${DS_INCLUDES_DIR}) +include_directories(${CMAKE_SOURCE_DIR}/includes) + +file(GLOB TRACK ${CMAKE_SOURCE_DIR}/src/*.cpp) + +set(CMAKE_CXX_FLAGS "-Wall -Wextra -fPIC") +set(CMAKE_CUDA_FLAGS "--compiler-options -fPIC") + +link_directories(${DS_LIBS_DIR}) +add_library(ByteTracker SHARED ${TRACK}) +target_link_libraries(ByteTracker nvds_meta) \ No newline at end of file diff --git a/deploy/DeepStream/README.md b/deploy/DeepStream/README.md new file mode 100644 index 00000000..325f57e9 --- /dev/null +++ b/deploy/DeepStream/README.md @@ -0,0 +1,17 @@ +# Byte-Track Integration with Deepstream +Integrating Byte-Track C++ code with the Deepstream-5.1 + +## Build Instructions +``` +$mkdir build && cd build + +$cmake .. + +$make ByteTracker +``` + +This will create lib/libByteTracker.so file which can be passed as the custom low level tracker library to deepstream. + +## References +1. [How to Implement a Custom Low-Level Tracker Library in Deepstream](https://docs.nvidia.com/metropolis/deepstream/dev-guide/text/DS_plugin_gst-nvtracker.html#how-to-implement-a-custom-low-level-tracker-library) +2. [Byte-Track](https://github.com/ifzhang/ByteTrack) diff --git a/deploy/DeepStream/cmake/FindGLib.cmake b/deploy/DeepStream/cmake/FindGLib.cmake new file mode 100644 index 00000000..d6f424aa --- /dev/null +++ b/deploy/DeepStream/cmake/FindGLib.cmake @@ -0,0 +1,66 @@ +# - Try to find the GLIB2 libraries +# Once done this will define +# +# GLIB2_FOUND - system has glib2 +# GLIB2_INCLUDE_DIR - the glib2 include directory +# GLIB2_LIBRARIES - glib2 library + +# Copyright (c) 2008 Laurent Montel, +# +# Redistribution and use is allowed according to the terms of the BSD license. +# For details see the accompanying COPYING-CMAKE-SCRIPTS file. + + +if (GLIB2_INCLUDE_DIR AND GLIB2_LIBRARIES) + # Already in cache, be silent + set(GLIB2_FIND_QUIETLY TRUE) +endif (GLIB2_INCLUDE_DIR AND GLIB2_LIBRARIES) + +if (NOT WIN32) + find_package(PkgConfig QUIET) + if (PKG_CONFIG_FOUND) + pkg_check_modules(PKG_GLIB QUIET glib-2.0) + endif () +endif (NOT WIN32) + +find_path(GLIB2_MAIN_INCLUDE_DIR glib.h + PATH_SUFFIXES glib-2.0 + HINTS ${PKG_GLIB_INCLUDE_DIRS} ${PKG_GLIB_INCLUDEDIR}) + +# search the glibconfig.h include dir under the same root where the library is found +find_library(GLIB2_LIBRARIES + NAMES glib-2.0 + HINTS ${PKG_GLIB_LIBRARY_DIRS} ${PKG_GLIB_LIBDIR}) + +find_path(GLIB2_INTERNAL_INCLUDE_DIR glibconfig.h + PATH_SUFFIXES glib-2.0/include ../lib/glib-2.0/include + HINTS ${PKG_GLIB_INCLUDE_DIRS} ${PKG_GLIB_LIBRARIES} ${CMAKE_SYSTEM_LIBRARY_PATH}) + +set(GLIB2_INCLUDE_DIR ${GLIB2_MAIN_INCLUDE_DIR}) + +# not sure if this include dir is optional or required +# for now it is optional +if (GLIB2_INTERNAL_INCLUDE_DIR) + set(GLIB2_INCLUDE_DIR ${GLIB2_INCLUDE_DIR} ${GLIB2_INTERNAL_INCLUDE_DIR}) +endif (GLIB2_INTERNAL_INCLUDE_DIR) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(GLIB2 DEFAULT_MSG GLIB2_LIBRARIES GLIB2_MAIN_INCLUDE_DIR) + +mark_as_advanced(GLIB2_INCLUDE_DIR GLIB2_LIBRARIES) + + +find_program(GLIB2_GENMARSHAL_UTIL glib-genmarshal) + +macro(glib2_genmarshal output_name) + file(REMOVE ${CMAKE_CURRENT_BINARY_DIR}/genmarshal_tmp) + foreach (_declaration ${ARGN}) + file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/genmarshal_tmp "${_declaration}\n") + endforeach () + add_custom_command( + OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/${output_name}.h ${CMAKE_CURRENT_BINARY_DIR}/${output_name}.c + COMMAND ${GLIB2_GENMARSHAL_UTIL} --header genmarshal_tmp > ${output_name}.h + COMMAND ${GLIB2_GENMARSHAL_UTIL} --body genmarshal_tmp > ${output_name}.c + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} + ) +endmacro() \ No newline at end of file diff --git a/deploy/DeepStream/includes/BYTETracker.h b/deploy/DeepStream/includes/BYTETracker.h new file mode 100644 index 00000000..7fa0f58f --- /dev/null +++ b/deploy/DeepStream/includes/BYTETracker.h @@ -0,0 +1,61 @@ +#pragma once + +#include "STrack.h" +#include "BYTETracker.h" +#include "nvdstracker.h" +#include +#include "DataType.h" +#include + +class NvObject { +public: + float rect[4]; + int label; + float prob; + NvMOTObjToTrack *associatedObjectIn; +}; + +class BYTETracker { +public: + BYTETracker(int frame_rate = 30, int track_buffer = 30); + + ~BYTETracker(); + + vector update(const vector &nvObjects); + +private: + vector joint_stracks(vector &tlista, vector &tlistb); + + vector joint_stracks(vector &tlista, vector &tlistb); + + vector sub_stracks(vector &tlista, vector &tlistb); + + void remove_duplicate_stracks(vector &resa, vector &resb, vector &stracksa, + vector &stracksb); + + void linear_assignment(vector > &cost_matrix, int cost_matrix_size, int cost_matrix_size_size, + float thresh, + vector > &matches, vector &unmatched_a, vector &unmatched_b); + + vector>iou_distance(vector &atracks, vector &btracks, int &dist_size, int &dist_size_size); + + vector> iou_distance(vector &atracks, vector &btracks); + + vector> ious(vector > &atlbrs, vector > &btlbrs); + + double lapjv(const vector > &cost, vector &rowsol, vector &colsol, + bool extend_cost = false, float cost_limit = LONG_MAX, bool return_cost = true); + +private: + + float track_thresh; + float high_thresh; + float match_thresh; + int frame_id; + int max_time_lost; + + vector tracked_stracks; + vector lost_stracks; + vector removed_stracks; + byte_kalman::KalmanFilter kalman_filter; +}; \ No newline at end of file diff --git a/deploy/DeepStream/includes/DataType.h b/deploy/DeepStream/includes/DataType.h new file mode 100644 index 00000000..0b057d26 --- /dev/null +++ b/deploy/DeepStream/includes/DataType.h @@ -0,0 +1,37 @@ +#pragma once + +#include +#include + +#include +#include + +typedef Eigen::Matrix DETECTBOX; +typedef Eigen::Matrix DETECTBOXSS; +typedef Eigen::Matrix FEATURE; +typedef Eigen::Matrix FEATURESS; +//typedef std::vector FEATURESS; + +//Kalmanfilter +//typedef Eigen::Matrix KAL_FILTER; +typedef Eigen::Matrix KAL_MEAN; +typedef Eigen::Matrix KAL_COVA; +typedef Eigen::Matrix KAL_HMEAN; +typedef Eigen::Matrix KAL_HCOVA; +using KAL_DATA = std::pair; +using KAL_HDATA = std::pair; + +//main +using RESULT_DATA = std::pair; + +//tracker: +using TRACKER_DATA = std::pair; +using MATCH_DATA = std::pair; +typedef struct t { + std::vector matches; + std::vector unmatched_tracks; + std::vector unmatched_detections; +} TRACHER_MATCHD; + +//linear_assignment: +typedef Eigen::Matrix DYNAMICM; diff --git a/deploy/DeepStream/includes/KalmanFilter.h b/deploy/DeepStream/includes/KalmanFilter.h new file mode 100644 index 00000000..86718787 --- /dev/null +++ b/deploy/DeepStream/includes/KalmanFilter.h @@ -0,0 +1,34 @@ +#pragma once + +#include "DataType.h" + +namespace byte_kalman { + class KalmanFilter { + public: + static const double chi2inv95[10]; + + KalmanFilter(); + + KAL_DATA initiate(const DETECTBOX &measurement); + + void predict(KAL_MEAN &mean, KAL_COVA &covariance); + + KAL_HDATA project(const KAL_MEAN &mean, const KAL_COVA &covariance); + + KAL_DATA update(const KAL_MEAN &mean, + const KAL_COVA &covariance, + const DETECTBOX &measurement); + + Eigen::Matrix gating_distance( + const KAL_MEAN &mean, + const KAL_COVA &covariance, + const std::vector &measurements, + bool only_position = false); + + private: + Eigen::Matrix _motion_mat; + Eigen::Matrix _update_mat; + float _std_weight_position; + float _std_weight_velocity; + }; +} diff --git a/deploy/DeepStream/includes/Lapjv.h b/deploy/DeepStream/includes/Lapjv.h new file mode 100644 index 00000000..df897cf6 --- /dev/null +++ b/deploy/DeepStream/includes/Lapjv.h @@ -0,0 +1,65 @@ +#ifndef LAPJV_H +#define LAPJV_H + +#define LARGE 1000000 + +#if !defined TRUE +#define TRUE 1 +#endif +#if !defined FALSE +#define FALSE 0 +#endif + +#define NEW(x, t, n) if ((x = (t *)malloc(sizeof(t) * (n))) == 0) { return -1; } +#define FREE(x) if (x != 0) { free(x); x = 0; } +#define SWAP_INDICES(a, b) { int_t _temp_index = a; a = b; b = _temp_index; } + +#if 0 +#include +#define ASSERT(cond) assert(cond) +#define PRINTF(fmt, ...) printf(fmt, ##__VA_ARGS__) +#define PRINT_COST_ARRAY(a, n) \ + while (1) { \ + printf(#a" = ["); \ + if ((n) > 0) { \ + printf("%f", (a)[0]); \ + for (uint_t j = 1; j < n; j++) { \ + printf(", %f", (a)[j]); \ + } \ + } \ + printf("]\n"); \ + break; \ + } +#define PRINT_INDEX_ARRAY(a, n) \ + while (1) { \ + printf(#a" = ["); \ + if ((n) > 0) { \ + printf("%d", (a)[0]); \ + for (uint_t j = 1; j < n; j++) { \ + printf(", %d", (a)[j]); \ + } \ + } \ + printf("]\n"); \ + break; \ + } +#else +#define ASSERT(cond) +#define PRINTF(fmt, ...) +#define PRINT_COST_ARRAY(a, n) +#define PRINT_INDEX_ARRAY(a, n) +#endif + + +typedef signed int int_t; +typedef unsigned int uint_t; +typedef double cost_t; +typedef char boolean; +typedef enum fp_t { + FP_1 = 1, FP_2 = 2, FP_DYNAMIC = 3 +} fp_t; + +extern int_t lapjv_internal( + const uint_t n, cost_t *cost[], + int_t *x, int_t *y); + +#endif // LAPJV_H diff --git a/deploy/DeepStream/includes/STrack.h b/deploy/DeepStream/includes/STrack.h new file mode 100644 index 00000000..41c3ecc7 --- /dev/null +++ b/deploy/DeepStream/includes/STrack.h @@ -0,0 +1,63 @@ +#pragma once +#include +#include "KalmanFilter.h" + +using namespace std; + +enum TrackState { + New = 0, Tracked, Lost, Removed +}; + +class STrack { +public: + STrack(vector tlwh_, float score, int label, NvMOTObjToTrack *associatedObjectIn); + + ~STrack(); + + vector static tlbr_to_tlwh(vector &tlbr); + + void static multi_predict(vector &stracks, byte_kalman::KalmanFilter &kalman_filter); + + void static_tlwh(); + + void static_tlbr(); + + vector tlwh_to_xyah(vector tlwh_tmp); + + vector to_xyah(); + + void mark_lost(); + + void mark_removed(); + + int next_id(); + + int end_frame(); + + void activate(byte_kalman::KalmanFilter &kalman_filter, int frame_id); + + void re_activate(STrack &new_track, int frame_id, bool new_id = false); + + void update(STrack &new_track, int frame_id); + +public: + bool is_activated; + int track_id; + int state; + + vector original_tlwh; + vector _tlwh; + vector tlwh; + vector tlbr; + int frame_id; + int tracklet_len; + int start_frame; + int label; + KAL_MEAN mean; + KAL_COVA covariance; + float score; + NvMOTObjToTrack *associatedObjectIn; + +private: + byte_kalman::KalmanFilter kalman_filter; +}; diff --git a/deploy/DeepStream/includes/Tracker.h b/deploy/DeepStream/includes/Tracker.h new file mode 100644 index 00000000..a4460746 --- /dev/null +++ b/deploy/DeepStream/includes/Tracker.h @@ -0,0 +1,65 @@ +// +// Created by Mayur Kulkarni on 11/11/21. +// + +#ifndef DNSTARPROD_TRACKER_H +#define DNSTARPROD_TRACKER_H + +#include "nvdstracker.h" +#include "BYTETracker.h" +#include + +/** +* @brief Context for input video streams +* +* The stream context holds all necessary state to perform multi-object tracking +* within the stream. +* +*/ +class NvMOTContext { +public: + NvMOTContext(const NvMOTConfig &configIn, NvMOTConfigResponse &configResponse); + + ~NvMOTContext() {}; + + /** + * @brief Process a batch of frames + * + * Internal implementation of NvMOT_Process() + * + * @param [in] pParam Pointer to parameters for the frame to be processed + * @param [out] pTrackedObjectsBatch Pointer to object tracks output + */ + NvMOTStatus processFrame(const NvMOTProcessParams *params, + NvMOTTrackedObjBatch *pTrackedObjectsBatch); + + /** + * @brief Output the past-frame data if there are + * + * Internal implementation of NvMOT_ProcessPast() + * + * @param [in] pParam Pointer to parameters for the frame to be processed + * @param [out] pPastFrameObjectsBatch Pointer to past frame object tracks output + */ + NvMOTStatus processFramePast(const NvMOTProcessParams *params, + NvDsPastFrameObjBatch *pPastFrameObjectsBatch); + + /** + * @brief Terminate trackers and release resources for a stream when the stream is removed + * + * Internal implementation of NvMOT_RemoveStreams() + * + * @param [in] streamIdMask removed stream ID + */ + NvMOTStatus removeStream(const NvMOTStreamId streamIdMask); + +protected: + + /** + * Users can include an actual tracker implementation here as a member + * `IMultiObjectTracker` can be assumed to an user-defined interface class + */ + std::map> byteTrackerMap; +}; + +#endif //DNSTARPROD_TRACKER_H diff --git a/deploy/DeepStream/src/BYTETracker.cpp b/deploy/DeepStream/src/BYTETracker.cpp new file mode 100644 index 00000000..5cd94f9f --- /dev/null +++ b/deploy/DeepStream/src/BYTETracker.cpp @@ -0,0 +1,215 @@ +#include "BYTETracker.h" + +BYTETracker::BYTETracker(int frame_rate, int track_buffer) { + track_thresh = 0; + high_thresh = 0.2; + match_thresh = 0.8; + frame_id = 0; + max_time_lost = int(frame_rate / 30.0 * track_buffer); +} + +BYTETracker::~BYTETracker() { +} + +vector BYTETracker::update(const vector &nvObjects) { + + ////////////////// Step 1: Get detections ////////////////// + this->frame_id++; + vector activated_stracks; + vector refind_stracks; + vector removed_stracks; + vector lost_stracks; + vector detections; + vector detections_low; + + vector detections_cp; + vector tracked_stracks_swap; + vector resa, resb; + vector output_stracks; + + vector unconfirmed; + vector tracked_stracks; + vector strack_pool; + vector r_tracked_stracks; + + + if (nvObjects.size() > 0) { + for (int i = 0; i < nvObjects.size(); i++) { + vector tlwh_; + tlwh_.resize(4); + tlwh_[0] = nvObjects[i].rect[0]; + tlwh_[1] = nvObjects[i].rect[1]; + tlwh_[2] = nvObjects[i].rect[2]; + tlwh_[3] = nvObjects[i].rect[3]; + float score = nvObjects[i].prob; + + STrack strack(tlwh_, score, nvObjects[i].label, nvObjects[i].associatedObjectIn); + if (score >= track_thresh) { + detections.push_back(strack); + } else { + detections_low.push_back(strack); + } + } + } + + // Add newly detected tracklets to tracked_stracks + for (int i = 0; i < this->tracked_stracks.size(); i++) { + if (!this->tracked_stracks[i].is_activated) + unconfirmed.push_back(&this->tracked_stracks[i]); + else + tracked_stracks.push_back(&this->tracked_stracks[i]); + } + + + ////////////////// Step 2: First association, with IoU ////////////////// + strack_pool = joint_stracks(tracked_stracks, this->lost_stracks); + STrack::multi_predict(strack_pool, this->kalman_filter); + + vector> dists; + int dist_size = 0, dist_size_size = 0; + dists = iou_distance(strack_pool, detections, dist_size, dist_size_size); + + vector> matches; + vector u_track, u_detection; + linear_assignment(dists, dist_size, dist_size_size, match_thresh, matches, u_track, u_detection); + + for (int i = 0; i < matches.size(); i++) { + STrack *track = strack_pool[matches[i][0]]; + STrack *det = &detections[matches[i][1]]; + if (track->state == TrackState::Tracked) { + track->update(*det, this->frame_id); + activated_stracks.push_back(*track); + } else { + track->re_activate(*det, this->frame_id, false); + refind_stracks.push_back(*track); + } + } + + ////////////////// Step 3: Second association, using low score dets ////////////////// + for (int i = 0; i < u_detection.size(); i++) { + detections_cp.push_back(detections[u_detection[i]]); + } + detections.clear(); + detections.assign(detections_low.begin(), detections_low.end()); + + for (int i = 0; i < u_track.size(); i++) { + if (strack_pool[u_track[i]]->state == TrackState::Tracked) { + r_tracked_stracks.push_back(strack_pool[u_track[i]]); + } + } + + dists.clear(); + dists = iou_distance(r_tracked_stracks, detections, dist_size, dist_size_size); + + matches.clear(); + u_track.clear(); + u_detection.clear(); + linear_assignment(dists, dist_size, dist_size_size, 0.5, matches, u_track, u_detection); + + for (int i = 0; i < matches.size(); i++) { + STrack *track = r_tracked_stracks[matches[i][0]]; + STrack *det = &detections[matches[i][1]]; + if (track->state == TrackState::Tracked) { + track->update(*det, this->frame_id); + activated_stracks.push_back(*track); + } else { + track->re_activate(*det, this->frame_id, false); + refind_stracks.push_back(*track); + } + } + + for (int i = 0; i < u_track.size(); i++) { + STrack *track = r_tracked_stracks[u_track[i]]; + if (track->state != TrackState::Lost) { + track->mark_lost(); + lost_stracks.push_back(*track); + } + } + + // Deal with unconfirmed tracks, usually tracks with only one beginning frame + detections.clear(); + detections.assign(detections_cp.begin(), detections_cp.end()); + + dists.clear(); + dists = iou_distance(unconfirmed, detections, dist_size, dist_size_size); + + matches.clear(); + vector u_unconfirmed; + u_detection.clear(); + linear_assignment(dists, dist_size, dist_size_size, 0.7, matches, u_unconfirmed, u_detection); + + for (int i = 0; i < matches.size(); i++) { + unconfirmed[matches[i][0]]->update(detections[matches[i][1]], this->frame_id); + activated_stracks.push_back(*unconfirmed[matches[i][0]]); + } + + for (int i = 0; i < u_unconfirmed.size(); i++) { + STrack *track = unconfirmed[u_unconfirmed[i]]; + track->mark_removed(); + removed_stracks.push_back(*track); + } + + ////////////////// Step 4: Init new stracks ////////////////// + for (int i = 0; i < u_detection.size(); i++) { + STrack *track = &detections[u_detection[i]]; + if (track->score < this->high_thresh) + continue; + track->activate(this->kalman_filter, this->frame_id); + activated_stracks.push_back(*track); + } + + ////////////////// Step 5: Update state ////////////////// + for (int i = 0; i < this->lost_stracks.size(); i++) { + if (this->frame_id - this->lost_stracks[i].end_frame() > this->max_time_lost) { + this->lost_stracks[i].mark_removed(); + removed_stracks.push_back(this->lost_stracks[i]); + } + } + + for (int i = 0; i < this->tracked_stracks.size(); i++) { + if (this->tracked_stracks[i].state == TrackState::Tracked) { + tracked_stracks_swap.push_back(this->tracked_stracks[i]); + } + } + this->tracked_stracks.clear(); + this->tracked_stracks.assign(tracked_stracks_swap.begin(), tracked_stracks_swap.end()); + + this->tracked_stracks = joint_stracks(this->tracked_stracks, activated_stracks); + this->tracked_stracks = joint_stracks(this->tracked_stracks, refind_stracks); + + //std::cout << activated_stracks.size() << std::endl; + + this->lost_stracks = sub_stracks(this->lost_stracks, this->tracked_stracks); + for (int i = 0; i < lost_stracks.size(); i++) { + this->lost_stracks.push_back(lost_stracks[i]); + } + + this->lost_stracks = sub_stracks(this->lost_stracks, this->removed_stracks); + for (int i = 0; i < removed_stracks.size(); i++) { + this->removed_stracks.push_back(removed_stracks[i]); + } + + remove_duplicate_stracks(resa, resb, this->tracked_stracks, this->lost_stracks); + + this->tracked_stracks.clear(); + this->tracked_stracks.assign(resa.begin(), resa.end()); + this->lost_stracks.clear(); + this->lost_stracks.assign(resb.begin(), resb.end()); + + for (int i = 0; i < this->tracked_stracks.size(); i++) { + if (this->tracked_stracks[i].is_activated) { + output_stracks.push_back(this->tracked_stracks[i]); + } + } + + // clean up old objects + vector filtered_output_stracks; + std::copy_if(output_stracks.begin(), + output_stracks.end(), + std::back_inserter(filtered_output_stracks), + [](STrack track) { + return track.associatedObjectIn != NULL && + track.associatedObjectIn->classId == 0; + }); + return filtered_output_stracks; +} diff --git a/deploy/DeepStream/src/BYTETrackerUtils.cpp b/deploy/DeepStream/src/BYTETrackerUtils.cpp new file mode 100644 index 00000000..58f2ec3c --- /dev/null +++ b/deploy/DeepStream/src/BYTETrackerUtils.cpp @@ -0,0 +1,345 @@ +#include "BYTETracker.h" +#include "Lapjv.h" +#include "STrack.h" + +vector BYTETracker::joint_stracks(vector &tlista, vector &tlistb) { + map exists; + vector < STrack * > res; + for (int i = 0; i < tlista.size(); i++) { + exists.insert(pair(tlista[i]->track_id, 1)); + res.push_back(tlista[i]); + } + for (int i = 0; i < tlistb.size(); i++) { + int tid = tlistb[i].track_id; + if (!exists[tid] || exists.count(tid) == 0) { + exists[tid] = 1; + res.push_back(&tlistb[i]); + } + } + return res; +} + +vector BYTETracker::joint_stracks(vector &tlista, vector &tlistb) { + std::map exists; + vector res; + for (int i = 0; i < tlista.size(); i++) { + exists.insert(pair(tlista[i].track_id, 1)); + res.push_back(tlista[i]); + } + for (int i = 0; i < tlistb.size(); i++) { + int tid = tlistb[i].track_id; + if (!exists[tid] || exists.count(tid) == 0) { + exists[tid] = 1; + res.push_back(tlistb[i]); + } + } + return res; +} + +vector BYTETracker::sub_stracks(vector &tlista, vector &tlistb) { + map stracks; + for (int i = 0; i < tlista.size(); i++) { + stracks.insert(pair(tlista[i].track_id, tlista[i])); + } + for (int i = 0; i < tlistb.size(); i++) { + int tid = tlistb[i].track_id; + if (stracks.count(tid) != 0) { + stracks.erase(tid); + } + } + + vector res; + std::map::iterator it; + for (it = stracks.begin(); it != stracks.end(); ++it) { + res.push_back(it->second); + } + + return res; +} + +void BYTETracker::remove_duplicate_stracks(vector &resa, vector &resb, vector &stracksa, + vector &stracksb) { + vector > pdist = iou_distance(stracksa, stracksb); + vector > pairs; + for (int i = 0; i < pdist.size(); i++) { + for (int j = 0; j < pdist[i].size(); j++) { + if (pdist[i][j] < 0.15) { + pairs.push_back(pair(i, j)); + } + } + } + + vector dupa, dupb; + for (int i = 0; i < pairs.size(); i++) { + int timep = stracksa[pairs[i].first].frame_id - stracksa[pairs[i].first].start_frame; + int timeq = stracksb[pairs[i].second].frame_id - stracksb[pairs[i].second].start_frame; + if (timep > timeq) + dupb.push_back(pairs[i].second); + else + dupa.push_back(pairs[i].first); + } + + for (int i = 0; i < stracksa.size(); i++) { + vector::iterator iter = find(dupa.begin(), dupa.end(), i); + if (iter == dupa.end()) { + resa.push_back(stracksa[i]); + } + } + + for (int i = 0; i < stracksb.size(); i++) { + vector::iterator iter = find(dupb.begin(), dupb.end(), i); + if (iter == dupb.end()) { + resb.push_back(stracksb[i]); + } + } +} + +void +BYTETracker::linear_assignment(vector > &cost_matrix, int cost_matrix_size, int cost_matrix_size_size, + float thresh, + vector > &matches, vector &unmatched_a, vector &unmatched_b) { + if (cost_matrix.size() == 0) { + for (int i = 0; i < cost_matrix_size; i++) { + unmatched_a.push_back(i); + } + for (int i = 0; i < cost_matrix_size_size; i++) { + unmatched_b.push_back(i); + } + return; + } + + vector rowsol; + vector colsol; + float c = lapjv(cost_matrix, rowsol, colsol, true, thresh); + for (int i = 0; i < rowsol.size(); i++) { + if (rowsol[i] >= 0) { + vector match; + match.push_back(i); + match.push_back(rowsol[i]); + matches.push_back(match); + } else { + unmatched_a.push_back(i); + } + } + + for (int i = 0; i < colsol.size(); i++) { + if (colsol[i] < 0) { + unmatched_b.push_back(i); + } + } +} + +vector > BYTETracker::ious(vector > &atlbrs, vector > &btlbrs) { + vector > ious; + if (atlbrs.size() * btlbrs.size() == 0) + return ious; + + ious.resize(atlbrs.size()); + for (int i = 0; i < ious.size(); i++) { + ious[i].resize(btlbrs.size()); + } + + //bbox_ious + for (int k = 0; k < btlbrs.size(); k++) { + vector ious_tmp; + float box_area = (btlbrs[k][2] - btlbrs[k][0] + 1) * (btlbrs[k][3] - btlbrs[k][1] + 1); + for (int n = 0; n < atlbrs.size(); n++) { + float iw = min(atlbrs[n][2], btlbrs[k][2]) - max(atlbrs[n][0], btlbrs[k][0]) + 1; + if (iw > 0) { + float ih = min(atlbrs[n][3], btlbrs[k][3]) - max(atlbrs[n][1], btlbrs[k][1]) + 1; + if (ih > 0) { + float ua = (atlbrs[n][2] - atlbrs[n][0] + 1) * (atlbrs[n][3] - atlbrs[n][1] + 1) + box_area - + iw * ih; + ious[n][k] = iw * ih / ua; + } else { + ious[n][k] = 0.0; + } + } else { + ious[n][k] = 0.0; + } + } + } + + return ious; +} + +vector > +BYTETracker::iou_distance(vector &atracks, vector &btracks, int &dist_size, int &dist_size_size) { + vector > cost_matrix; + if (atracks.size() * btracks.size() == 0) { + dist_size = atracks.size(); + dist_size_size = btracks.size(); + return cost_matrix; + } + vector > atlbrs, btlbrs; + for (int i = 0; i < atracks.size(); i++) { + atlbrs.push_back(atracks[i]->tlbr); + } + for (int i = 0; i < btracks.size(); i++) { + btlbrs.push_back(btracks[i].tlbr); + } + + dist_size = atracks.size(); + dist_size_size = btracks.size(); + + vector > _ious = ious(atlbrs, btlbrs); + + for (int i = 0; i < _ious.size(); i++) { + vector _iou; + for (int j = 0; j < _ious[i].size(); j++) { + _iou.push_back(1 - _ious[i][j]); + } + cost_matrix.push_back(_iou); + } + + return cost_matrix; +} + +vector > BYTETracker::iou_distance(vector &atracks, vector &btracks) { + vector > atlbrs, btlbrs; + for (int i = 0; i < atracks.size(); i++) { + atlbrs.push_back(atracks[i].tlbr); + } + for (int i = 0; i < btracks.size(); i++) { + btlbrs.push_back(btracks[i].tlbr); + } + + vector > _ious = ious(atlbrs, btlbrs); + vector > cost_matrix; + for (int i = 0; i < _ious.size(); i++) { + vector _iou; + for (int j = 0; j < _ious[i].size(); j++) { + _iou.push_back(1 - _ious[i][j]); + } + cost_matrix.push_back(_iou); + } + + return cost_matrix; +} + +double BYTETracker::lapjv(const vector > &cost, vector &rowsol, vector &colsol, + bool extend_cost, float cost_limit, bool return_cost) { + vector > cost_c; + cost_c.assign(cost.begin(), cost.end()); + + vector > cost_c_extended; + + int n_rows = cost.size(); + int n_cols = cost[0].size(); + rowsol.resize(n_rows); + colsol.resize(n_cols); + + int n = 0; + if (n_rows == n_cols) { + n = n_rows; + } else { + if (!extend_cost) { + cout << "set extend_cost=True" << endl; + system("pause"); + exit(0); + } + } + + if (extend_cost || cost_limit < LONG_MAX) { + n = n_rows + n_cols; + cost_c_extended.resize(n); + for (int i = 0; i < cost_c_extended.size(); i++) + cost_c_extended[i].resize(n); + + if (cost_limit < LONG_MAX) { + for (int i = 0; i < cost_c_extended.size(); i++) { + for (int j = 0; j < cost_c_extended[i].size(); j++) { + cost_c_extended[i][j] = cost_limit / 2.0; + } + } + } else { + float cost_max = -1; + + for (int i = 0; i < cost_c.size(); i++) { + for (int j = 0; j < cost_c[i].size(); j++) { + if (cost_c[i][j] > cost_max) + cost_max = cost_c[i][j]; + } + } + for (int i = 0; i < cost_c_extended.size(); i++) { + for (int j = 0; j < cost_c_extended[i].size(); j++) { + cost_c_extended[i][j] = cost_max + 1; + } + } + } + + for (int i = n_rows; i < cost_c_extended.size(); i++) { + for (int j = n_cols; j < cost_c_extended[i].size(); j++) { + cost_c_extended[i][j] = 0; + } + } + for (int i = 0; i < n_rows; i++) { + for (int j = 0; j < n_cols; j++) { + cost_c_extended[i][j] = cost_c[i][j]; + } + } + + cost_c.clear(); + cost_c.assign(cost_c_extended.begin(), cost_c_extended.end()); + } + + double **cost_ptr; + cost_ptr = new double *[sizeof(double *) * n]; + for (int i = 0; i < n; i++) + cost_ptr[i] = new double[sizeof(double) * n]; + + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + cost_ptr[i][j] = cost_c[i][j]; + } + } + + int *x_c = new int[sizeof(int) * n]; + int *y_c = new int[sizeof(int) * n]; + + int ret = lapjv_internal(n, cost_ptr, x_c, y_c); + if (ret != 0) { + std::cout << "Calculate Wrong!" << endl; + system("pause"); + exit(0); + } + + double opt = 0.0; + + if (n != n_rows) { + for (int i = 0; i < n; i++) { + if (x_c[i] >= n_cols) + x_c[i] = -1; + if (y_c[i] >= n_rows) + y_c[i] = -1; + } + for (int i = 0; i < n_rows; i++) { + rowsol[i] = x_c[i]; + } + for (int i = 0; i < n_cols; i++) { + colsol[i] = y_c[i]; + } + + if (return_cost) { + for (int i = 0; i < rowsol.size(); i++) { + if (rowsol[i] != -1) { + //cout << i << "\t" << rowsol[i] << "\t" << cost_ptr[i][rowsol[i]] << endl; + opt += cost_ptr[i][rowsol[i]]; + } + } + } + } else if (return_cost) { + for (int i = 0; i < rowsol.size(); i++) { + opt += cost_ptr[i][rowsol[i]]; + } + } + + for (int i = 0; i < n; i++) { + delete[]cost_ptr[i]; + } + delete[]cost_ptr; + delete[]x_c; + delete[]y_c; + + return opt; +} \ No newline at end of file diff --git a/deploy/DeepStream/src/KalmanFilter.cpp b/deploy/DeepStream/src/KalmanFilter.cpp new file mode 100644 index 00000000..ba5eb2e6 --- /dev/null +++ b/deploy/DeepStream/src/KalmanFilter.cpp @@ -0,0 +1,148 @@ +#include "KalmanFilter.h" +#include + +namespace byte_kalman { + const double KalmanFilter::chi2inv95[10] = { + 0, + 3.8415, + 5.9915, + 7.8147, + 9.4877, + 11.070, + 12.592, + 14.067, + 15.507, + 16.919 + }; + + KalmanFilter::KalmanFilter() { + int ndim = 4; + double dt = 1.; + + _motion_mat = Eigen::MatrixXf::Identity(8, 8); + for (int i = 0; i < ndim; i++) { + _motion_mat(i, ndim + i) = dt; + } + _update_mat = Eigen::MatrixXf::Identity(4, 8); + + this->_std_weight_position = 1. / 20; + this->_std_weight_velocity = 1. / 160; + } + + KAL_DATA KalmanFilter::initiate(const DETECTBOX &measurement) { + DETECTBOX mean_pos = measurement; + DETECTBOX mean_vel; + for (int i = 0; i < 4; i++) mean_vel(i) = 0; + + KAL_MEAN mean; + for (int i = 0; i < 8; i++) { + if (i < 4) mean(i) = mean_pos(i); + else mean(i) = mean_vel(i - 4); + } + + KAL_MEAN std; + std(0) = 2 * _std_weight_position * measurement[3]; + std(1) = 2 * _std_weight_position * measurement[3]; + std(2) = 1e-2; + std(3) = 2 * _std_weight_position * measurement[3]; + std(4) = 10 * _std_weight_velocity * measurement[3]; + std(5) = 10 * _std_weight_velocity * measurement[3]; + std(6) = 1e-5; + std(7) = 10 * _std_weight_velocity * measurement[3]; + + KAL_MEAN tmp = std.array().square(); + KAL_COVA var = tmp.asDiagonal(); + return std::make_pair(mean, var); + } + + void KalmanFilter::predict(KAL_MEAN &mean, KAL_COVA &covariance) { + //revise the data; + DETECTBOX std_pos; + std_pos << _std_weight_position * mean(3), + _std_weight_position * mean(3), + 1e-2, + _std_weight_position * mean(3); + DETECTBOX std_vel; + std_vel << _std_weight_velocity * mean(3), + _std_weight_velocity * mean(3), + 1e-5, + _std_weight_velocity * mean(3); + KAL_MEAN tmp; + tmp.block<1, 4>(0, 0) = std_pos; + tmp.block<1, 4>(0, 4) = std_vel; + tmp = tmp.array().square(); + KAL_COVA motion_cov = tmp.asDiagonal(); + KAL_MEAN mean1 = this->_motion_mat * mean.transpose(); + KAL_COVA covariance1 = this->_motion_mat * covariance * (_motion_mat.transpose()); + covariance1 += motion_cov; + + mean = mean1; + covariance = covariance1; + } + + KAL_HDATA KalmanFilter::project(const KAL_MEAN &mean, const KAL_COVA &covariance) { + DETECTBOX std; + std << _std_weight_position * mean(3), _std_weight_position * mean(3), + 1e-1, _std_weight_position * mean(3); + KAL_HMEAN mean1 = _update_mat * mean.transpose(); + KAL_HCOVA covariance1 = _update_mat * covariance * (_update_mat.transpose()); + Eigen::Matrix diag = std.asDiagonal(); + diag = diag.array().square().matrix(); + covariance1 += diag; + // covariance1.diagonal() << diag; + return std::make_pair(mean1, covariance1); + } + + KAL_DATA + KalmanFilter::update( + const KAL_MEAN &mean, + const KAL_COVA &covariance, + const DETECTBOX &measurement) { + KAL_HDATA pa = project(mean, covariance); + KAL_HMEAN projected_mean = pa.first; + KAL_HCOVA projected_cov = pa.second; + + //chol_factor, lower = + //scipy.linalg.cho_factor(projected_cov, lower=True, check_finite=False) + //kalmain_gain = + //scipy.linalg.cho_solve((cho_factor, lower), + //np.dot(covariance, self._upadte_mat.T).T, + //check_finite=False).T + Eigen::Matrix B = (covariance * (_update_mat.transpose())).transpose(); + Eigen::Matrix kalman_gain = (projected_cov.llt().solve(B)).transpose(); // eg.8x4 + Eigen::Matrix innovation = measurement - projected_mean; //eg.1x4 + auto tmp = innovation * (kalman_gain.transpose()); + KAL_MEAN new_mean = (mean.array() + tmp.array()).matrix(); + KAL_COVA new_covariance = + covariance - kalman_gain * projected_cov * (kalman_gain.transpose()); + return std::make_pair(new_mean, new_covariance); + } + + Eigen::Matrix + KalmanFilter::gating_distance( + const KAL_MEAN &mean, + const KAL_COVA &covariance, + const std::vector &measurements, + bool only_position) { + KAL_HDATA pa = this->project(mean, covariance); + if (only_position) { + printf("not implement!"); + exit(0); + } + KAL_HMEAN mean1 = pa.first; + KAL_HCOVA covariance1 = pa.second; + + // Eigen::Matrix d(size, 4); + DETECTBOXSS d(measurements.size(), 4); + int pos = 0; + for (DETECTBOX box : measurements) { + d.row(pos++) = box - mean1; + } + Eigen::Matrix factor = covariance1.llt().matrixL(); + Eigen::Matrix z = factor.triangularView().solve( + d).transpose(); + auto zz = ((z.array()) * (z.array())).matrix(); + auto square_maha = zz.colwise().sum(); + return square_maha; + } +} diff --git a/deploy/DeepStream/src/Lapjv.cpp b/deploy/DeepStream/src/Lapjv.cpp new file mode 100644 index 00000000..b8cdcf45 --- /dev/null +++ b/deploy/DeepStream/src/Lapjv.cpp @@ -0,0 +1,330 @@ +#include +#include +#include + +#include "Lapjv.h" + +/** Column-reduction and reduction transfer for a dense cost matrix. + */ +int_t _ccrrt_dense(const uint_t n, cost_t *cost[], + int_t *free_rows, int_t *x, int_t *y, cost_t *v) { + int_t n_free_rows; + boolean *unique; + + for (uint_t i = 0; i < n; i++) { + x[i] = -1; + v[i] = LARGE; + y[i] = 0; + } + for (uint_t i = 0; i < n; i++) { + for (uint_t j = 0; j < n; j++) { + const cost_t c = cost[i][j]; + if (c < v[j]) { + v[j] = c; + y[j] = i; + } + PRINTF("i=%d, j=%d, c[i,j]=%f, v[j]=%f y[j]=%d\n", i, j, c, v[j], y[j]); + } + } + PRINT_COST_ARRAY(v, n); + PRINT_INDEX_ARRAY(y, n); + NEW(unique, boolean, n); + memset(unique, TRUE, n); + { + int_t j = n; + do { + j--; + const int_t i = y[j]; + if (x[i] < 0) { + x[i] = j; + } else { + unique[i] = FALSE; + y[j] = -1; + } + } while (j > 0); + } + n_free_rows = 0; + for (uint_t i = 0; i < n; i++) { + if (x[i] < 0) { + free_rows[n_free_rows++] = i; + } else if (unique[i]) { + const int_t j = x[i]; + cost_t min = LARGE; + for (uint_t j2 = 0; j2 < n; j2++) { + if (j2 == (uint_t) j) { + continue; + } + const cost_t c = cost[i][j2] - v[j2]; + if (c < min) { + min = c; + } + } + PRINTF("v[%d] = %f - %f\n", j, v[j], min); + v[j] -= min; + } + } + FREE(unique); + return n_free_rows; +} + + +/** Augmenting row reduction for a dense cost matrix. + */ +int_t _carr_dense( + const uint_t n, cost_t *cost[], + const uint_t n_free_rows, + int_t *free_rows, int_t *x, int_t *y, cost_t *v) { + uint_t current = 0; + int_t new_free_rows = 0; + uint_t rr_cnt = 0; + PRINT_INDEX_ARRAY(x, n); + PRINT_INDEX_ARRAY(y, n); + PRINT_COST_ARRAY(v, n); + PRINT_INDEX_ARRAY(free_rows, n_free_rows); + while (current < n_free_rows) { + int_t i0; + int_t j1, j2; + cost_t v1, v2, v1_new; + boolean v1_lowers; + + rr_cnt++; + PRINTF("current = %d rr_cnt = %d\n", current, rr_cnt); + const int_t free_i = free_rows[current++]; + j1 = 0; + v1 = cost[free_i][0] - v[0]; + j2 = -1; + v2 = LARGE; + for (uint_t j = 1; j < n; j++) { + PRINTF("%d = %f %d = %f\n", j1, v1, j2, v2); + const cost_t c = cost[free_i][j] - v[j]; + if (c < v2) { + if (c >= v1) { + v2 = c; + j2 = j; + } else { + v2 = v1; + v1 = c; + j2 = j1; + j1 = j; + } + } + } + i0 = y[j1]; + v1_new = v[j1] - (v2 - v1); + v1_lowers = v1_new < v[j1]; + PRINTF("%d %d 1=%d,%f 2=%d,%f v1'=%f(%d,%g) \n", free_i, i0, j1, v1, j2, v2, v1_new, v1_lowers, v[j1] - v1_new); + if (rr_cnt < current * n) { + if (v1_lowers) { + v[j1] = v1_new; + } else if (i0 >= 0 && j2 >= 0) { + j1 = j2; + i0 = y[j2]; + } + if (i0 >= 0) { + if (v1_lowers) { + free_rows[--current] = i0; + } else { + free_rows[new_free_rows++] = i0; + } + } + } else { + PRINTF("rr_cnt=%d >= %d (current=%d * n=%d)\n", rr_cnt, current * n, current, n); + if (i0 >= 0) { + free_rows[new_free_rows++] = i0; + } + } + x[free_i] = j1; + y[j1] = free_i; + } + return new_free_rows; +} + + +/** Find columns with minimum d[j] and put them on the SCAN list. + */ +uint_t _find_dense(const uint_t n, uint_t lo, cost_t *d, int_t *cols, int_t *y) { + uint_t hi = lo + 1; + cost_t mind = d[cols[lo]]; + for (uint_t k = hi; k < n; k++) { + int_t j = cols[k]; + if (d[j] <= mind) { + if (d[j] < mind) { + hi = lo; + mind = d[j]; + } + cols[k] = cols[hi]; + cols[hi++] = j; + } + } + return hi; +} + + +// Scan all columns in TODO starting from arbitrary column in SCAN +// and try to decrease d of the TODO columns using the SCAN column. +int_t _scan_dense(const uint_t n, cost_t *cost[], + uint_t *plo, uint_t *phi, + cost_t *d, int_t *cols, int_t *pred, + int_t *y, cost_t *v) { + uint_t lo = *plo; + uint_t hi = *phi; + cost_t h, cred_ij; + + while (lo != hi) { + int_t j = cols[lo++]; + const int_t i = y[j]; + const cost_t mind = d[j]; + h = cost[i][j] - v[j] - mind; + PRINTF("i=%d j=%d h=%f\n", i, j, h); + // For all columns in TODO + for (uint_t k = hi; k < n; k++) { + j = cols[k]; + cred_ij = cost[i][j] - v[j] - h; + if (cred_ij < d[j]) { + d[j] = cred_ij; + pred[j] = i; + if (cred_ij == mind) { + if (y[j] < 0) { + return j; + } + cols[k] = cols[hi]; + cols[hi++] = j; + } + } + } + } + *plo = lo; + *phi = hi; + return -1; +} + + +/** Single iteration of modified Dijkstra shortest path algorithm as explained in the JV paper. + * + * This is a dense matrix version. + * + * \return The closest free column index. + */ +int_t find_path_dense( + const uint_t n, cost_t *cost[], + const int_t start_i, + int_t *y, cost_t *v, + int_t *pred) { + uint_t lo = 0, hi = 0; + int_t final_j = -1; + uint_t n_ready = 0; + int_t *cols; + cost_t *d; + + NEW(cols, int_t, n); + NEW(d, cost_t, n); + + for (uint_t i = 0; i < n; i++) { + cols[i] = i; + pred[i] = start_i; + d[i] = cost[start_i][i] - v[i]; + } + PRINT_COST_ARRAY(d, n); + while (final_j == -1) { + // No columns left on the SCAN list. + if (lo == hi) { + PRINTF("%d..%d -> find\n", lo, hi); + n_ready = lo; + hi = _find_dense(n, lo, d, cols, y); + PRINTF("check %d..%d\n", lo, hi); + PRINT_INDEX_ARRAY(cols, n); + for (uint_t k = lo; k < hi; k++) { + const int_t j = cols[k]; + if (y[j] < 0) { + final_j = j; + } + } + } + if (final_j == -1) { + PRINTF("%d..%d -> scan\n", lo, hi); + final_j = _scan_dense( + n, cost, &lo, &hi, d, cols, pred, y, v); + PRINT_COST_ARRAY(d, n); + PRINT_INDEX_ARRAY(cols, n); + PRINT_INDEX_ARRAY(pred, n); + } + } + + PRINTF("found final_j=%d\n", final_j); + PRINT_INDEX_ARRAY(cols, n); + { + const cost_t mind = d[cols[lo]]; + for (uint_t k = 0; k < n_ready; k++) { + const int_t j = cols[k]; + v[j] += d[j] - mind; + } + } + + FREE(cols); + FREE(d); + + return final_j; +} + + +/** Augment for a dense cost matrix. + */ +int_t _ca_dense( + const uint_t n, cost_t *cost[], + const uint_t n_free_rows, + int_t *free_rows, int_t *x, int_t *y, cost_t *v) { + int_t *pred; + + NEW(pred, int_t, n); + + for (int_t *pfree_i = free_rows; pfree_i < free_rows + n_free_rows; pfree_i++) { + int_t i = -1, j; + uint_t k = 0; + + PRINTF("looking at free_i=%d\n", *pfree_i); + j = find_path_dense(n, cost, *pfree_i, y, v, pred); + ASSERT(j >= 0); + ASSERT(j < n); + while (i != *pfree_i) { + PRINTF("augment %d\n", j); + PRINT_INDEX_ARRAY(pred, n); + i = pred[j]; + PRINTF("y[%d]=%d -> %d\n", j, y[j], i); + y[j] = i; + PRINT_INDEX_ARRAY(x, n); + SWAP_INDICES(j, x[i]); + k++; + if (k >= n) { + ASSERT(FALSE); + } + } + } + FREE(pred); + return 0; +} + + +/** Solve dense sparse LAP. + */ +int lapjv_internal( + const uint_t n, cost_t *cost[], + int_t *x, int_t *y) { + int ret; + int_t *free_rows; + cost_t *v; + + NEW(free_rows, int_t, n); + NEW(v, cost_t, n); + ret = _ccrrt_dense(n, cost, free_rows, x, y, v); + int i = 0; + while (ret > 0 && i < 2) { + ret = _carr_dense(n, cost, ret, free_rows, x, y, v); + i++; + } + if (ret > 0) { + ret = _ca_dense(n, cost, ret, free_rows, x, y, v); + } + FREE(v); + FREE(free_rows); + return ret; +} diff --git a/deploy/DeepStream/src/NvMOTContext.cpp b/deploy/DeepStream/src/NvMOTContext.cpp new file mode 100644 index 00000000..6c22aa84 --- /dev/null +++ b/deploy/DeepStream/src/NvMOTContext.cpp @@ -0,0 +1,69 @@ +#include "Tracker.h" +#include "BYTETracker.h" +#include + +NvMOTContext::NvMOTContext(const NvMOTConfig &configIn, NvMOTConfigResponse &configResponse) { + configResponse.summaryStatus = NvMOTConfigStatus_OK; +} + +NvMOTStatus NvMOTContext::processFrame(const NvMOTProcessParams *params, NvMOTTrackedObjBatch *pTrackedObjectsBatch) { + for (uint streamIdx = 0; streamIdx < pTrackedObjectsBatch->numFilled; streamIdx++){ + NvMOTTrackedObjList *trackedObjList = &pTrackedObjectsBatch->list[streamIdx]; + NvMOTFrame *frame = ¶ms->frameList[streamIdx]; + std::vector nvObjects(frame->objectsIn.numFilled); + for (uint32_t numObjects = 0; numObjects < frame->objectsIn.numFilled; numObjects++) { + NvMOTObjToTrack *objectToTrack = &frame->objectsIn.list[numObjects]; + NvObject nvObject; + nvObject.prob = objectToTrack->confidence; + nvObject.label = objectToTrack->classId; + nvObject.rect[0] = objectToTrack->bbox.x; + nvObject.rect[1] = objectToTrack->bbox.y; + nvObject.rect[2] = objectToTrack->bbox.width; + nvObject.rect[3] = objectToTrack->bbox.height; + nvObject.associatedObjectIn = objectToTrack; + nvObjects.push_back(nvObject); + } + + if (byteTrackerMap.find(frame->streamID) == byteTrackerMap.end()) + byteTrackerMap.insert(std::pair>(frame->streamID, std::make_shared(15, 30))); + + std::vector outputTracks = byteTrackerMap.at(frame->streamID)->update(nvObjects); + + NvMOTTrackedObj *trackedObjs = new NvMOTTrackedObj[512]; + int filled = 0; + + for (STrack &sTrack: outputTracks) { + std::vector tlwh = sTrack.original_tlwh; + NvMOTRect motRect{tlwh[0], tlwh[1], tlwh[2], tlwh[3]}; + NvMOTTrackedObj *trackedObj = new NvMOTTrackedObj; + trackedObj->classId = 0; + trackedObj->trackingId = (uint64_t) sTrack.track_id; + trackedObj->bbox = motRect; + trackedObj->confidence = 1; + trackedObj->age = (uint32_t) sTrack.tracklet_len; + trackedObj->associatedObjectIn = sTrack.associatedObjectIn; + trackedObj->associatedObjectIn->doTracking = true; + trackedObjs[filled++] = *trackedObj; + } + + trackedObjList->streamID = frame->streamID; + trackedObjList->frameNum = frame->frameNum; + trackedObjList->valid = true; + trackedObjList->list = trackedObjs; + trackedObjList->numFilled = filled; + trackedObjList->numAllocated = 512; + } +} + +NvMOTStatus NvMOTContext::processFramePast(const NvMOTProcessParams *params, + NvDsPastFrameObjBatch *pPastFrameObjectsBatch) { + return NvMOTStatus_OK; +} + +NvMOTStatus NvMOTContext::removeStream(const NvMOTStreamId streamIdMask) { + if (byteTrackerMap.find(streamIdMask) != byteTrackerMap.end()){ + std::cout << "Removing tracker for stream: " << streamIdMask << std::endl; + byteTrackerMap.erase(streamIdMask); + } + return NvMOTStatus_OK; +} diff --git a/deploy/DeepStream/src/STrack.cpp b/deploy/DeepStream/src/STrack.cpp new file mode 100644 index 00000000..380bf811 --- /dev/null +++ b/deploy/DeepStream/src/STrack.cpp @@ -0,0 +1,182 @@ +#include "STrack.h" + +STrack::STrack(vector tlwh_, float score, int label, NvMOTObjToTrack *associatedObjectIn) { + _tlwh.resize(4); + _tlwh.assign(tlwh_.begin(), tlwh_.end()); + original_tlwh.resize(4); + original_tlwh.assign(tlwh_.begin(), tlwh_.end()); + + is_activated = false; + track_id = 0; + state = TrackState::New; + + tlwh.resize(4); + tlbr.resize(4); + + static_tlwh(); + static_tlbr(); + frame_id = 0; + tracklet_len = 0; + this->score = score; + start_frame = 0; + this->label = label; + this->associatedObjectIn = associatedObjectIn; +} + +STrack::~STrack() { +} + +void STrack::activate(byte_kalman::KalmanFilter &kalman_filter, int frame_id) { + this->kalman_filter = kalman_filter; + this->track_id = this->next_id(); + + vector _tlwh_tmp(4); + _tlwh_tmp[0] = this->_tlwh[0]; + _tlwh_tmp[1] = this->_tlwh[1]; + _tlwh_tmp[2] = this->_tlwh[2]; + _tlwh_tmp[3] = this->_tlwh[3]; + vector xyah = tlwh_to_xyah(_tlwh_tmp); + DETECTBOX xyah_box; + xyah_box[0] = xyah[0]; + xyah_box[1] = xyah[1]; + xyah_box[2] = xyah[2]; + xyah_box[3] = xyah[3]; + auto mc = this->kalman_filter.initiate(xyah_box); + this->mean = mc.first; + this->covariance = mc.second; + + static_tlwh(); + static_tlbr(); + + this->tracklet_len = 0; + this->state = TrackState::Tracked; + if (frame_id == 1) { + this->is_activated = true; + } + //this->is_activated = true; + this->frame_id = frame_id; + this->start_frame = frame_id; +} + +void STrack::re_activate(STrack &new_track, int frame_id, bool new_id) { + vector xyah = tlwh_to_xyah(new_track.tlwh); + DETECTBOX xyah_box; + xyah_box[0] = xyah[0]; + xyah_box[1] = xyah[1]; + xyah_box[2] = xyah[2]; + xyah_box[3] = xyah[3]; + auto mc = this->kalman_filter.update(this->mean, this->covariance, xyah_box); + this->mean = mc.first; + this->covariance = mc.second; + + static_tlwh(); + static_tlbr(); + + this->tracklet_len = 0; + this->state = TrackState::Tracked; + this->is_activated = true; + this->frame_id = frame_id; + this->score = new_track.score; + this->associatedObjectIn = new_track.associatedObjectIn; + this->original_tlwh.resize(4); + this->original_tlwh.assign(new_track.original_tlwh.begin(), new_track.original_tlwh.end()); + if (new_id) + this->track_id = next_id(); +} + +void STrack::update(STrack &new_track, int frame_id) { + this->frame_id = frame_id; + this->tracklet_len++; + + vector xyah = tlwh_to_xyah(new_track.tlwh); + DETECTBOX xyah_box; + xyah_box[0] = xyah[0]; + xyah_box[1] = xyah[1]; + xyah_box[2] = xyah[2]; + xyah_box[3] = xyah[3]; + + auto mc = this->kalman_filter.update(this->mean, this->covariance, xyah_box); + this->mean = mc.first; + this->covariance = mc.second; + + static_tlwh(); + static_tlbr(); + + this->state = TrackState::Tracked; + this->is_activated = true; + this->score = new_track.score; + this->associatedObjectIn = new_track.associatedObjectIn; + this->original_tlwh.resize(4); + this->original_tlwh.assign(new_track.original_tlwh.begin(), new_track.original_tlwh.end()); +} + +void STrack::static_tlwh() { + if (this->state == TrackState::New) { + tlwh[0] = _tlwh[0]; + tlwh[1] = _tlwh[1]; + tlwh[2] = _tlwh[2]; + tlwh[3] = _tlwh[3]; + return; + } + + tlwh[0] = mean[0]; + tlwh[1] = mean[1]; + tlwh[2] = mean[2]; + tlwh[3] = mean[3]; + + tlwh[2] *= tlwh[3]; + tlwh[0] -= tlwh[2] / 2; + tlwh[1] -= tlwh[3] / 2; +} + +void STrack::static_tlbr() { + tlbr.clear(); + tlbr.assign(tlwh.begin(), tlwh.end()); + tlbr[2] += tlbr[0]; + tlbr[3] += tlbr[1]; +} + +vector STrack::tlwh_to_xyah(vector tlwh_tmp) { + vector tlwh_output = tlwh_tmp; + tlwh_output[0] += tlwh_output[2] / 2; + tlwh_output[1] += tlwh_output[3] / 2; + tlwh_output[2] /= tlwh_output[3]; + return tlwh_output; +} + +vector STrack::to_xyah() { + return tlwh_to_xyah(tlwh); +} + +vector STrack::tlbr_to_tlwh(vector &tlbr) { + tlbr[2] -= tlbr[0]; + tlbr[3] -= tlbr[1]; + return tlbr; +} + +void STrack::mark_lost() { + state = TrackState::Lost; +} + +void STrack::mark_removed() { + state = TrackState::Removed; +} + +int STrack::next_id() { + static int _count = 0; + _count++; + return _count; +} + +int STrack::end_frame() { + return this->frame_id; +} + +void STrack::multi_predict(vector &stracks, byte_kalman::KalmanFilter &kalman_filter) { + for (int i = 0; i < stracks.size(); i++) { + if (stracks[i]->state != TrackState::Tracked) { + stracks[i]->mean[7] = 0; + } + kalman_filter.predict(stracks[i]->mean, stracks[i]->covariance); + } +} \ No newline at end of file diff --git a/deploy/DeepStream/src/Tracker.cpp b/deploy/DeepStream/src/Tracker.cpp new file mode 100644 index 00000000..0c14f376 --- /dev/null +++ b/deploy/DeepStream/src/Tracker.cpp @@ -0,0 +1,73 @@ +#include "Tracker.h" +#include + +NvMOTStatus NvMOT_Query(uint16_t customConfigFilePathSize, + char *pCustomConfigFilePath, + NvMOTQuery *pQuery) { + /** + * Users can parse the low-level config file in pCustomConfigFilePath to check + * the low-level tracker's requirements + */ + + pQuery->computeConfig = NVMOTCOMP_CPU; // among {NVMOTCOMP_GPU, NVMOTCOMP_CPU} + pQuery->numTransforms = 0; // 0 for IOU tracker, 1 for NvDCF or DeepSORT tracker as they require the video frames + pQuery->colorFormats[0] = NVBUF_COLOR_FORMAT_NV12; // among {NVBUF_COLOR_FORMAT_NV12, NVBUF_COLOR_FORMAT_RGBA} + + // among {NVBUF_MEM_DEFAULT, NVBUF_MEM_CUDA_DEVICE, NVBUF_MEM_CUDA_UNIFIED, NVBUF_MEM_CUDA_PINNED, ... } +#ifdef __aarch64__ + pQuery->memType = NVBUF_MEM_DEFAULT; +#else + pQuery->memType = NVBUF_MEM_CUDA_DEVICE; +#endif + + pQuery->supportBatchProcessing = false; // set NvMOTBatchMode_Batch if the low-level tracker supports batch processing mode. Otherwise, NvMOTBatchMode_NonBatch + pQuery->supportPastFrame = false; // set true if the low-level tracker supports the past-frame data or not + + /** + * return NvMOTStatus_Error if something is wrong + * return NvMOTStatus_OK if everything went well + */ + std::cout << "[BYTETrack Initialized]" << std::endl; + return NvMOTStatus_OK; +} + + +NvMOTStatus NvMOT_Init(NvMOTConfig *pConfigIn, + NvMOTContextHandle *pContextHandle, + NvMOTConfigResponse *pConfigResponse) { + if (pContextHandle != nullptr) { + NvMOT_DeInit(*pContextHandle); + } + + /// User-defined class for the context + NvMOTContext *pContext = nullptr; + + /// Instantiate the user-defined context + pContext = new NvMOTContext(*pConfigIn, *pConfigResponse); + + /// Pass the pointer as the context handle + *pContextHandle = pContext; + + /** + * return NvMOTStatus_Error if something is wrong + * return NvMOTStatus_OK if everything went well + */ + return NvMOTStatus_OK; +} + +void NvMOT_DeInit(NvMOTContextHandle contextHandle) { + /// Destroy the context handle + delete contextHandle; +} + +NvMOTStatus NvMOT_Process(NvMOTContextHandle contextHandle, + NvMOTProcessParams *pParams, + NvMOTTrackedObjBatch *pTrackedObjectsBatch) { + /// Process the given video frame using the user-defined method in the context, and generate outputs + contextHandle->processFrame(pParams, pTrackedObjectsBatch); + return NvMOTStatus_OK; +} + +void NvMOT_RemoveStreams(NvMOTContextHandle contextHandle, NvMOTStreamId streamIdMask) { + +}