finish SORT
This commit is contained in:
parent
c561a85a3c
commit
b7782a061e
|
@ -1,6 +1,8 @@
|
||||||
#include "sv_mot.h"
|
#include "sv_mot.h"
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <fstream>
|
#include <fstream>
|
||||||
|
#include <limits>
|
||||||
|
#include <vector>
|
||||||
#include "gason.h"
|
#include "gason.h"
|
||||||
#include "sv_util.h"
|
#include "sv_util.h"
|
||||||
|
|
||||||
|
@ -116,11 +118,11 @@ KalmanFilter::KalmanFilter()
|
||||||
this->_F = MatrixXd::Identity(8, 8);
|
this->_F = MatrixXd::Identity(8, 8);
|
||||||
for (int i=0; i<4; i++)
|
for (int i=0; i<4; i++)
|
||||||
{
|
{
|
||||||
this->_F(i,i+4) = 1;
|
this->_F(i,i+4) = 1.; //1
|
||||||
}
|
}
|
||||||
this->_H = MatrixXd::Identity(4, 8);
|
this->_H = MatrixXd::Identity(4, 8);
|
||||||
this->_std_weight_position = 1. / 20;
|
this->_std_weight_position = 1. / 20; //1./20
|
||||||
this->_std_weight_vel = 1. / 160;
|
this->_std_weight_vel = 1. / 160; //1./160
|
||||||
}
|
}
|
||||||
|
|
||||||
KalmanFilter::~KalmanFilter()
|
KalmanFilter::~KalmanFilter()
|
||||||
|
@ -130,7 +132,9 @@ KalmanFilter::~KalmanFilter()
|
||||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::initiate(Vector4d &bbox)
|
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::initiate(Vector4d &bbox)
|
||||||
{
|
{
|
||||||
Matrix<double,8,1> mean;
|
Matrix<double,8,1> mean;
|
||||||
mean << bbox(0), bbox(1), bbox(2) / bbox(3), bbox(3), 0, 0, 0, 0;
|
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);
|
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), \
|
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);
|
10 * this->_std_weight_vel * mean(3), 10 * this->_std_weight_vel * mean(3), 1e-5, 10 * this->_std_weight_vel * mean(3);
|
||||||
|
@ -142,31 +146,43 @@ pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::initiate(Vector4
|
||||||
|
|
||||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::update(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> covariances, sv::Box &box)
|
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::update(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> covariances, sv::Box &box)
|
||||||
{
|
{
|
||||||
MatrixXd R;
|
|
||||||
Vector4d stds;
|
|
||||||
|
|
||||||
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();
|
|
||||||
R = squared.asDiagonal();
|
|
||||||
MatrixXd S = this->_H * covariances * this->_H.transpose() + R;
|
|
||||||
|
|
||||||
MatrixXd Kalman_gain = covariances * this->_H.transpose() * S.inverse();
|
|
||||||
VectorXd measurement(4);
|
VectorXd measurement(4);
|
||||||
measurement << box.x1, box.y1, (box.x2-box.x1) / (box.y2-box.y1), box.y2-box.y1;
|
double a = (double)(box.x2-box.x1) / (double)(box.y2-box.y1);
|
||||||
Matrix<double, 8, 1> new_mean = mean + Kalman_gain * (measurement - this->_H * mean);
|
measurement << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, a, box.y2-box.y1;
|
||||||
Matrix<double, 8, 8> new_covariances = (MatrixXd::Identity(8, 8) - Kalman_gain * this->_H) * covariances;
|
pair<Matrix<double, 4, 1>, Matrix<double, 4, 4> > projected = project(mean, covariances);
|
||||||
return make_pair(new_mean, new_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)
|
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::predict(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> covariances)
|
||||||
{
|
{
|
||||||
VectorXd stds(8);
|
VectorXd stds(8);
|
||||||
stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 0.01, this->_std_weight_position * mean(3), \
|
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);
|
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 squared = stds.array().square();
|
||||||
MatrixXd Q = squared.asDiagonal();
|
MatrixXd Q = squared.asDiagonal();
|
||||||
Matrix<double, 8, 1> pre_mean = this->_F * mean;
|
Matrix<double, 8, 1> pre_mean = this->_F * mean;
|
||||||
Matrix<double, 8, 8> pre_cov = this->_F * covariances * this->_F.transpose() + Q;
|
Matrix<double, 8, 8> pre_cov = this->_F * covariances * this->_F.transpose()+Q;//+Q
|
||||||
return make_pair(pre_mean, pre_cov);
|
return make_pair(pre_mean, pre_cov);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -178,7 +194,7 @@ SORT::~SORT()
|
||||||
void SORT::update(TargetsInFrame& tgts)
|
void SORT::update(TargetsInFrame& tgts)
|
||||||
{
|
{
|
||||||
sv::KalmanFilter kf;
|
sv::KalmanFilter kf;
|
||||||
if (! this->_tracklets.size())
|
if (! this->_tracklets.size() || tgts.targets.size() == 0)
|
||||||
{
|
{
|
||||||
Vector4d bbox;
|
Vector4d bbox;
|
||||||
for (int i=0; i<tgts.targets.size(); i++)
|
for (int i=0; i<tgts.targets.size(); i++)
|
||||||
|
@ -187,34 +203,36 @@ void SORT::update(TargetsInFrame& tgts)
|
||||||
tgts.targets[i].getBox(box);
|
tgts.targets[i].getBox(box);
|
||||||
Tracklet tracklet;
|
Tracklet tracklet;
|
||||||
tracklet.id = ++ this->_next_tracklet_id;
|
tracklet.id = ++ this->_next_tracklet_id;
|
||||||
// cout << tracklet.id << endl;
|
|
||||||
tgts.targets[i].tracked_id = this->_next_tracklet_id;
|
tgts.targets[i].tracked_id = this->_next_tracklet_id;
|
||||||
tgts.targets[i].has_tid = true;
|
tgts.targets[i].has_tid = true;
|
||||||
|
|
||||||
tracklet.bbox << box.x1, box.y1, box.x2-box.x1, box.y2-box.y1; // x,y,w,h
|
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.age = 0;
|
||||||
tracklet.hits = 1;
|
tracklet.hits = 1;
|
||||||
tracklet.misses = 0;
|
tracklet.misses = 0;
|
||||||
tracklet.frame_id = tgts.frame_id;
|
tracklet.frame_id = tgts.frame_id;
|
||||||
|
tracklet.category_id = tgts.targets[i].category_id;
|
||||||
tracklet.tentative = true;
|
tracklet.tentative = true;
|
||||||
|
|
||||||
// initate the motion
|
// initate the motion
|
||||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > motion = kf.initiate(tracklet.bbox);
|
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > motion = kf.initiate(tracklet.bbox);
|
||||||
tracklet.mean = motion.first;
|
tracklet.mean = motion.first;
|
||||||
tracklet.covariance = motion.second;
|
tracklet.covariance = motion.second;
|
||||||
|
|
||||||
this->_tracklets.push_back(tracklet);
|
this->_tracklets.push_back(tracklet);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
cout<<"frame id:"<<tgts.frame_id<<endl;
|
||||||
for (int i=0; i<tgts.targets.size(); i++)
|
for (int i=0; i<tgts.targets.size(); i++)
|
||||||
{
|
{
|
||||||
tgts.targets[i].tracked_id = 0;
|
tgts.targets[i].tracked_id = 0;
|
||||||
tgts.targets[i].has_tid = true;
|
tgts.targets[i].has_tid = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
array<int, 100> match_det;
|
vector<int> match_det(tgts.targets.size(), -1);
|
||||||
match_det.fill(-1);
|
|
||||||
// predict the next state of each tracklet
|
// predict the next state of each tracklet
|
||||||
for (auto& tracklet : this->_tracklets)
|
for (auto& tracklet : this->_tracklets)
|
||||||
{
|
{
|
||||||
|
@ -226,8 +244,6 @@ void SORT::update(TargetsInFrame& tgts)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Match the detections to the existing tracklets
|
// Match the detections to the existing tracklets
|
||||||
// cout << "the num of targets: " << tgts.targets.size() << endl;
|
|
||||||
// cout << "the num of tracklets: " << this->_tracklets.size() << endl;
|
|
||||||
vector<vector<double> > iouMatrix(this->_tracklets.size(), vector<double> (tgts.targets.size(), 0));
|
vector<vector<double> > iouMatrix(this->_tracklets.size(), vector<double> (tgts.targets.size(), 0));
|
||||||
for (int i=0; i<this->_tracklets.size(); i++)
|
for (int i=0; i<this->_tracklets.size(); i++)
|
||||||
{
|
{
|
||||||
|
@ -238,6 +254,7 @@ void SORT::update(TargetsInFrame& tgts)
|
||||||
iouMatrix[i][j] = this->_iou(this->_tracklets[i], box);
|
iouMatrix[i][j] = this->_iou(this->_tracklets[i], box);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
vector<pair<int, int> > matches = this->_hungarian(iouMatrix);
|
vector<pair<int, int> > matches = this->_hungarian(iouMatrix);
|
||||||
for (auto& match : matches)
|
for (auto& match : matches)
|
||||||
{
|
{
|
||||||
|
@ -245,50 +262,58 @@ void SORT::update(TargetsInFrame& tgts)
|
||||||
int detectionIndex = match.second;
|
int detectionIndex = match.second;
|
||||||
if (trackletIndex >= 0 && detectionIndex >= 0)
|
if (trackletIndex >= 0 && detectionIndex >= 0)
|
||||||
{
|
{
|
||||||
if (iouMatrix[match.first][match.second] >= _iou_threshold) // iou_thrshold
|
if (iouMatrix[match.first][match.second] <= 1-_iou_threshold) // iou_thrshold
|
||||||
{
|
{
|
||||||
sv::Box box;
|
sv::Box box;
|
||||||
tgts.targets[detectionIndex].getBox(box);
|
tgts.targets[detectionIndex].getBox(box);
|
||||||
this->_tracklets[trackletIndex].age = 0;
|
this->_tracklets[trackletIndex].age = 0;
|
||||||
this->_tracklets[trackletIndex].hits++;
|
this->_tracklets[trackletIndex].hits++;
|
||||||
this->_tracklets[trackletIndex].frame_id = tgts.frame_id;
|
this->_tracklets[trackletIndex].frame_id = tgts.frame_id;
|
||||||
this->_tracklets[trackletIndex].bbox << box.x1, box.y1, box.x2-box.x1, box.y2-box.y1;
|
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;
|
||||||
|
|
||||||
auto[mean, covariance] = kf.update(this->_tracklets[trackletIndex].mean, this->_tracklets[trackletIndex].covariance, box);
|
|
||||||
this->_tracklets[trackletIndex].mean = mean;
|
|
||||||
this->_tracklets[trackletIndex].covariance = covariance;
|
|
||||||
|
|
||||||
tgts.targets[detectionIndex].tracked_id = this->_tracklets[trackletIndex].id;
|
tgts.targets[detectionIndex].tracked_id = this->_tracklets[trackletIndex].id;
|
||||||
match_det[detectionIndex] = detectionIndex;
|
match_det[detectionIndex] = trackletIndex;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// create new tracklets for unmatched detections
|
std::vector <vector<double>> ().swap(iouMatrix);
|
||||||
for (int i=0; i<tgts.targets.size(); i++)
|
for (int i=0; i<tgts.targets.size(); i++)
|
||||||
{
|
{
|
||||||
|
//cout<<"match_det: index: "<<i<<" value: "<<match_det[i]<<endl;
|
||||||
if (match_det[i] == -1)
|
if (match_det[i] == -1)
|
||||||
{
|
{
|
||||||
|
cout<<"create new tracklet."<<endl;
|
||||||
sv::Box box;
|
sv::Box box;
|
||||||
tgts.targets[i].getBox(box);
|
tgts.targets[i].getBox(box);
|
||||||
Tracklet tracklet;
|
Tracklet tracklet;
|
||||||
tracklet.id = ++ this->_next_tracklet_id;
|
tracklet.id = ++ this->_next_tracklet_id;
|
||||||
tracklet.bbox << box.x1, box.y1, box.x2-box.x1, box.y2-box.y1;
|
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;
|
||||||
|
|
||||||
tracklet.age = 0;
|
tracklet.age = 0;
|
||||||
tracklet.hits = 1;
|
tracklet.hits = 1;
|
||||||
tracklet.misses = 0;
|
tracklet.misses = 0;
|
||||||
tracklet.frame_id = tgts.frame_id;
|
tracklet.frame_id = tgts.frame_id;
|
||||||
|
tracklet.category_id = tgts.targets[i].category_id;
|
||||||
tracklet.tentative = true;
|
tracklet.tentative = true;
|
||||||
|
|
||||||
auto[new_mean, new_covariance] = kf.initiate(tracklet.bbox);
|
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > new_motion = kf.initiate(tracklet.bbox);
|
||||||
tracklet.mean = new_mean;
|
tracklet.mean = new_motion.first;
|
||||||
tracklet.covariance = new_covariance;
|
tracklet.covariance = new_motion.second;
|
||||||
|
|
||||||
tgts.targets[i].tracked_id = this->_next_tracklet_id;
|
tgts.targets[i].tracked_id = this->_next_tracklet_id;
|
||||||
tgts.targets[i].has_tid = true;
|
tgts.targets[i].has_tid = true;
|
||||||
this->_tracklets.push_back(tracklet);
|
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)
|
for (auto& tracklet : this->_tracklets)
|
||||||
{
|
{
|
||||||
if (tracklet.hits >= _min_hits)
|
if (tracklet.hits >= _min_hits)
|
||||||
|
@ -312,15 +337,16 @@ vector<Tracklet> SORT::getTracklets() const
|
||||||
|
|
||||||
double SORT::_iou(Tracklet& tracklet, sv::Box& box)
|
double SORT::_iou(Tracklet& tracklet, sv::Box& box)
|
||||||
{
|
{
|
||||||
double trackletX1 = tracklet.bbox(0);
|
double trackletX1 = tracklet.bbox(0)-tracklet.bbox(2)/2;
|
||||||
double trackletY1 = tracklet.bbox(1);
|
double trackletY1 = tracklet.bbox(1)-tracklet.bbox(3)/2;
|
||||||
double trackletX2 = tracklet.bbox(0) + tracklet.bbox(2);
|
double trackletX2 = tracklet.bbox(0) + tracklet.bbox(2)/2;
|
||||||
double trackletY2 = tracklet.bbox(1) + tracklet.bbox(3);
|
double trackletY2 = tracklet.bbox(1) + tracklet.bbox(3)/2;
|
||||||
|
|
||||||
double detectionX1 = box.x1;
|
double detectionX1 = box.x1;
|
||||||
double detectionY1 = box.y1;
|
double detectionY1 = box.y1;
|
||||||
double detectionX2 = box.x2;
|
double detectionX2 = box.x2;
|
||||||
double detectionY2 = box.y2;
|
double detectionY2 = box.y2;
|
||||||
|
|
||||||
double intersectionX1 = max(trackletX1, detectionX1);
|
double intersectionX1 = max(trackletX1, detectionX1);
|
||||||
double intersectionY1 = max(trackletY1, detectionY1);
|
double intersectionY1 = max(trackletY1, detectionY1);
|
||||||
double intersectionX2 = min(trackletX2, detectionX2);
|
double intersectionX2 = min(trackletX2, detectionX2);
|
||||||
|
@ -339,19 +365,55 @@ double SORT::_iou(Tracklet& tracklet, sv::Box& box)
|
||||||
return iou;
|
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)
|
vector<pair<int, int> > SORT::_hungarian(vector<vector<double> > costMatrix)
|
||||||
{
|
{
|
||||||
int numRows = costMatrix.size();
|
size_t numRows = costMatrix.size();
|
||||||
int numCols = costMatrix[0].size();
|
size_t numCols = costMatrix[0].size();
|
||||||
|
|
||||||
|
//transpose the matrix if necessary
|
||||||
const bool transposed = numCols > numRows;
|
const bool transposed = numCols > numRows;
|
||||||
// transpose the matrix if necessary
|
if (transposed) {
|
||||||
if (transposed)
|
vector<vector<double>> transposedMatrix(numCols, vector<double>(numRows));
|
||||||
{
|
for (int i = 0; i < numRows; i++)
|
||||||
vector<vector<double> > transposedMatrix(numCols, vector<double>(numRows));
|
|
||||||
for (int i=0; i<numRows; i++)
|
|
||||||
{
|
{
|
||||||
for (int j=0; j<numCols; j++)
|
for (int j = 0; j < numCols; j++)
|
||||||
{
|
{
|
||||||
transposedMatrix[j][i] = costMatrix[i][j];
|
transposedMatrix[j][i] = costMatrix[i][j];
|
||||||
}
|
}
|
||||||
|
@ -359,76 +421,58 @@ vector<pair<int, int> > SORT::_hungarian(vector<vector<double> > costMatrix)
|
||||||
costMatrix = transposedMatrix;
|
costMatrix = transposedMatrix;
|
||||||
swap(numRows, numCols);
|
swap(numRows, numCols);
|
||||||
}
|
}
|
||||||
vector<double>rowMin(numRows, numeric_limits<double>::infinity());
|
// Determine the larger dimension for matching
|
||||||
vector<double>colMin(numCols, numeric_limits<double>::infinity());
|
size_t maxDim = std::max(numRows, numCols);
|
||||||
vector<int>rowMatch(numRows, -1);
|
|
||||||
vector<int>colMatch(numCols, -1);
|
|
||||||
vector<pair<int, int> > matches;
|
|
||||||
// step1: Subtract the row minimums from each row
|
|
||||||
for (int i=0; i<numRows; i++)
|
|
||||||
{
|
|
||||||
for (int j=0; j<numCols; j++)
|
|
||||||
{
|
|
||||||
rowMin[i] = min(rowMin[i], costMatrix[i][j]);
|
|
||||||
}
|
|
||||||
for (int j=0; j<numCols; j++)
|
|
||||||
{
|
|
||||||
costMatrix[i][j] -= rowMin[i];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// step2: substract the colcum minimums from each column
|
|
||||||
for (int j=0; j<numCols; j++)
|
|
||||||
{
|
|
||||||
for (int i=0; i<numRows; i++)
|
|
||||||
{
|
|
||||||
colMin[j] = min(colMin[j], costMatrix[i][j]);
|
|
||||||
}
|
|
||||||
for (int i=0; i<numRows; i++)
|
|
||||||
{
|
|
||||||
costMatrix[i][j] -= colMin[j];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// step3: find a maximal matching
|
|
||||||
for (int i=0; i<numRows; i++)
|
|
||||||
{
|
|
||||||
vector<bool> visited(numCols, false);
|
|
||||||
this->_augment(costMatrix, i, rowMatch, colMatch, visited);
|
|
||||||
}
|
|
||||||
// step4: calculate the matches
|
|
||||||
matches.clear();
|
|
||||||
for (int j=0; j<numCols; j++)
|
|
||||||
{
|
|
||||||
matches.push_back(make_pair(colMatch[j], j));
|
|
||||||
}
|
|
||||||
if (transposed)
|
|
||||||
{
|
|
||||||
for (auto& match : matches)
|
|
||||||
{
|
|
||||||
swap(match.first, match.second);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return matches;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool SORT::_augment(const vector<vector<double> >& costMatrix, int row, vector<int>& rowMatch, vector<int>& colMatch, vector<bool>& visited)
|
// Create a square cost matrix by padding with zeros if necessary
|
||||||
{
|
std::vector<std::vector<double>> squareMatrix(maxDim, std::vector<double>(maxDim, 0.0));
|
||||||
int numCols = costMatrix[0].size();
|
for (size_t row = 0; row < numRows; ++row) {
|
||||||
for (int j=0; j<numCols; j++)
|
for (size_t col = 0; col < numCols; ++col) {
|
||||||
{
|
squareMatrix[row][col] = costMatrix[row][col];
|
||||||
if (costMatrix[row][j] == 0 && !visited[j])
|
}
|
||||||
{
|
}
|
||||||
visited[j] = true;
|
|
||||||
if (colMatch[j] == -1 || this->_augment(costMatrix, colMatch[j], rowMatch, colMatch, visited))
|
// Subtract the minimum value from each row and column
|
||||||
{
|
_subtractMinFromRows(squareMatrix);
|
||||||
rowMatch[row] = j;
|
_subtractMinFromCols(squareMatrix);
|
||||||
colMatch[j] = row;
|
|
||||||
return true;
|
// 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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return false;
|
|
||||||
}
|
// 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) {
|
||||||
|
// assignmentPairs.emplace_back(row, col);
|
||||||
|
// }
|
||||||
|
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;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -56,6 +56,7 @@ public:
|
||||||
int hits;
|
int hits;
|
||||||
int misses;
|
int misses;
|
||||||
int frame_id = 0;
|
int frame_id = 0;
|
||||||
|
int category_id;
|
||||||
bool tentative;
|
bool tentative;
|
||||||
std::vector<double> features;
|
std::vector<double> features;
|
||||||
Eigen::Matrix<double, 8, 1> mean;
|
Eigen::Matrix<double, 8, 1> mean;
|
||||||
|
@ -70,6 +71,7 @@ public:
|
||||||
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> > 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> > 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, 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:
|
private:
|
||||||
Eigen::Matrix<double, 8, 8> _F;
|
Eigen::Matrix<double, 8, 8> _F;
|
||||||
Eigen::Matrix<double, 4, 8> _H;
|
Eigen::Matrix<double, 4, 8> _H;
|
||||||
|
@ -88,7 +90,10 @@ public:
|
||||||
private:
|
private:
|
||||||
double _iou(Tracklet &tracklet, Box &box);
|
double _iou(Tracklet &tracklet, Box &box);
|
||||||
std::vector<std::pair<int,int>> _hungarian(std::vector<std::vector<double>> costMatrix);
|
std::vector<std::pair<int,int>> _hungarian(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 _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;
|
double _iou_threshold;
|
||||||
int _max_age;
|
int _max_age;
|
||||||
|
|
|
@ -0,0 +1,41 @@
|
||||||
|
#!/bin/bash -e
|
||||||
|
export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH
|
||||||
|
|
||||||
|
root_dir=${HOME}"/SpireCV/models"
|
||||||
|
|
||||||
|
|
||||||
|
coco_model1="COCO-yolov5s.wts"
|
||||||
|
coco_model2="COCO-yolov5s6.wts"
|
||||||
|
coco_model3="COCO-yolov5s-seg.wts"
|
||||||
|
coco_model1_fn=${root_dir}/${coco_model1}
|
||||||
|
coco_model2_fn=${root_dir}/${coco_model2}
|
||||||
|
coco_model3_fn=${root_dir}/${coco_model3}
|
||||||
|
|
||||||
|
drone_model1="Drone-yolov5s-ap935-v230302.wts"
|
||||||
|
drone_model2="Drone-yolov5s6-ap949-v230302.wts"
|
||||||
|
drone_model1_fn=${root_dir}/${drone_model1}
|
||||||
|
drone_model2_fn=${root_dir}/${drone_model2}
|
||||||
|
|
||||||
|
personcar_model1="PersonCar-yolov5s-ap606-v230306.wts"
|
||||||
|
personcar_model2="PersonCar-yolov5s6-ap702-v230306.wts"
|
||||||
|
personcar_model1_fn=${root_dir}/${personcar_model1}
|
||||||
|
personcar_model2_fn=${root_dir}/${personcar_model2}
|
||||||
|
|
||||||
|
landing_model1="LandingMarker-resnet34-v230228.onnx"
|
||||||
|
landing_model1_fn=${root_dir}/${landing_model1}
|
||||||
|
|
||||||
|
SpireCVDet -s ${coco_model1_fn} ${root_dir}/COCO.engine 80 s
|
||||||
|
SpireCVDet -s ${coco_model2_fn} ${root_dir}/COCO_HD.engine 80 s6
|
||||||
|
SpireCVSeg -s ${coco_model3_fn} ${root_dir}/COCO_SEG.engine 80 s
|
||||||
|
|
||||||
|
SpireCVDet -s ${drone_model1_fn} ${root_dir}/Drone.engine 1 s
|
||||||
|
SpireCVDet -s ${drone_model2_fn} ${root_dir}/Drone_HD.engine 1 s6
|
||||||
|
|
||||||
|
SpireCVDet -s ${personcar_model1_fn} ${root_dir}/PersonCar.engine 8 s
|
||||||
|
SpireCVDet -s ${personcar_model2_fn} ${root_dir}/PersonCar_HD.engine 8 s6
|
||||||
|
|
||||||
|
cd /usr/src/tensorrt/bin/
|
||||||
|
./trtexec --explicitBatch --onnx=${landing_model1_fn} --saveEngine=${root_dir}/LandingMarker.engine --fp16
|
||||||
|
|
||||||
|
echo "export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH" >> ~/.bashrc
|
||||||
|
|
|
@ -0,0 +1,107 @@
|
||||||
|
#!/bin/bash -e
|
||||||
|
|
||||||
|
root_dir=${HOME}"/SpireCV/models"
|
||||||
|
root_server="https://download.amovlab.com/model"
|
||||||
|
|
||||||
|
sv_params1=${HOME}"/SpireCV/sv_algorithm_params.json"
|
||||||
|
sv_params2=${HOME}"/SpireCV/sv_algorithm_params_coco_640.json"
|
||||||
|
sv_params3=${HOME}"/SpireCV/sv_algorithm_params_coco_1280.json"
|
||||||
|
camera_params1=${HOME}"/SpireCV/calib_webcam_640x480.yaml"
|
||||||
|
camera_params2=${HOME}"/SpireCV/calib_webcam_1280x720.yaml"
|
||||||
|
|
||||||
|
coco_model1="COCO-yolov5s.wts"
|
||||||
|
coco_model2="COCO-yolov5s6.wts"
|
||||||
|
coco_model3="COCO-yolov5s-seg.wts"
|
||||||
|
coco_model1_fn=${root_dir}/${coco_model1}
|
||||||
|
coco_model2_fn=${root_dir}/${coco_model2}
|
||||||
|
coco_model3_fn=${root_dir}/${coco_model3}
|
||||||
|
|
||||||
|
drone_model1="Drone-yolov5s-ap935-v230302.wts"
|
||||||
|
drone_model2="Drone-yolov5s6-ap949-v230302.wts"
|
||||||
|
drone_model1_fn=${root_dir}/${drone_model1}
|
||||||
|
drone_model2_fn=${root_dir}/${drone_model2}
|
||||||
|
|
||||||
|
personcar_model1="PersonCar-yolov5s-ap606-v230306.wts"
|
||||||
|
personcar_model2="PersonCar-yolov5s6-ap702-v230306.wts"
|
||||||
|
personcar_model1_fn=${root_dir}/${personcar_model1}
|
||||||
|
personcar_model2_fn=${root_dir}/${personcar_model2}
|
||||||
|
|
||||||
|
track_model1="dasiamrpn_model.onnx"
|
||||||
|
track_model2="dasiamrpn_kernel_cls1.onnx"
|
||||||
|
track_model3="dasiamrpn_kernel_r1.onnx"
|
||||||
|
track_model4="nanotrack_backbone_sim.onnx"
|
||||||
|
track_model5="nanotrack_head_sim.onnx"
|
||||||
|
track_model1_fn=${root_dir}/${track_model1}
|
||||||
|
track_model2_fn=${root_dir}/${track_model2}
|
||||||
|
track_model3_fn=${root_dir}/${track_model3}
|
||||||
|
track_model4_fn=${root_dir}/${track_model4}
|
||||||
|
track_model5_fn=${root_dir}/${track_model5}
|
||||||
|
|
||||||
|
landing_model1="LandingMarker-resnet34-v230228.onnx"
|
||||||
|
landing_model1_fn=${root_dir}/${landing_model1}
|
||||||
|
|
||||||
|
|
||||||
|
if [ ! -d ${root_dir} ]; then
|
||||||
|
echo -e "\033[32m[INFO]: ${root_dir} not exist, creating it ... \033[0m"
|
||||||
|
mkdir -p ${root_dir}
|
||||||
|
fi
|
||||||
|
|
||||||
|
if [ ! -f ${sv_params1} ]; then
|
||||||
|
echo -e "\033[32m[INFO]: ${sv_params1} not exist, downloading ... \033[0m"
|
||||||
|
wget -O ${sv_params1} ${root_server}/install/a-params/sv_algorithm_params.json
|
||||||
|
fi
|
||||||
|
if [ ! -f ${sv_params2} ]; then
|
||||||
|
echo -e "\033[32m[INFO]: ${sv_params2} not exist, downloading ... \033[0m"
|
||||||
|
wget -O ${sv_params2} ${root_server}/install/a-params/sv_algorithm_params_coco_640.json
|
||||||
|
fi
|
||||||
|
if [ ! -f ${sv_params3} ]; then
|
||||||
|
echo -e "\033[32m[INFO]: ${sv_params3} not exist, downloading ... \033[0m"
|
||||||
|
wget -O ${sv_params3} ${root_server}/install/a-params/sv_algorithm_params_coco_1280.json
|
||||||
|
fi
|
||||||
|
|
||||||
|
if [ ! -f ${camera_params1} ]; then
|
||||||
|
echo -e "\033[32m[INFO]: ${camera_params1} not exist, downloading ... \033[0m"
|
||||||
|
wget -O ${camera_params1} ${root_server}/install/c-params/calib_webcam_640x480.yaml
|
||||||
|
fi
|
||||||
|
if [ ! -f ${camera_params2} ]; then
|
||||||
|
echo -e "\033[32m[INFO]: ${camera_params2} not exist, downloading ... \033[0m"
|
||||||
|
wget -O ${camera_params2} ${root_server}/install/c-params/calib_webcam_1280x720.yaml
|
||||||
|
fi
|
||||||
|
|
||||||
|
if [ ! -f ${coco_model1_fn} ]; then
|
||||||
|
echo -e "\033[32m[INFO]: ${coco_model1_fn} not exist, downloading ... \033[0m"
|
||||||
|
wget -O ${coco_model1_fn} ${root_server}/install/${coco_model1}
|
||||||
|
wget -O ${coco_model2_fn} ${root_server}/install/${coco_model2}
|
||||||
|
wget -O ${coco_model3_fn} ${root_server}/install/${coco_model3}
|
||||||
|
fi
|
||||||
|
|
||||||
|
if [ ! -f ${drone_model1_fn} ]; then
|
||||||
|
echo -e "\033[32m[INFO]: ${drone_model1_fn} not exist, downloading ... \033[0m"
|
||||||
|
wget -O ${drone_model1_fn} ${root_server}/install/${drone_model1}
|
||||||
|
wget -O ${drone_model2_fn} ${root_server}/install/${drone_model2}
|
||||||
|
fi
|
||||||
|
|
||||||
|
if [ ! -f ${personcar_model1_fn} ]; then
|
||||||
|
echo -e "\033[32m[INFO]: ${personcar_model1_fn} not exist, downloading ... \033[0m"
|
||||||
|
wget -O ${personcar_model1_fn} ${root_server}/install/${personcar_model1}
|
||||||
|
wget -O ${personcar_model2_fn} ${root_server}/install/${personcar_model2}
|
||||||
|
fi
|
||||||
|
|
||||||
|
if [ ! -f ${track_model1_fn} ]; then
|
||||||
|
echo -e "\033[32m[INFO]: ${track_model1_fn} not exist, downloading ... \033[0m"
|
||||||
|
wget -O ${track_model1_fn} ${root_server}/${track_model1}
|
||||||
|
wget -O ${track_model2_fn} ${root_server}/${track_model2}
|
||||||
|
wget -O ${track_model3_fn} ${root_server}/${track_model3}
|
||||||
|
fi
|
||||||
|
|
||||||
|
if [ ! -f ${track_model4_fn} ]; then
|
||||||
|
echo -e "\033[32m[INFO]: ${track_model4_fn} not exist, downloading ... \033[0m"
|
||||||
|
wget -O ${track_model4_fn} ${root_server}/${track_model4}
|
||||||
|
wget -O ${track_model5_fn} ${root_server}/${track_model5}
|
||||||
|
fi
|
||||||
|
|
||||||
|
if [ ! -f ${landing_model1_fn} ]; then
|
||||||
|
echo -e "\033[32m[INFO]: ${landing_model1_fn} not exist, downloading ... \033[0m"
|
||||||
|
wget -O ${landing_model1_fn} ${root_server}/install/${landing_model1}
|
||||||
|
fi
|
||||||
|
|
Loading…
Reference in New Issue