floraachy
/
SpireCV
forked from floratest1/SpireCV
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add a new mot Bytetrack.

This commit is contained in:
Daniel 2024-05-25 18:29:11 +08:00
parent 091d50e66b
commit a58496dbbd
15 changed files with 2125 additions and 433 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
CMakeLists.txt
View File

@ -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)

262
algorithm/mot/bytetrack/BYTETracker.cpp Executable file
View File

@ -0,0 +1,262 @@
#include "BYTETracker.h"
#include <fstream>
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<STrack> activated_stracks;
std::vector<STrack> refind_stracks;
std::vector<STrack> removed_stracks;
std::vector<STrack> lost_stracks;
std::vector<STrack> detections;
std::vector<STrack> detections_low;
std::vector<STrack> detections_cp;
std::vector<STrack> tracked_stracks_swap;
std::vector<STrack> resa, resb;
std::vector<STrack> output_stracks;
std::vector<STrack *> unconfirmed;
std::vector<STrack *> tracked_stracks;
std::vector<STrack *> strack_pool;
std::vector<STrack *> 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<float> 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<std::vector<float>> dists;
int dist_size = 0, dist_size_size = 0;
dists = iou_distance(strack_pool, detections, dist_size, dist_size_size);
std::vector<std::vector<int>> matches;
std::vector<int> 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<int> 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<float> 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);
}
}
}
}

56
algorithm/mot/bytetrack/BYTETracker.h Executable file
View File

@ -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_<float> 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<STrack*> joint_stracks( std::vector<STrack*> &tlista, std::vector<STrack> &tlistb);
std::vector<STrack> joint_stracks( std::vector<STrack> &tlista, std::vector<STrack> &tlistb);
std::vector<STrack> sub_stracks( std::vector<STrack> &tlista, std::vector<STrack> &tlistb);
void remove_duplicate_stracks( std::vector<STrack> &resa, std::vector<STrack> &resb, std::vector<STrack> &stracksa, std::vector<STrack> &stracksb);
void linear_assignment( std::vector< std::vector<float> > &cost_matrix, int cost_matrix_size, int cost_matrix_size_size, float thresh,
std::vector< std::vector<int> > &matches, std::vector<int> &unmatched_a, std::vector<int> &unmatched_b);
std::vector< std::vector<float> > iou_distance( std::vector<STrack*> &atracks, std::vector<STrack> &btracks, int &dist_size, int &dist_size_size);
std::vector< std::vector<float> > iou_distance( std::vector<STrack> &atracks, std::vector<STrack> &btracks);
std::vector< std::vector<float> > ious( std::vector< std::vector<float> > &atlbrs, std::vector< std::vector<float> > &btlbrs);
double lapjv(const std::vector< std::vector<float> > &cost, std::vector<int> &rowsol, std::vector<int> &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<STrack> tracked_stracks;
std::vector<STrack> lost_stracks;
std::vector<STrack> removed_stracks;
byte_kalman::ByteKalmanFilter kalman_filter;
};
}
#endif

152
algorithm/mot/bytetrack/BytekalmanFilter.cpp Executable file
View File

@ -0,0 +1,152 @@
#include "BytekalmanFilter.h"
#include <Eigen/Cholesky>
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<float, 4, 4> 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<float, 4, 8> B = (covariance * (_update_mat.transpose())).transpose();
Eigen::Matrix<float, 8, 4> kalman_gain = (projected_cov.llt().solve(B)).transpose(); // eg.8x4
Eigen::Matrix<float, 1, 4> 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<float, 1, -1>
ByteKalmanFilter::gating_distance(
const KAL_MEAN &mean,
const KAL_COVA &covariance,
const std::vector<DETECTBOX> &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<float, -1, 4, Eigen::RowMajor> d(size, 4);
DETECTBOXSS d(measurements.size(), 4);
int pos = 0;
for (DETECTBOX box : measurements) {
d.row(pos++) = box - mean1;
}
Eigen::Matrix<float, -1, -1, Eigen::RowMajor> factor = covariance1.llt().matrixL();
Eigen::Matrix<float, -1, -1> z = factor.triangularView<Eigen::Lower>().solve<Eigen::OnTheRight>(d).transpose();
auto zz = ((z.array())*(z.array())).matrix();
auto square_maha = zz.colwise().sum();
return square_maha;
}
}

31
algorithm/mot/bytetrack/BytekalmanFilter.h Executable file
View File

@ -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<float, 1, -1> gating_distance(
const KAL_MEAN& mean,
const KAL_COVA& covariance,
const std::vector<DETECTBOX>& measurements,
bool only_position = false);
private:
Eigen::Matrix<float, 8, 8, Eigen::RowMajor> _motion_mat;
Eigen::Matrix<float, 4, 8, Eigen::RowMajor> _update_mat;
float _std_weight_position;
float _std_weight_velocity;
};
}

192
algorithm/mot/bytetrack/STrack.cpp Executable file
View File

@ -0,0 +1,192 @@
#include "STrack.h"
STrack::STrack( std::vector<float> 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<float> _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<float> 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<float> 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<float> 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<float> STrack::tlwh_to_xyah( std::vector<float> tlwh_tmp)
{
std::vector<float> 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<float> STrack::to_xyah()
{
return tlwh_to_xyah(tlwh);
}
std::vector<float> STrack::tlbr_to_tlwh( std::vector<float> &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<STrack*> &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);
}
}

47
algorithm/mot/bytetrack/STrack.h Executable file
View File

@ -0,0 +1,47 @@
#pragma once
#include <opencv2/opencv.hpp>
#include "BytekalmanFilter.h"
enum TrackState { New = 0, Tracked, Lost, Removed };
class STrack
{
public:
STrack( std::vector<float> tlwh_, float score);
~STrack();
std::vector<float> static tlbr_to_tlwh( std::vector<float> &tlbr);
void static multi_predict( std::vector<STrack*> &stracks, byte_kalman::ByteKalmanFilter &kalman_filter);
void static_tlwh();
void static_tlbr();
std::vector<float> tlwh_to_xyah( std::vector<float> tlwh_tmp);
std::vector<float> 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<float> _tlwh;
std::vector<float> tlwh;
std::vector<float> tlbr;
int frame_id;
int tracklet_len;
int start_frame;
KAL_MEAN mean;
KAL_COVA covariance;
float score;
private:
byte_kalman::ByteKalmanFilter kalman_filter;
};

43
algorithm/mot/bytetrack/dataType.h Executable file
View File

@ -0,0 +1,43 @@
#pragma once
#include <cstddef>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>
//const int k_feature_dim=512;//feature dim
typedef Eigen::Matrix<float, 1, 4, Eigen::RowMajor> DETECTBOX;
typedef Eigen::Matrix<float, -1, 4, Eigen::RowMajor> DETECTBOXSS;
//typedef Eigen::Matrix<float, 1, k_feature_dim, Eigen::RowMajor> FEATURE;
//typedef Eigen::Matrix<float, Eigen::Dynamic, k_feature_dim, Eigen::RowMajor> FEATURESS;
//typedef std::vector<FEATURE> FEATURESS;
//Kalmanfilter
//typedef Eigen::Matrix<float, 8, 8, Eigen::RowMajor> KAL_FILTER;
typedef Eigen::Matrix<float, 1, 8, Eigen::RowMajor> KAL_MEAN;
typedef Eigen::Matrix<float, 8, 8, Eigen::RowMajor> KAL_COVA;
typedef Eigen::Matrix<float, 1, 4, Eigen::RowMajor> KAL_HMEAN;
typedef Eigen::Matrix<float, 4, 4, Eigen::RowMajor> KAL_HCOVA;
using KAL_DATA = std::pair<KAL_MEAN, KAL_COVA>;
using KAL_HDATA = std::pair<KAL_HMEAN, KAL_HCOVA>;
//main
//using RESULT_DATA = std::pair<int, DETECTBOX>;
//tracker:
//using TRACKER_DATA = std::pair<int, FEATURESS>;
//using MATCH_DATA = std::pair<int, int>;
// typedef struct t{
// std::vector<MATCH_DATA> matches;
// std::vector<int> unmatched_tracks;
// std::vector<int> unmatched_detections;
// }TRACHER_MATCHD;
//linear_assignment:
//typedef Eigen::Matrix<float, -1, -1, Eigen::RowMajor> DYNAMICM;

343
algorithm/mot/bytetrack/lapjv.cpp Executable file
View File

@ -0,0 +1,343 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#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;
}

63
algorithm/mot/bytetrack/lapjv.h Executable file
View File

@ -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 <assert.h>
#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

432
algorithm/mot/bytetrack/utils.cpp Executable file
View File

@ -0,0 +1,432 @@
#include "BYTETracker.h"
#include "lapjv.h"
namespace sv {
std::vector<STrack*> BYTETracker::joint_stracks( std::vector<STrack*> &tlista, std::vector<STrack> &tlistb)
{
std::map<int, int> exists;
std::vector<STrack*> res;
for (int i = 0; i < tlista.size(); i++)
{
exists.insert(std::pair<int, int>(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<STrack> BYTETracker::joint_stracks( std::vector<STrack> &tlista, std::vector<STrack> &tlistb)
{
std::map<int, int> exists;
std::vector<STrack> res;
for (int i = 0; i < tlista.size(); i++)
{
exists.insert(std::pair<int, int>(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<STrack> BYTETracker::sub_stracks( std::vector<STrack> &tlista, std::vector<STrack> &tlistb)
{
std::map<int, STrack> stracks;
for (int i = 0; i < tlista.size(); i++)
{
stracks.insert(std::pair<int, STrack>(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<STrack> res;
std::map<int, STrack>::iterator it;
for (it = stracks.begin(); it != stracks.end(); ++it)
{
res.push_back(it->second);
}
return res;
}
void BYTETracker::remove_duplicate_stracks( std::vector<STrack> &resa, std::vector<STrack> &resb, std::vector<STrack> &stracksa, std::vector<STrack> &stracksb)
{
std::vector< std::vector<float> > pdist = iou_distance(stracksa, stracksb);
std::vector<std::pair<int, int> > 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<int, int>(i, j));
}
}
}
std::vector<int> 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<int>::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<int>::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<float> > &cost_matrix, int cost_matrix_size, int cost_matrix_size_size, float thresh,
std::vector< std::vector<int> > &matches, std::vector<int> &unmatched_a, std::vector<int> &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<int> rowsol; std::vector<int> colsol;
float c = lapjv(cost_matrix, rowsol, colsol, true, thresh);
for (int i = 0; i < rowsol.size(); i++)
{
if (rowsol[i] >= 0)
{
std::vector<int> 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<float> > BYTETracker::ious( std::vector< std::vector<float> > &atlbrs, std::vector< std::vector<float> > &btlbrs)
{
std::vector< std::vector<float> > 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<float> 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<float> > BYTETracker::iou_distance( std::vector<STrack*> &atracks, std::vector<STrack> &btracks, int &dist_size, int &dist_size_size)
{
std::vector< std::vector<float> > cost_matrix;
if (atracks.size() * btracks.size() == 0)
{
dist_size = atracks.size();
dist_size_size = btracks.size();
return cost_matrix;
}
std::vector< std::vector<float> > 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<float> > _ious = ious(atlbrs, btlbrs);
for (int i = 0; i < _ious.size();i++)
{
std::vector<float> _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<float> > BYTETracker::iou_distance( std::vector<STrack> &atracks, std::vector<STrack> &btracks)
{
std::vector< std::vector<float> > 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<float> > _ious = ious(atlbrs, btlbrs);
std::vector< std::vector<float> > cost_matrix;
for (int i = 0; i < _ious.size(); i++)
{
std::vector<float> _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<float> > &cost, std::vector<int> &rowsol, std::vector<int> &colsol,
bool extend_cost, float cost_limit, bool return_cost)
{
std::vector< std::vector<float> > cost_c;
cost_c.assign(cost.begin(), cost.end());
std::vector< std::vector<float> > 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);
}
}

377
algorithm/mot/sort/sort.cpp Executable file
View File

@ -0,0 +1,377 @@
#include "sort.h"
#include <cmath>
#include <fstream>
#include <limits>
#include <vector>
#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<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::initiate(Vector4d &bbox)
{
Matrix<double,8,1> mean;
Matrix<double,4,1> 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<double, 8, 8> covariances;
covariances = squared.asDiagonal();
return make_pair(mean, covariances);
}
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::update(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> 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<double, 4, 1>, Matrix<double, 4, 4> > projected = project(mean, covariances);
Matrix<double, 4, 1> projected_mean = projected.first;
Matrix<double, 4, 4> projected_cov = projected.second;
Eigen::LLT<Eigen::MatrixXd> chol_factor(projected_cov);
MatrixXd Kalman_gain = (chol_factor.solve((covariances * this->_H.transpose()).transpose())).transpose();
VectorXd innovation = measurement - projected_mean;
Matrix<double, 8, 1> new_mean = mean + Kalman_gain *innovation;
Matrix<double, 8, 8> new_covariances = covariances - Kalman_gain * projected_cov * Kalman_gain.transpose();
return make_pair(new_mean, new_covariances);
}
pair<Matrix<double, 4, 1>, Matrix<double, 4, 4> > KalmanFilter::project(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> 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<double, 4, 1> pro_mean = this->_H * mean;
Matrix<double, 4, 4> pro_covariances = this->_H * covariances * this->_H.transpose() + R;
return make_pair(pro_mean, pro_covariances);
}
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::predict(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> 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<double, 8, 1> pre_mean = this->_F * mean;
Matrix<double, 8, 8> 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<tgts.targets.size(); i++)
{
sv::Box box;
tgts.targets[i].getBox(box);
Tracklet tracklet;
tracklet.id = ++ this->_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<double, 8, 1>, Matrix<double, 8, 8> > motion = kf.initiate(tracklet.bbox);
tracklet.mean = motion.first;
tracklet.covariance = motion.second;
this->_tracklets.push_back(tracklet);
}
}
else
{
for (int i=0; i<tgts.targets.size(); i++)
{
tgts.targets[i].tracked_id = 0;
tgts.targets[i].has_tid = true;
}
vector<int> match_det(tgts.targets.size(), -1);
// predict the next state of each tracklet
for (auto& tracklet : this->_tracklets)
{
tracklet.age++;
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > 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<vector<double> > iouMatrix(this->_tracklets.size(), vector<double> (tgts.targets.size(), 0));
for (int i=0; i<this->_tracklets.size(); i++)
{
for (int j=0; j<tgts.targets.size(); j++)
{
sv::Box box;
tgts.targets[j].getBox(box);
iouMatrix[i][j] = this->_iou(this->_tracklets[i], box);
}
}
vector<pair<int, int> > 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 <vector<double>> ().swap(iouMatrix);
for (int i=0; i<tgts.targets.size(); i++)
{
if (match_det[i] == -1)
{
sv::Box box;
tgts.targets[i].getBox(box);
Tracklet tracklet;
tracklet.id = ++ this->_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<double, 8, 1>, Matrix<double, 8, 8> > 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<double, 8, 1>, Matrix<double, 8, 8> > 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 <Tracklet> ().swap(_new_tracklets);
}
}
vector<Tracklet> 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<double>& vec) {
double minVal = std::numeric_limits<double>::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<std::vector<double>>& 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<std::vector<double>>& costMatrix) {
for (size_t col = 0; col < costMatrix[0].size(); ++col) {
double minVal = std::numeric_limits<double>::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<pair<int, int> > SORT::_hungarian(vector<vector<double> > 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<vector<double>> transposedMatrix(numCols, vector<double>(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<std::vector<double>> squareMatrix(maxDim, std::vector<double>(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<int> rowAssignment(maxDim, -1);
std::vector<int> colAssignment(maxDim, -1);
// Perform the matching
for (size_t row = 0; row < maxDim; ++row) {
std::vector<bool> 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<int, int> format
std::vector<std::pair<int, int>> 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;
}
}

77
algorithm/mot/sort/sort.h Executable file
View File

@ -0,0 +1,77 @@
#ifndef __SV_SORT__
#define __SV_SORT__
#include "sv_core.h"
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/tracking.hpp>
#include <string>
#include <chrono>
#include <Eigen/Dense>
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<double> features;
Eigen::Matrix<double, 8, 1> mean;
Eigen::Matrix<double, 8, 8> covariance;
};
class KalmanFilter {
public:
KalmanFilter();
~KalmanFilter();
std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > initiate(Eigen::Vector4d &bbox);
std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > update(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances, Box &box);
std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > predict(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances);
std::pair<Eigen::Matrix<double, 4, 1>, Eigen::Matrix<double, 4, 4> > project(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances);
private:
Eigen::Matrix<double, 8, 8> _F;
Eigen::Matrix<double, 4, 8> _H;
Eigen::Matrix<double, 9, 1> _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<Tracklet> getTracklets() const;
private:
double _iou(Tracklet &tracklet, Box &box);
std::vector<std::pair<int,int>> _hungarian(std::vector<std::vector<double>> costMatrix);
double _findMin(const std::vector<double>& vec);
void _subtractMinFromRows(std::vector<std::vector<double>>& costMatrix);
void _subtractMinFromCols(std::vector<std::vector<double>>& costMatrix);
//bool _augment(const std::vector<std::vector<double>>& costMatrix, int row, std::vector<int>& rowMatch, std::vector<int>& colMatch, std::vector<bool>& visited);
double _iou_threshold;
int _max_age;
int _min_hits;
int _next_tracklet_id;
std::vector <Tracklet> _tracklets;
std::vector <Tracklet> _new_tracklets;
};
}
#endif

411
algorithm/mot/sv_mot.cpp Normal file → Executable file
View File

@ -5,6 +5,8 @@
#include <vector>
#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<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::initiate(Vector4d &bbox)
{
Matrix<double,8,1> mean;
Matrix<double,4,1> 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<double, 8, 8> covariances;
covariances = squared.asDiagonal();
return make_pair(mean, covariances);
}
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::update(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> 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<double, 4, 1>, Matrix<double, 4, 4> > projected = project(mean, covariances);
Matrix<double, 4, 1> projected_mean = projected.first;
Matrix<double, 4, 4> projected_cov = projected.second;
Eigen::LLT<Eigen::MatrixXd> chol_factor(projected_cov);
MatrixXd Kalman_gain = (chol_factor.solve((covariances * this->_H.transpose()).transpose())).transpose();
VectorXd innovation = measurement - projected_mean;
Matrix<double, 8, 1> new_mean = mean + Kalman_gain *innovation;
Matrix<double, 8, 8> new_covariances = covariances - Kalman_gain * projected_cov * Kalman_gain.transpose();
return make_pair(new_mean, new_covariances);
}
pair<Matrix<double, 4, 1>, Matrix<double, 4, 4> > KalmanFilter::project(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> 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<double, 4, 1> pro_mean = this->_H * mean;
Matrix<double, 4, 4> pro_covariances = this->_H * covariances * this->_H.transpose() + R;
return make_pair(pro_mean, pro_covariances);
}
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::predict(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> 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<double, 8, 1> pre_mean = this->_F * mean;
Matrix<double, 8, 8> 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<tgts.targets.size(); i++)
{
sv::Box box;
tgts.targets[i].getBox(box);
Tracklet tracklet;
tracklet.id = ++ this->_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<double, 8, 1>, Matrix<double, 8, 8> > motion = kf.initiate(tracklet.bbox);
tracklet.mean = motion.first;
tracklet.covariance = motion.second;
this->_tracklets.push_back(tracklet);
}
}
else
{
for (int i=0; i<tgts.targets.size(); i++)
{
tgts.targets[i].tracked_id = 0;
tgts.targets[i].has_tid = true;
}
vector<int> match_det(tgts.targets.size(), -1);
// predict the next state of each tracklet
for (auto& tracklet : this->_tracklets)
{
tracklet.age++;
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > 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<vector<double> > iouMatrix(this->_tracklets.size(), vector<double> (tgts.targets.size(), 0));
for (int i=0; i<this->_tracklets.size(); i++)
{
for (int j=0; j<tgts.targets.size(); j++)
{
sv::Box box;
tgts.targets[j].getBox(box);
iouMatrix[i][j] = this->_iou(this->_tracklets[i], box);
}
}
vector<pair<int, int> > 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 <vector<double>> ().swap(iouMatrix);
for (int i=0; i<tgts.targets.size(); i++)
{
if (match_det[i] == -1)
{
sv::Box box;
tgts.targets[i].getBox(box);
Tracklet tracklet;
tracklet.id = ++ this->_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<double, 8, 1>, Matrix<double, 8, 8> > 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<double, 8, 1>, Matrix<double, 8, 8> > 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 <Tracklet> ().swap(_new_tracklets);
}
}
vector<Tracklet> 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<double>& vec) {
double minVal = std::numeric_limits<double>::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<std::vector<double>>& 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<std::vector<double>>& costMatrix) {
for (size_t col = 0; col < costMatrix[0].size(); ++col) {
double minVal = std::numeric_limits<double>::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<pair<int, int> > SORT::_hungarian(vector<vector<double> > 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<vector<double>> transposedMatrix(numCols, vector<double>(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<std::vector<double>> squareMatrix(maxDim, std::vector<double>(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<int> rowAssignment(maxDim, -1);
std::vector<int> colAssignment(maxDim, -1);
// Perform the matching
for (size_t row = 0; row < maxDim; ++row) {
std::vector<bool> 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<int, int> format
std::vector<std::pair<int, int>> 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;
}
}

66
include/sv_mot.h
View File

@ -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<double> features;
Eigen::Matrix<double, 8, 1> mean;
Eigen::Matrix<double, 8, 8> covariance;
};
class KalmanFilter {
public:
KalmanFilter();
~KalmanFilter();
std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > initiate(Eigen::Vector4d &bbox);
std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > update(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances, Box &box);
std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > predict(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances);
std::pair<Eigen::Matrix<double, 4, 1>, Eigen::Matrix<double, 4, 4> > project(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances);
private:
Eigen::Matrix<double, 8, 8> _F;
Eigen::Matrix<double, 4, 8> _H;
Eigen::Matrix<double, 9, 1> _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<Tracklet> getTracklets() const;
private:
double _iou(Tracklet &tracklet, Box &box);
std::vector<std::pair<int,int>> _hungarian(std::vector<std::vector<double>> costMatrix);
double _findMin(const std::vector<double>& vec);
void _subtractMinFromRows(std::vector<std::vector<double>>& costMatrix);
void _subtractMinFromCols(std::vector<std::vector<double>>& costMatrix);
//bool _augment(const std::vector<std::vector<double>>& costMatrix, int row, std::vector<int>& rowMatch, std::vector<int>& colMatch, std::vector<bool>& visited);
double _iou_threshold;
int _max_age;
int _min_hits;
int _next_tracklet_id;
std::vector <Tracklet> _tracklets;
std::vector <Tracklet> _new_tracklets;
};
}
#endif