diff --git a/CMakeLists.txt b/CMakeLists.txt index 8ac6758..6e0960b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,6 +83,8 @@ include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/common_det/cuda ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/landing_det/cuda ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/sot/ocv470 + ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/mot/sort + ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/mot/bytetrack ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/color_line ${CMAKE_CURRENT_SOURCE_DIR}/video_io ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/veri/cuda @@ -182,6 +184,10 @@ file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/video_io/*.cpp) list(APPEND spirecv_SRCS ${ALG_SRC_FILES}) file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/utils/*.cpp) list(APPEND spirecv_SRCS ${ALG_SRC_FILES}) +file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/mot/sort/*.cpp) +list(APPEND spirecv_SRCS ${ALG_SRC_FILES}) +file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/mot/bytetrack/*.cpp) +list(APPEND spirecv_SRCS ${ALG_SRC_FILES}) if(USE_CUDA) list(APPEND spirecv_SRCS algorithm/common_det/cuda/yolov7/preprocess.cu) diff --git a/algorithm/mot/bytetrack/BYTETracker.cpp b/algorithm/mot/bytetrack/BYTETracker.cpp new file mode 100755 index 0000000..45aace9 --- /dev/null +++ b/algorithm/mot/bytetrack/BYTETracker.cpp @@ -0,0 +1,262 @@ + +#include "BYTETracker.h" +#include + +namespace sv +{ + + BYTETracker::BYTETracker(int frame_rate, int track_buffer) + { + track_thresh = 0.5; + high_thresh = 0.6; + match_thresh = 0.8; + + frame_id = 0; + max_time_lost = int(frame_rate / 30.0 * track_buffer); + } + + BYTETracker::~BYTETracker() + { + } + + void BYTETracker::update(TargetsInFrame &tgts) + { + ////////////////// Step 1: Get detections ////////////////// + this->frame_id++; + std::vector activated_stracks; + std::vector refind_stracks; + std::vector removed_stracks; + std::vector lost_stracks; + std::vector detections; + std::vector detections_low; + + std::vector detections_cp; + std::vector tracked_stracks_swap; + std::vector resa, resb; + std::vector output_stracks; + + std::vector unconfirmed; + std::vector tracked_stracks; + std::vector strack_pool; + std::vector r_tracked_stracks; + + if (tgts.targets.size() > 0) + { + for (int i = 0; i < tgts.targets.size(); i++) + { + + tgts.targets[i].tracked_id = 0; + tgts.targets[i].has_tid = true; + + std::vector tlbr_; + tlbr_.resize(4); + tlbr_[0] = tgts.targets[i].cx * tgts.width - tgts.targets[i].w * tgts.width / 2; + tlbr_[1] = tgts.targets[i].cy * tgts.height - tgts.targets[i].h * tgts.height / 2; + tlbr_[2] = tgts.targets[i].cx * tgts.width + tgts.targets[i].w * tgts.width / 2; + tlbr_[3] = tgts.targets[i].cy * tgts.height + tgts.targets[i].h * tgts.height / 2; + + float score = tgts.targets[i].score; + + STrack strack(STrack::tlbr_to_tlwh(tlbr_), score); + 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); + + std::vector> dists; + int dist_size = 0, dist_size_size = 0; + dists = iou_distance(strack_pool, detections, dist_size, dist_size_size); + + std::vector> matches; + std::vector u_track, u_detection; + linear_assignment(dists, dist_size, dist_size_size, match_thresh, matches, u_track, u_detection); + + for (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(); + std::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]); + } + } + + tgts.targets.clear(); + + for (unsigned long i = 0; i < output_stracks.size(); i++) + { + std::vector tlwh = output_stracks[i].tlwh; + bool vertical = tlwh[2] / tlwh[3] > 1.6; + + if (tlwh[2] * tlwh[3] > 20 && !vertical) + { + Target tgt; + tgt.setBox(tlwh[0], tlwh[1], tlwh[0] + tlwh[2], tlwh[1] + tlwh[3], tgts.width, tgts.height); + tgt.setTrackID(output_stracks[i].track_id); + tgts.targets.push_back(tgt); + } + } + } +} diff --git a/algorithm/mot/bytetrack/BYTETracker.h b/algorithm/mot/bytetrack/BYTETracker.h new file mode 100755 index 0000000..83c9bc4 --- /dev/null +++ b/algorithm/mot/bytetrack/BYTETracker.h @@ -0,0 +1,56 @@ +#ifndef __SV_BYTETrack__ +#define __SV_BYTETrack__ + +#include "sv_core.h" +#include "STrack.h" + +class detect_result +{ +public: + int classId; + float confidence; + cv::Rect_ box; +}; + +namespace sv { + +class BYTETracker +{ +public: + BYTETracker(int frame_rate = 30, int track_buffer = 30); + ~BYTETracker(); + + void update(TargetsInFrame &tgts); + cv::Scalar get_color(int idx); + +private: + std::vector joint_stracks( std::vector &tlista, std::vector &tlistb); + std::vector joint_stracks( std::vector &tlista, std::vector &tlistb); + + std::vector sub_stracks( std::vector &tlista, std::vector &tlistb); + void remove_duplicate_stracks( std::vector &resa, std::vector &resb, std::vector &stracksa, std::vector &stracksb); + + void linear_assignment( std::vector< std::vector > &cost_matrix, int cost_matrix_size, int cost_matrix_size_size, float thresh, + std::vector< std::vector > &matches, std::vector &unmatched_a, std::vector &unmatched_b); + std::vector< std::vector > iou_distance( std::vector &atracks, std::vector &btracks, int &dist_size, int &dist_size_size); + std::vector< std::vector > iou_distance( std::vector &atracks, std::vector &btracks); + std::vector< std::vector > ious( std::vector< std::vector > &atlbrs, std::vector< std::vector > &btlbrs); + + double lapjv(const std::vector< std::vector > &cost, std::vector &rowsol, std::vector &colsol, + bool extend_cost = false, float cost_limit = LONG_MAX, bool return_cost = true); + +private: + + float track_thresh; + float high_thresh; + float match_thresh; + int frame_id; + int max_time_lost; + + std::vector tracked_stracks; + std::vector lost_stracks; + std::vector removed_stracks; + byte_kalman::ByteKalmanFilter kalman_filter; +}; +} +#endif \ No newline at end of file diff --git a/algorithm/mot/bytetrack/BytekalmanFilter.cpp b/algorithm/mot/bytetrack/BytekalmanFilter.cpp new file mode 100755 index 0000000..727da0d --- /dev/null +++ b/algorithm/mot/bytetrack/BytekalmanFilter.cpp @@ -0,0 +1,152 @@ +#include "BytekalmanFilter.h" +#include + +namespace byte_kalman +{ + const double ByteKalmanFilter::chi2inv95[10] = { + 0, + 3.8415, + 5.9915, + 7.8147, + 9.4877, + 11.070, + 12.592, + 14.067, + 15.507, + 16.919 + }; + ByteKalmanFilter::ByteKalmanFilter() + { + 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 ByteKalmanFilter::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 ByteKalmanFilter::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 ByteKalmanFilter::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 + ByteKalmanFilter::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 + ByteKalmanFilter::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/algorithm/mot/bytetrack/BytekalmanFilter.h b/algorithm/mot/bytetrack/BytekalmanFilter.h new file mode 100755 index 0000000..18ce541 --- /dev/null +++ b/algorithm/mot/bytetrack/BytekalmanFilter.h @@ -0,0 +1,31 @@ +#pragma once + +#include "dataType.h" + +namespace byte_kalman +{ + class ByteKalmanFilter + { + public: + static const double chi2inv95[10]; + ByteKalmanFilter(); + 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; + }; +} \ No newline at end of file diff --git a/algorithm/mot/bytetrack/STrack.cpp b/algorithm/mot/bytetrack/STrack.cpp new file mode 100755 index 0000000..728d507 --- /dev/null +++ b/algorithm/mot/bytetrack/STrack.cpp @@ -0,0 +1,192 @@ +#include "STrack.h" + +STrack::STrack( std::vector tlwh_, float score) +{ + _tlwh.resize(4); + _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; +} + +STrack::~STrack() +{ +} + +void STrack::activate(byte_kalman::ByteKalmanFilter &kalman_filter, int frame_id) +{ + this->kalman_filter = kalman_filter; + this->track_id = this->next_id(); + + std::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]; + std::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) +{ + std::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; + 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++; + + std::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; +} + +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]; +} + + std::vector STrack::tlwh_to_xyah( std::vector tlwh_tmp) +{ + std::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; +} + + std::vector STrack::to_xyah() +{ + return tlwh_to_xyah(tlwh); +} + + std::vector STrack::tlbr_to_tlwh( std::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( std::vector &stracks, byte_kalman::ByteKalmanFilter &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/algorithm/mot/bytetrack/STrack.h b/algorithm/mot/bytetrack/STrack.h new file mode 100755 index 0000000..96e4874 --- /dev/null +++ b/algorithm/mot/bytetrack/STrack.h @@ -0,0 +1,47 @@ +#pragma once + +#include +#include "BytekalmanFilter.h" + +enum TrackState { New = 0, Tracked, Lost, Removed }; + +class STrack +{ +public: + STrack( std::vector tlwh_, float score); + ~STrack(); + + std::vector static tlbr_to_tlwh( std::vector &tlbr); + void static multi_predict( std::vector &stracks, byte_kalman::ByteKalmanFilter &kalman_filter); + void static_tlwh(); + void static_tlbr(); + std::vector tlwh_to_xyah( std::vector tlwh_tmp); + std::vector to_xyah(); + void mark_lost(); + void mark_removed(); + int next_id(); + int end_frame(); + + void activate(byte_kalman::ByteKalmanFilter &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; + + std::vector _tlwh; + std::vector tlwh; + std::vector tlbr; + int frame_id; + int tracklet_len; + int start_frame; + + KAL_MEAN mean; + KAL_COVA covariance; + float score; + +private: + byte_kalman::ByteKalmanFilter kalman_filter; +}; \ No newline at end of file diff --git a/algorithm/mot/bytetrack/dataType.h b/algorithm/mot/bytetrack/dataType.h new file mode 100755 index 0000000..cff28ed --- /dev/null +++ b/algorithm/mot/bytetrack/dataType.h @@ -0,0 +1,43 @@ +#pragma once + +#include +#include + +#include +#include + + +//const int k_feature_dim=512;//feature dim + +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/algorithm/mot/bytetrack/lapjv.cpp b/algorithm/mot/bytetrack/lapjv.cpp new file mode 100755 index 0000000..0ed0096 --- /dev/null +++ b/algorithm/mot/bytetrack/lapjv.cpp @@ -0,0 +1,343 @@ +#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; +} \ No newline at end of file diff --git a/algorithm/mot/bytetrack/lapjv.h b/algorithm/mot/bytetrack/lapjv.h new file mode 100755 index 0000000..82096b8 --- /dev/null +++ b/algorithm/mot/bytetrack/lapjv.h @@ -0,0 +1,63 @@ +#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 \ No newline at end of file diff --git a/algorithm/mot/bytetrack/utils.cpp b/algorithm/mot/bytetrack/utils.cpp new file mode 100755 index 0000000..056fd4e --- /dev/null +++ b/algorithm/mot/bytetrack/utils.cpp @@ -0,0 +1,432 @@ +#include "BYTETracker.h" +#include "lapjv.h" + +namespace sv { + +std::vector BYTETracker::joint_stracks( std::vector &tlista, std::vector &tlistb) +{ + std::map exists; + std::vector res; + for (int i = 0; i < tlista.size(); i++) + { + exists.insert(std::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; +} + +std::vector BYTETracker::joint_stracks( std::vector &tlista, std::vector &tlistb) +{ + std::map exists; + std::vector res; + for (int i = 0; i < tlista.size(); i++) + { + exists.insert(std::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; +} + +std::vector BYTETracker::sub_stracks( std::vector &tlista, std::vector &tlistb) +{ + std::map stracks; + for (int i = 0; i < tlista.size(); i++) + { + stracks.insert(std::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); + } + } + + std::vector res; + std::map::iterator it; + for (it = stracks.begin(); it != stracks.end(); ++it) + { + res.push_back(it->second); + } + + return res; +} + +void BYTETracker::remove_duplicate_stracks( std::vector &resa, std::vector &resb, std::vector &stracksa, std::vector &stracksb) +{ + std::vector< std::vector > pdist = iou_distance(stracksa, stracksb); + std::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(std::pair(i, j)); + } + } + } + + std::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++) + { + std::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++) + { + std::vector::iterator iter = find(dupb.begin(), dupb.end(), i); + if (iter == dupb.end()) + { + resb.push_back(stracksb[i]); + } + } +} + +void BYTETracker::linear_assignment( std::vector< std::vector > &cost_matrix, int cost_matrix_size, int cost_matrix_size_size, float thresh, + std::vector< std::vector > &matches, std::vector &unmatched_a, std::vector &unmatched_b) +{ + if (cost_matrix.size() == 0) + { + for (int i = 0; i < cost_matrix_size; i++) + { + unmatched_a.push_back(i); + } + for (int i = 0; i < cost_matrix_size_size; i++) + { + unmatched_b.push_back(i); + } + return; + } + + std::vector rowsol; std::vector colsol; + float c = lapjv(cost_matrix, rowsol, colsol, true, thresh); + for (int i = 0; i < rowsol.size(); i++) + { + if (rowsol[i] >= 0) + { + std::vector match; + match.push_back(i); + match.push_back(rowsol[i]); + matches.push_back(match); + } + else + { + unmatched_a.push_back(i); + } + } + + for (int i = 0; i < colsol.size(); i++) + { + if (colsol[i] < 0) + { + unmatched_b.push_back(i); + } + } +} + +std::vector< std::vector > BYTETracker::ious( std::vector< std::vector > &atlbrs, std::vector< std::vector > &btlbrs) +{ + std::vector< std::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++) + { + std::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 = cv::min(atlbrs[n][2], btlbrs[k][2]) - cv::max(atlbrs[n][0], btlbrs[k][0]) + 1; + if (iw > 0) + { + float ih = cv::min(atlbrs[n][3], btlbrs[k][3]) - cv::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; +} + +std::vector< std::vector > BYTETracker::iou_distance( std::vector &atracks, std::vector &btracks, int &dist_size, int &dist_size_size) +{ + std::vector< std::vector > cost_matrix; + if (atracks.size() * btracks.size() == 0) + { + dist_size = atracks.size(); + dist_size_size = btracks.size(); + return cost_matrix; + } + std::vector< std::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(); + + std::vector< std::vector > _ious = ious(atlbrs, btlbrs); + + for (int i = 0; i < _ious.size();i++) + { + std::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; +} + +std::vector< std::vector > BYTETracker::iou_distance( std::vector &atracks, std::vector &btracks) +{ + std::vector< std::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); + } + + std::vector< std::vector > _ious = ious(atlbrs, btlbrs); + std::vector< std::vector > cost_matrix; + for (int i = 0; i < _ious.size(); i++) + { + std::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 std::vector< std::vector > &cost, std::vector &rowsol, std::vector &colsol, + bool extend_cost, float cost_limit, bool return_cost) +{ + std::vector< std::vector > cost_c; + cost_c.assign(cost.begin(), cost.end()); + + std::vector< std::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) + { + std::cout << "set extend_cost=True" << std::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!" << std::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; +} + +cv::Scalar BYTETracker::get_color(int idx) +{ + idx += 3; + return cv::Scalar(37 * idx % 255, 17 * idx % 255, 29 * idx % 255); +} +} \ No newline at end of file diff --git a/algorithm/mot/sort/sort.cpp b/algorithm/mot/sort/sort.cpp new file mode 100755 index 0000000..05250cc --- /dev/null +++ b/algorithm/mot/sort/sort.cpp @@ -0,0 +1,377 @@ +#include "sort.h" +#include +#include +#include +#include +#include "gason.h" +#include "sv_util.h" + +using namespace std; +using namespace Eigen; + +namespace sv { + + +KalmanFilter::KalmanFilter() +{ + this->_chi2inv95 << 3.8415, 5.9915, 7.8147, 9.4877, 11.070, 12.592, 14.067, 15.507, 16.919; + this->_F = MatrixXd::Identity(8, 8); + for (int i=0; i<4; i++) + { + this->_F(i,i+4) = 1.; //1 + } + this->_H = MatrixXd::Identity(4, 8); + this->_std_weight_position = 1. / 20; //1./20 + this->_std_weight_vel = 1. / 160; //1./160 +} + +KalmanFilter::~KalmanFilter() +{ +} + +pair, Matrix > KalmanFilter::initiate(Vector4d &bbox) +{ + Matrix mean; + Matrix zero_vector; + zero_vector.setZero(); + mean << bbox(0), bbox(1), (double)bbox(2) / (double)bbox(3), bbox(3), zero_vector; + VectorXd stds(8); + stds << 2 * this->_std_weight_position * mean(3), 2 * this->_std_weight_position * mean(3), 0.01, 2 * this->_std_weight_position * mean(3), \ + 10 * this->_std_weight_vel * mean(3), 10 * this->_std_weight_vel * mean(3), 1e-5, 10 * this->_std_weight_vel * mean(3); + MatrixXd squared = stds.array().square(); + Matrix covariances; + covariances = squared.asDiagonal(); + return make_pair(mean, covariances); +} + +pair, Matrix > KalmanFilter::update(Matrix mean, Matrix covariances, sv::Box &box) +{ + VectorXd measurement(4); + double a = (double)(box.x2-box.x1) / (double)(box.y2-box.y1); + measurement << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, a, box.y2-box.y1; + pair, Matrix > projected = project(mean, covariances); + Matrix projected_mean = projected.first; + Matrix projected_cov = projected.second; + + Eigen::LLT chol_factor(projected_cov); + MatrixXd Kalman_gain = (chol_factor.solve((covariances * this->_H.transpose()).transpose())).transpose(); + + VectorXd innovation = measurement - projected_mean; + Matrix new_mean = mean + Kalman_gain *innovation; + Matrix new_covariances = covariances - Kalman_gain * projected_cov * Kalman_gain.transpose(); + + return make_pair(new_mean, new_covariances); +} + +pair, Matrix > KalmanFilter::project(Matrix mean, Matrix covariances) +{ + VectorXd stds(4); + stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 0.1, this->_std_weight_position * mean(3); + MatrixXd squared = stds.array().square(); + MatrixXd R = squared.asDiagonal(); + + Matrix pro_mean = this->_H * mean; + Matrix pro_covariances = this->_H * covariances * this->_H.transpose() + R; + return make_pair(pro_mean, pro_covariances); +} +pair, Matrix > KalmanFilter::predict(Matrix mean, Matrix covariances) +{ + VectorXd stds(8); + stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 1e-2, this->_std_weight_position * mean(3), \ + this->_std_weight_vel * mean(3), this->_std_weight_vel * mean(3), 1e-5, this->_std_weight_vel * mean(3); // a = 0.01 + MatrixXd squared = stds.array().square(); + MatrixXd Q = squared.asDiagonal(); + Matrix pre_mean = this->_F * mean; + Matrix pre_cov = this->_F * covariances * this->_F.transpose()+Q;//+Q + return make_pair(pre_mean, pre_cov); +} + + +SORT::~SORT() +{ +} + +void SORT::update(TargetsInFrame& tgts) +{ + KalmanFilter kf; + if (! this->_tracklets.size() || tgts.targets.size() == 0) + { + Vector4d bbox; + for (int i=0; i_next_tracklet_id; + tgts.targets[i].tracked_id = this->_next_tracklet_id; + tgts.targets[i].has_tid = true; + + tracklet.bbox << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, box.x2-box.x1, box.y2-box.y1; // x,y,w,h; center(x,y) + + tracklet.age = 0; + tracklet.hits = 1; + //tracklet.misses = 0; + tracklet.frame_id = tgts.frame_id; + tracklet.category_id = tgts.targets[i].category_id; + if (tgts.frame_id == 0) + { + tracklet.tentative = false; + } + else + { + tracklet.tentative = true; + } + // initate the motion + pair, Matrix > motion = kf.initiate(tracklet.bbox); + tracklet.mean = motion.first; + tracklet.covariance = motion.second; + + this->_tracklets.push_back(tracklet); + } + } + else + { + for (int i=0; i match_det(tgts.targets.size(), -1); + // predict the next state of each tracklet + for (auto& tracklet : this->_tracklets) + { + tracklet.age++; + pair, Matrix > motion = kf.predict(tracklet.mean, tracklet.covariance); + tracklet.bbox << motion.first(0), motion.first(1), motion.first(2) * motion.first(3), motion.first(3); + tracklet.mean = motion.first; + tracklet.covariance = motion.second; + } + + // Match the detections to the existing tracklets + vector > iouMatrix(this->_tracklets.size(), vector (tgts.targets.size(), 0)); + for (int i=0; i_tracklets.size(); i++) + { + for (int j=0; j_iou(this->_tracklets[i], box); + } + } + + vector > matches = this->_hungarian(iouMatrix); + for (auto& match : matches) + { + int trackletIndex = match.first; + int detectionIndex = match.second; + if (trackletIndex >= 0 && detectionIndex >= 0) + { + if (iouMatrix[match.first][match.second] <= 1-_iou_threshold) // iou_thrshold + { + sv::Box box; + tgts.targets[detectionIndex].getBox(box); + this->_tracklets[trackletIndex].age = 0; + this->_tracklets[trackletIndex].hits++; + this->_tracklets[trackletIndex].frame_id = tgts.frame_id; + this->_tracklets[trackletIndex].bbox << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, box.x2-box.x1, box.y2-box.y1; + tgts.targets[detectionIndex].tracked_id = this->_tracklets[trackletIndex].id; + match_det[detectionIndex] = trackletIndex; + } + } + } + std::vector > ().swap(iouMatrix); + for (int i=0; i_next_tracklet_id; + tracklet.bbox << box.x1+(box.x2-box.x1)/2, (double)(box.y1+(box.y2-box.y1)/2), box.x2-box.x1, box.y2-box.y1; // c_x, c_y, w, h + tracklet.age = 0; + tracklet.hits = 1; + tracklet.frame_id = tgts.frame_id; + tracklet.category_id = tgts.targets[i].category_id; + tracklet.tentative = true; + + pair, Matrix > new_motion = kf.initiate(tracklet.bbox); + tracklet.mean = new_motion.first; + tracklet.covariance = new_motion.second; + + tgts.targets[i].tracked_id = this->_next_tracklet_id; + tgts.targets[i].has_tid = true; + this->_tracklets.push_back(tracklet); + } + else + { + sv::Box box; + int track_id = match_det[i]; + tgts.targets[i].getBox(box); + pair, Matrix > updated = kf.update(this->_tracklets[track_id].mean, this->_tracklets[track_id].covariance, box); + this->_tracklets[track_id].mean = updated.first; + this->_tracklets[track_id].covariance = updated.second; + } + } + + //sift tracklets + for (auto& tracklet : this->_tracklets) + { + if (tracklet.hits >= _min_hits) + { + tracklet.tentative = false; + } + if ((tgts.frame_id-tracklet.frame_id <= _max_age) && !(tracklet.tentative && tracklet.frame_id != tgts.frame_id)) + { + _new_tracklets.push_back(tracklet); + } + } + _tracklets = _new_tracklets; + std::vector ().swap(_new_tracklets); + } +} + +vector SORT::getTracklets() const +{ + return this->_tracklets; +} + +double SORT::_iou(Tracklet& tracklet, sv::Box& box) +{ + double trackletX1 = tracklet.bbox(0)-tracklet.bbox(2)/2; + double trackletY1 = tracklet.bbox(1)-tracklet.bbox(3)/2; + double trackletX2 = tracklet.bbox(0) + tracklet.bbox(2)/2; + double trackletY2 = tracklet.bbox(1) + tracklet.bbox(3)/2; + + double detectionX1 = box.x1; + double detectionY1 = box.y1; + double detectionX2 = box.x2; + double detectionY2 = box.y2; + + double intersectionX1 = max(trackletX1, detectionX1); + double intersectionY1 = max(trackletY1, detectionY1); + double intersectionX2 = min(trackletX2, detectionX2); + double intersectionY2 = min(trackletY2, detectionY2); + + double w = (intersectionX2-intersectionX1 > 0.0) ? (intersectionX2-intersectionX1) : 0.0; + double h = (intersectionY2-intersectionY1 > 0.0) ? (intersectionY2-intersectionY1) : 0.0; + double intersectionArea = w * h; + + double trackletArea = tracklet.bbox(2) * tracklet.bbox(3); + + double detectionArea = (box.x2-box.x1) * (box.y2-box.y1); + double unionArea = trackletArea + detectionArea - intersectionArea; + double iou = (1 - intersectionArea / unionArea * 1.0); + + return iou; +} + +// Function to find the minimum element in a vector +double SORT::_findMin(const std::vector& vec) { + double minVal = std::numeric_limits::max(); + for (double val : vec) { + if (val < minVal) { + minVal = val; + } + } + return minVal; +} + +// Function to subtract the minimum value from each row of the cost matrix +void SORT::_subtractMinFromRows(std::vector>& costMatrix) { + for (auto& row : costMatrix) { + double minVal = _findMin(row); + for (double& val : row) { + val -= minVal; + } + } +} + +// Function to subtract the minimum value from each column of the cost matrix +void SORT::_subtractMinFromCols(std::vector>& costMatrix) { + for (size_t col = 0; col < costMatrix[0].size(); ++col) { + double minVal = std::numeric_limits::max(); + for (size_t row = 0; row < costMatrix.size(); ++row) { + if (costMatrix[row][col] < minVal) { + minVal = costMatrix[row][col]; + } + } + for (size_t row = 0; row < costMatrix.size(); ++row) { + costMatrix[row][col] -= minVal; + } + } +} + +// Function to find a matching using the Hungarian algorithm +vector > SORT::_hungarian(vector > costMatrix) +{ + size_t numRows = costMatrix.size(); + size_t numCols = costMatrix[0].size(); + + //transpose the matrix if necessary + const bool transposed = numCols > numRows; + if (transposed) { + vector> transposedMatrix(numCols, vector(numRows)); + for (int i = 0; i < numRows; i++) + { + for (int j = 0; j < numCols; j++) + { + transposedMatrix[j][i] = costMatrix[i][j]; + } + } + costMatrix = transposedMatrix; + swap(numRows, numCols); + } + // Determine the larger dimension for matching + size_t maxDim = std::max(numRows, numCols); + + // Create a square cost matrix by padding with zeros if necessary + std::vector> squareMatrix(maxDim, std::vector(maxDim, 0.0)); + for (size_t row = 0; row < numRows; ++row) { + for (size_t col = 0; col < numCols; ++col) { + squareMatrix[row][col] = costMatrix[row][col]; + } + } + + // Subtract the minimum value from each row and column + _subtractMinFromRows(squareMatrix); + _subtractMinFromCols(squareMatrix); + + // Initialize the assignment vectors with -1 values + std::vector rowAssignment(maxDim, -1); + std::vector colAssignment(maxDim, -1); + + // Perform the matching + for (size_t row = 0; row < maxDim; ++row) { + std::vector visitedCols(maxDim, false); + for (size_t col = 0; col < maxDim; ++col) { + if (squareMatrix[row][col] == 0 && colAssignment[col] == -1) { + rowAssignment[row] = col; + colAssignment[col] = row; + break; + } + } + } + + // Convert the assignment vectors to pair format + std::vector> assignmentPairs; + for (size_t row = 0; row < numRows; ++row) { + int col = rowAssignment[row]; + if (col != -1) { + if (col >= numCols) { + col = -1; + } + assignmentPairs.emplace_back(row, col); + } + } + if (transposed) { + for (auto& assignment : assignmentPairs) + { + swap(assignment.first, assignment.second); + } + } + return assignmentPairs; + } +} + diff --git a/algorithm/mot/sort/sort.h b/algorithm/mot/sort/sort.h new file mode 100755 index 0000000..df43ee6 --- /dev/null +++ b/algorithm/mot/sort/sort.h @@ -0,0 +1,77 @@ +#ifndef __SV_SORT__ +#define __SV_SORT__ + +#include "sv_core.h" +#include +#include +#include +#include +#include +#include + + +namespace sv { + + + +// define the tracklet struct to store the tracked objects. +struct Tracklet +{ +/* data */ +public: + Eigen::Vector4d bbox; // double x, y, w, h; + int id = 0; + int age; + int hits; + int misses; + int frame_id = 0; + int category_id; + bool tentative; + std::vector features; + Eigen::Matrix mean; + Eigen::Matrix covariance; +}; + + +class KalmanFilter { +public: + KalmanFilter(); + ~KalmanFilter(); + std::pair, Eigen::Matrix > initiate(Eigen::Vector4d &bbox); + std::pair, Eigen::Matrix > update(Eigen::Matrix mean, Eigen::Matrix covariances, Box &box); + std::pair, Eigen::Matrix > predict(Eigen::Matrix mean, Eigen::Matrix covariances); + std::pair, Eigen::Matrix > project(Eigen::Matrix mean, Eigen::Matrix covariances); +private: + Eigen::Matrix _F; + Eigen::Matrix _H; + Eigen::Matrix _chi2inv95; + double _std_weight_position; + double _std_weight_vel; +}; + + +class SORT { +public: + SORT(double iou_threshold, int max_age, int min_hits): _iou_threshold(iou_threshold), _max_age(max_age), _min_hits(min_hits), _next_tracklet_id(0) {}; + ~SORT(); + void update(TargetsInFrame &tgts); + std::vector getTracklets() const; +private: + double _iou(Tracklet &tracklet, Box &box); + std::vector> _hungarian(std::vector> costMatrix); + double _findMin(const std::vector& vec); + void _subtractMinFromRows(std::vector>& costMatrix); + void _subtractMinFromCols(std::vector>& costMatrix); + //bool _augment(const std::vector>& costMatrix, int row, std::vector& rowMatch, std::vector& colMatch, std::vector& visited); + + double _iou_threshold; + int _max_age; + int _min_hits; + int _next_tracklet_id; + std::vector _tracklets; + std::vector _new_tracklets; +}; + + +} +#endif diff --git a/algorithm/mot/sv_mot.cpp b/algorithm/mot/sv_mot.cpp old mode 100644 new mode 100755 index c60f376..0adcbf5 --- a/algorithm/mot/sv_mot.cpp +++ b/algorithm/mot/sv_mot.cpp @@ -5,6 +5,8 @@ #include #include "gason.h" #include "sv_util.h" +#include "sort.h" +#include "BYTETracker.h" using namespace std; using namespace Eigen; @@ -16,16 +18,22 @@ MultipleObjectTracker::MultipleObjectTracker() { this->_params_loaded = false; this->_sort_impl = NULL; + this->_bytetrack_impl = NULL; } MultipleObjectTracker::~MultipleObjectTracker() { if (this->_sort_impl) delete this->_sort_impl; + else if (this->_bytetrack_impl) + delete this->_bytetrack_impl; } sv::TargetsInFrame MultipleObjectTracker::track(cv::Mat img_, TargetsInFrame& tgts_) { sv::TargetsInFrame person_tgts(tgts_.frame_id); + person_tgts.width = img_.size().width; + person_tgts.height = img_.size().height; + if (!this->_params_loaded) { this->_load(); @@ -43,6 +51,18 @@ sv::TargetsInFrame MultipleObjectTracker::track(cv::Mat img_, TargetsInFrame& tg } this->_sort_impl->update(person_tgts); } + else if ("bytetrack" == this->_algorithm && this->_bytetrack_impl) + { + this->_detector->detect(img_, tgts_); + for (auto target : tgts_.targets) + { + if (target.category_id == 0) + { + person_tgts.targets.push_back(target); + } + } + this->_bytetrack_impl->update(person_tgts); + } return person_tgts; } @@ -57,6 +77,10 @@ void MultipleObjectTracker::init(CommonObjectDetector* detector_) { this->_sort_impl = new SORT(this->_iou_thres, this->_max_age, this->_min_hits); } + else if("bytetrack" == this->_algorithm) + { + this->_bytetrack_impl = new BYTETracker(this->_frame_rate, this->_track_buffer); + } this->_detector = detector_; } @@ -80,7 +104,7 @@ void MultipleObjectTracker::_load() else this->_with_reid = false; - std::cout << "with_reid: " << this->_with_reid << std::endl; + //std::cout << "with_reid: " << this->_with_reid << std::endl; } else if ("reid_input_size" == std::string(i->key) && i->value.getTag() == JSON_ARRAY) { int jcnt = 0; @@ -93,394 +117,39 @@ void MultipleObjectTracker::_load() } jcnt += 1; } - std::cout << "reid_input_w: " << this->_reid_input_w << std::endl; - std::cout << "reid_input_h: " << this->_reid_input_h << std::endl; + //std::cout << "reid_input_w: " << this->_reid_input_w << std::endl; + //std::cout << "reid_input_h: " << this->_reid_input_h << std::endl; } else if ("reid_num_samples" == std::string(i->key)) { this->_reid_num_samples = i->value.toNumber(); - std::cout << "reid_num_samples: " << this->_reid_num_samples << std::endl; + //std::cout << "reid_num_samples: " << this->_reid_num_samples << std::endl; } else if ("reid_match_thres" == std::string(i->key)) { this->_reid_match_thres = i->value.toNumber(); - std::cout << "reid_match_thres: " << this->_reid_match_thres << std::endl; + //std::cout << "reid_match_thres: " << this->_reid_match_thres << std::endl; } else if ("iou_thres" == std::string(i->key)) { this->_iou_thres = i->value.toNumber(); - std::cout << "iou_thres: " << this->_iou_thres << std::endl; + //std::cout << "iou_thres: " << this->_iou_thres << std::endl; } else if ("max_age" == std::string(i->key)) { this->_max_age = i->value.toNumber(); - std::cout << "max_age: " << this->_max_age << std::endl; + //std::cout << "max_age: " << this->_max_age << std::endl; } else if ("min_hits" == std::string(i->key)) { this->_min_hits = i->value.toNumber(); - std::cout << "min_hits: " << this->_min_hits << std::endl; + //std::cout << "min_hits: " << this->_min_hits << std::endl; + } + else if ("frame_rate" == std::string(i->key)) { + this->_frame_rate = i->value.toNumber(); + //std::cout << "max_age: " << this->_max_age << std::endl; + } + else if ("track_buffer" == std::string(i->key)) { + this->_track_buffer = i->value.toNumber(); + //std::cout << "min_hits: " << this->_min_hits << std::endl; } } } - - -KalmanFilter::KalmanFilter() -{ - this->_chi2inv95 << 3.8415, 5.9915, 7.8147, 9.4877, 11.070, 12.592, 14.067, 15.507, 16.919; - this->_F = MatrixXd::Identity(8, 8); - for (int i=0; i<4; i++) - { - this->_F(i,i+4) = 1.; //1 - } - this->_H = MatrixXd::Identity(4, 8); - this->_std_weight_position = 1. / 20; //1./20 - this->_std_weight_vel = 1. / 160; //1./160 -} - -KalmanFilter::~KalmanFilter() -{ -} - -pair, Matrix > KalmanFilter::initiate(Vector4d &bbox) -{ - Matrix mean; - Matrix zero_vector; - zero_vector.setZero(); - mean << bbox(0), bbox(1), (double)bbox(2) / (double)bbox(3), bbox(3), zero_vector; - VectorXd stds(8); - stds << 2 * this->_std_weight_position * mean(3), 2 * this->_std_weight_position * mean(3), 0.01, 2 * this->_std_weight_position * mean(3), \ - 10 * this->_std_weight_vel * mean(3), 10 * this->_std_weight_vel * mean(3), 1e-5, 10 * this->_std_weight_vel * mean(3); - MatrixXd squared = stds.array().square(); - Matrix covariances; - covariances = squared.asDiagonal(); - return make_pair(mean, covariances); -} - -pair, Matrix > KalmanFilter::update(Matrix mean, Matrix covariances, sv::Box &box) -{ - VectorXd measurement(4); - double a = (double)(box.x2-box.x1) / (double)(box.y2-box.y1); - measurement << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, a, box.y2-box.y1; - pair, Matrix > projected = project(mean, covariances); - Matrix projected_mean = projected.first; - Matrix projected_cov = projected.second; - - Eigen::LLT chol_factor(projected_cov); - MatrixXd Kalman_gain = (chol_factor.solve((covariances * this->_H.transpose()).transpose())).transpose(); - - VectorXd innovation = measurement - projected_mean; - Matrix new_mean = mean + Kalman_gain *innovation; - Matrix new_covariances = covariances - Kalman_gain * projected_cov * Kalman_gain.transpose(); - - return make_pair(new_mean, new_covariances); -} - -pair, Matrix > KalmanFilter::project(Matrix mean, Matrix covariances) -{ - VectorXd stds(4); - stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 0.1, this->_std_weight_position * mean(3); - MatrixXd squared = stds.array().square(); - MatrixXd R = squared.asDiagonal(); - - Matrix pro_mean = this->_H * mean; - Matrix pro_covariances = this->_H * covariances * this->_H.transpose() + R; - return make_pair(pro_mean, pro_covariances); -} -pair, Matrix > KalmanFilter::predict(Matrix mean, Matrix covariances) -{ - VectorXd stds(8); - stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 1e-2, this->_std_weight_position * mean(3), \ - this->_std_weight_vel * mean(3), this->_std_weight_vel * mean(3), 1e-5, this->_std_weight_vel * mean(3); // a = 0.01 - MatrixXd squared = stds.array().square(); - MatrixXd Q = squared.asDiagonal(); - Matrix pre_mean = this->_F * mean; - Matrix pre_cov = this->_F * covariances * this->_F.transpose()+Q;//+Q - return make_pair(pre_mean, pre_cov); -} - - -SORT::~SORT() -{ -} - -void SORT::update(TargetsInFrame& tgts) -{ - sv::KalmanFilter kf; - if (! this->_tracklets.size() || tgts.targets.size() == 0) - { - Vector4d bbox; - for (int i=0; i_next_tracklet_id; - tgts.targets[i].tracked_id = this->_next_tracklet_id; - tgts.targets[i].has_tid = true; - - tracklet.bbox << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, box.x2-box.x1, box.y2-box.y1; // x,y,w,h; center(x,y) - - tracklet.age = 0; - tracklet.hits = 1; - //tracklet.misses = 0; - tracklet.frame_id = tgts.frame_id; - tracklet.category_id = tgts.targets[i].category_id; - if (tgts.frame_id == 0) - { - tracklet.tentative = false; - } - else - { - tracklet.tentative = true; - } - // initate the motion - pair, Matrix > motion = kf.initiate(tracklet.bbox); - tracklet.mean = motion.first; - tracklet.covariance = motion.second; - - this->_tracklets.push_back(tracklet); - } - } - else - { - for (int i=0; i match_det(tgts.targets.size(), -1); - // predict the next state of each tracklet - for (auto& tracklet : this->_tracklets) - { - tracklet.age++; - pair, Matrix > motion = kf.predict(tracklet.mean, tracklet.covariance); - tracklet.bbox << motion.first(0), motion.first(1), motion.first(2) * motion.first(3), motion.first(3); - tracklet.mean = motion.first; - tracklet.covariance = motion.second; - } - - // Match the detections to the existing tracklets - vector > iouMatrix(this->_tracklets.size(), vector (tgts.targets.size(), 0)); - for (int i=0; i_tracklets.size(); i++) - { - for (int j=0; j_iou(this->_tracklets[i], box); - } - } - - vector > matches = this->_hungarian(iouMatrix); - for (auto& match : matches) - { - int trackletIndex = match.first; - int detectionIndex = match.second; - if (trackletIndex >= 0 && detectionIndex >= 0) - { - if (iouMatrix[match.first][match.second] <= 1-_iou_threshold) // iou_thrshold - { - sv::Box box; - tgts.targets[detectionIndex].getBox(box); - this->_tracklets[trackletIndex].age = 0; - this->_tracklets[trackletIndex].hits++; - this->_tracklets[trackletIndex].frame_id = tgts.frame_id; - this->_tracklets[trackletIndex].bbox << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, box.x2-box.x1, box.y2-box.y1; - tgts.targets[detectionIndex].tracked_id = this->_tracklets[trackletIndex].id; - match_det[detectionIndex] = trackletIndex; - } - } - } - std::vector > ().swap(iouMatrix); - for (int i=0; i_next_tracklet_id; - tracklet.bbox << box.x1+(box.x2-box.x1)/2, (double)(box.y1+(box.y2-box.y1)/2), box.x2-box.x1, box.y2-box.y1; // c_x, c_y, w, h - tracklet.age = 0; - tracklet.hits = 1; - tracklet.frame_id = tgts.frame_id; - tracklet.category_id = tgts.targets[i].category_id; - tracklet.tentative = true; - - pair, Matrix > new_motion = kf.initiate(tracklet.bbox); - tracklet.mean = new_motion.first; - tracklet.covariance = new_motion.second; - - tgts.targets[i].tracked_id = this->_next_tracklet_id; - tgts.targets[i].has_tid = true; - this->_tracklets.push_back(tracklet); - } - else - { - sv::Box box; - int track_id = match_det[i]; - tgts.targets[i].getBox(box); - pair, Matrix > updated = kf.update(this->_tracklets[track_id].mean, this->_tracklets[track_id].covariance, box); - this->_tracklets[track_id].mean = updated.first; - this->_tracklets[track_id].covariance = updated.second; - } - } - - //sift tracklets - for (auto& tracklet : this->_tracklets) - { - if (tracklet.hits >= _min_hits) - { - tracklet.tentative = false; - } - if ((tgts.frame_id-tracklet.frame_id <= _max_age) && !(tracklet.tentative && tracklet.frame_id != tgts.frame_id)) - { - _new_tracklets.push_back(tracklet); - } - } - _tracklets = _new_tracklets; - std::vector ().swap(_new_tracklets); - } -} - -vector SORT::getTracklets() const -{ - return this->_tracklets; -} - -double SORT::_iou(Tracklet& tracklet, sv::Box& box) -{ - double trackletX1 = tracklet.bbox(0)-tracklet.bbox(2)/2; - double trackletY1 = tracklet.bbox(1)-tracklet.bbox(3)/2; - double trackletX2 = tracklet.bbox(0) + tracklet.bbox(2)/2; - double trackletY2 = tracklet.bbox(1) + tracklet.bbox(3)/2; - - double detectionX1 = box.x1; - double detectionY1 = box.y1; - double detectionX2 = box.x2; - double detectionY2 = box.y2; - - double intersectionX1 = max(trackletX1, detectionX1); - double intersectionY1 = max(trackletY1, detectionY1); - double intersectionX2 = min(trackletX2, detectionX2); - double intersectionY2 = min(trackletY2, detectionY2); - - double w = (intersectionX2-intersectionX1 > 0.0) ? (intersectionX2-intersectionX1) : 0.0; - double h = (intersectionY2-intersectionY1 > 0.0) ? (intersectionY2-intersectionY1) : 0.0; - double intersectionArea = w * h; - - double trackletArea = tracklet.bbox(2) * tracklet.bbox(3); - - double detectionArea = (box.x2-box.x1) * (box.y2-box.y1); - double unionArea = trackletArea + detectionArea - intersectionArea; - double iou = (1 - intersectionArea / unionArea * 1.0); - - return iou; -} - -// Function to find the minimum element in a vector -double SORT::_findMin(const std::vector& vec) { - double minVal = std::numeric_limits::max(); - for (double val : vec) { - if (val < minVal) { - minVal = val; - } - } - return minVal; -} - -// Function to subtract the minimum value from each row of the cost matrix -void SORT::_subtractMinFromRows(std::vector>& costMatrix) { - for (auto& row : costMatrix) { - double minVal = _findMin(row); - for (double& val : row) { - val -= minVal; - } - } -} - -// Function to subtract the minimum value from each column of the cost matrix -void SORT::_subtractMinFromCols(std::vector>& costMatrix) { - for (size_t col = 0; col < costMatrix[0].size(); ++col) { - double minVal = std::numeric_limits::max(); - for (size_t row = 0; row < costMatrix.size(); ++row) { - if (costMatrix[row][col] < minVal) { - minVal = costMatrix[row][col]; - } - } - for (size_t row = 0; row < costMatrix.size(); ++row) { - costMatrix[row][col] -= minVal; - } - } -} - -// Function to find a matching using the Hungarian algorithm -vector > SORT::_hungarian(vector > costMatrix) -{ - size_t numRows = costMatrix.size(); - size_t numCols = costMatrix[0].size(); - - //transpose the matrix if necessary - const bool transposed = numCols > numRows; - if (transposed) { - vector> transposedMatrix(numCols, vector(numRows)); - for (int i = 0; i < numRows; i++) - { - for (int j = 0; j < numCols; j++) - { - transposedMatrix[j][i] = costMatrix[i][j]; - } - } - costMatrix = transposedMatrix; - swap(numRows, numCols); - } - // Determine the larger dimension for matching - size_t maxDim = std::max(numRows, numCols); - - // Create a square cost matrix by padding with zeros if necessary - std::vector> squareMatrix(maxDim, std::vector(maxDim, 0.0)); - for (size_t row = 0; row < numRows; ++row) { - for (size_t col = 0; col < numCols; ++col) { - squareMatrix[row][col] = costMatrix[row][col]; - } - } - - // Subtract the minimum value from each row and column - _subtractMinFromRows(squareMatrix); - _subtractMinFromCols(squareMatrix); - - // Initialize the assignment vectors with -1 values - std::vector rowAssignment(maxDim, -1); - std::vector colAssignment(maxDim, -1); - - // Perform the matching - for (size_t row = 0; row < maxDim; ++row) { - std::vector visitedCols(maxDim, false); - for (size_t col = 0; col < maxDim; ++col) { - if (squareMatrix[row][col] == 0 && colAssignment[col] == -1) { - rowAssignment[row] = col; - colAssignment[col] = row; - break; - } - } - } - - // Convert the assignment vectors to pair format - std::vector> assignmentPairs; - for (size_t row = 0; row < numRows; ++row) { - int col = rowAssignment[row]; - if (col != -1) { - if (col >= numCols) { - col = -1; - } - assignmentPairs.emplace_back(row, col); - } - } - if (transposed) { - for (auto& assignment : assignmentPairs) - { - swap(assignment.first, assignment.second); - } - } - return assignmentPairs; - } } diff --git a/include/sv_mot.h b/include/sv_mot.h index 07fb5b8..1fcc6e8 100644 --- a/include/sv_mot.h +++ b/include/sv_mot.h @@ -15,7 +15,7 @@ namespace sv { class SORT; - +class BYTETracker; class MultipleObjectTracker : public CameraAlgorithm { @@ -39,70 +39,12 @@ private: double _iou_thres; int _max_age; int _min_hits; + int _frame_rate; + int _track_buffer; SORT* _sort_impl; + BYTETracker* _bytetrack_impl; CommonObjectDetector* _detector; }; - - -// define the tracklet struct to store the tracked objects. -struct Tracklet -{ -/* data */ -public: - Eigen::Vector4d bbox; // double x, y, w, h; - int id = 0; - int age; - int hits; - int misses; - int frame_id = 0; - int category_id; - bool tentative; - std::vector features; - Eigen::Matrix mean; - Eigen::Matrix covariance; -}; - - -class KalmanFilter { -public: - KalmanFilter(); - ~KalmanFilter(); - std::pair, Eigen::Matrix > initiate(Eigen::Vector4d &bbox); - std::pair, Eigen::Matrix > update(Eigen::Matrix mean, Eigen::Matrix covariances, Box &box); - std::pair, Eigen::Matrix > predict(Eigen::Matrix mean, Eigen::Matrix covariances); - std::pair, Eigen::Matrix > project(Eigen::Matrix mean, Eigen::Matrix covariances); -private: - Eigen::Matrix _F; - Eigen::Matrix _H; - Eigen::Matrix _chi2inv95; - double _std_weight_position; - double _std_weight_vel; -}; - - -class SORT { -public: - SORT(double iou_threshold, int max_age, int min_hits): _iou_threshold(iou_threshold), _max_age(max_age), _min_hits(min_hits), _next_tracklet_id(0) {}; - ~SORT(); - void update(TargetsInFrame &tgts); - std::vector getTracklets() const; -private: - double _iou(Tracklet &tracklet, Box &box); - std::vector> _hungarian(std::vector> costMatrix); - double _findMin(const std::vector& vec); - void _subtractMinFromRows(std::vector>& costMatrix); - void _subtractMinFromCols(std::vector>& costMatrix); - //bool _augment(const std::vector>& costMatrix, int row, std::vector& rowMatch, std::vector& colMatch, std::vector& visited); - - double _iou_threshold; - int _max_age; - int _min_hits; - int _next_tracklet_id; - std::vector _tracklets; - std::vector _new_tracklets; -}; - - } #endif