forked from floratest1/SpireCV
update mot
This commit is contained in:
jario-jin
parent
1246099063
commit
3c2c7171fa
|
|
@ -52,6 +52,7 @@ if(USE_FFMPEG)
|
|||
endif()
|
||||
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
add_definitions(-DWITH_OCV470)
|
||||
find_package(OpenCV 4.7 REQUIRED)
|
||||
message(STATUS "OpenCV library status:")
|
||||
|
|
@ -60,6 +61,7 @@ message(STATUS " libraries: ${OpenCV_LIBS}")
|
|||
message(STATUS " include path: ${OpenCV_INCLUDE_DIRS}")
|
||||
|
||||
|
||||
include_directories(${EIGEN3_INCLUDE_DIRS})
|
||||
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include)
|
||||
include_directories(
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/IOs/serial/include
|
||||
|
|
|
|||
|
|
@ -4,6 +4,8 @@
|
|||
#include "gason.h"
|
||||
#include "sv_util.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace Eigen;
|
||||
|
||||
namespace sv {
|
||||
|
||||
|
|
@ -11,9 +13,12 @@ namespace sv {
|
|||
MultipleObjectTracker::MultipleObjectTracker()
|
||||
{
|
||||
this->_params_loaded = false;
|
||||
this->_sort_impl = NULL;
|
||||
}
|
||||
MultipleObjectTracker::~MultipleObjectTracker()
|
||||
{
|
||||
if (this->_sort_impl)
|
||||
delete this->_sort_impl;
|
||||
}
|
||||
|
||||
void MultipleObjectTracker::track(cv::Mat img_, TargetsInFrame& tgts_)
|
||||
|
|
@ -23,16 +28,25 @@ void MultipleObjectTracker::track(cv::Mat img_, TargetsInFrame& tgts_)
|
|||
this->_load();
|
||||
this->_params_loaded = true;
|
||||
}
|
||||
|
||||
if ("sort" == this->_algorithm && this->_sort_impl)
|
||||
{
|
||||
this->_detector->detect(img_, tgts_);
|
||||
this->_sort_impl->update(tgts_);
|
||||
}
|
||||
}
|
||||
|
||||
void MultipleObjectTracker::init()
|
||||
void MultipleObjectTracker::init(CommonObjectDetector* detector_)
|
||||
{
|
||||
if (!this->_params_loaded)
|
||||
{
|
||||
this->_load();
|
||||
this->_params_loaded = true;
|
||||
}
|
||||
if ("sort" == this->_algorithm)
|
||||
{
|
||||
this->_sort_impl = new SORT(this->_iou_thres, this->_max_age, this->_min_hits);
|
||||
}
|
||||
this->_detector = detector_;
|
||||
}
|
||||
|
||||
void MultipleObjectTracker::_load()
|
||||
|
|
@ -94,5 +108,306 @@ void MultipleObjectTracker::_load()
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
this->_H = MatrixXd::Identity(4, 8);
|
||||
this->_std_weight_position = 1. / 20;
|
||||
this->_std_weight_vel = 1. / 160;
|
||||
}
|
||||
|
||||
KalmanFilter::~KalmanFilter()
|
||||
{
|
||||
}
|
||||
|
||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::initiate(Vector4d &bbox)
|
||||
{
|
||||
Matrix<double,8,1> mean;
|
||||
mean << bbox(0), bbox(1), bbox(2)/bbox(3), bbox(3), 0, 0, 0, 0;
|
||||
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)
|
||||
{
|
||||
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);
|
||||
measurement << box.x1, box.y1, (box.x2-box.x1)/(box.y2-box.y1), box.y2-box.y1;
|
||||
Matrix<double, 8, 1> new_mean = mean + Kalman_gain * (measurement - this->_H * mean);
|
||||
Matrix<double, 8, 8> new_covariances = (MatrixXd::Identity(8, 8) - Kalman_gain * this->_H) * covariances;
|
||||
return make_pair(new_mean, new_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), 0.01, 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);
|
||||
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;
|
||||
return make_pair(pre_mean, pre_cov);
|
||||
}
|
||||
|
||||
|
||||
SORT::~SORT()
|
||||
{
|
||||
}
|
||||
|
||||
void SORT::update(TargetsInFrame& tgts)
|
||||
{
|
||||
sv::KalmanFilter kf;
|
||||
if (! this->_tracklets.size())
|
||||
{
|
||||
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;
|
||||
// cout << tracklet.id << endl;
|
||||
tgts.targets[i].tracked_id = this->_next_tracklet_id;
|
||||
|
||||
tracklet.bbox << box.x1,box.y1,box.x2-box.x1,box.y2-box.y1; // x,y,w,h
|
||||
tracklet.age = 0;
|
||||
tracklet.hits = 1;
|
||||
tracklet.misses = 0;
|
||||
// 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;
|
||||
}
|
||||
|
||||
array<int, 100> match_det;
|
||||
match_det.fill(-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
|
||||
// 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));
|
||||
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] >= 0)
|
||||
{
|
||||
sv::Box box;
|
||||
tgts.targets[detectionIndex].getBox(box);
|
||||
this->_tracklets[trackletIndex].age = 0;
|
||||
this->_tracklets[trackletIndex].hits++;
|
||||
this->_tracklets[trackletIndex].bbox << box.x1, box.y1, 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;
|
||||
match_det[detectionIndex] = detectionIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
// create new tracklets for unmatched detections
|
||||
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.y1, box.x2-box.x1, box.y2-box.y1;
|
||||
|
||||
tracklet.age = 0;
|
||||
tracklet.hits = 1;
|
||||
tracklet.misses = 0;
|
||||
|
||||
auto[new_mean, new_covariance] = kf.initiate(tracklet.bbox);
|
||||
tracklet.mean = new_mean;
|
||||
tracklet.covariance = new_covariance;
|
||||
|
||||
tgts.targets[i].tracked_id = this->_next_tracklet_id;
|
||||
this->_tracklets.push_back(tracklet);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
vector<Tracklet> SORT::getTracklets() const
|
||||
{
|
||||
return this->_tracklets;
|
||||
}
|
||||
|
||||
double SORT::_iou(Tracklet& tracklet, sv::Box& box)
|
||||
{
|
||||
double trackletX1 = tracklet.bbox(0);
|
||||
double trackletY1 = tracklet.bbox(1);
|
||||
double trackletX2 = tracklet.bbox(0) + tracklet.bbox(2);
|
||||
double trackletY2 = tracklet.bbox(1) + tracklet.bbox(3);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
vector<pair<int, int> > SORT::_hungarian(vector<vector<double> > costMatrix)
|
||||
{
|
||||
int numRows = costMatrix.size();
|
||||
int numCols = costMatrix[0].size();
|
||||
|
||||
const bool transposed = numCols > numRows;
|
||||
// transpose the matrix if necessary
|
||||
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);
|
||||
}
|
||||
vector<double>rowMin(numRows, numeric_limits<double>::infinity());
|
||||
vector<double>colMin(numCols, numeric_limits<double>::infinity());
|
||||
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);
|
||||
_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)
|
||||
{
|
||||
int numCols = costMatrix[0].size();
|
||||
for (int j=0; j<numCols; j++)
|
||||
{
|
||||
if (costMatrix[row][j] == 0 && !visited[j])
|
||||
{
|
||||
visited[j] = true;
|
||||
if (colMatch[j] == -1 || _augment(costMatrix, colMatch[j], rowMatch, colMatch, visited))
|
||||
{
|
||||
rowMatch[row] = j;
|
||||
colMatch[j] = row;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -2,24 +2,28 @@
|
|||
#define __SV_MOT__
|
||||
|
||||
#include "sv_core.h"
|
||||
#include "sv_common_det.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/aruco.hpp>
|
||||
#include <opencv2/tracking.hpp>
|
||||
#include <string>
|
||||
#include <chrono>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
|
||||
namespace sv {
|
||||
|
||||
|
||||
class SORT;
|
||||
|
||||
|
||||
class MultipleObjectTracker : public CameraAlgorithm
|
||||
{
|
||||
public:
|
||||
MultipleObjectTracker();
|
||||
~MultipleObjectTracker();
|
||||
|
||||
void init();
|
||||
void init(CommonObjectDetector* detector_);
|
||||
void track(cv::Mat img_, TargetsInFrame& tgts_);
|
||||
|
||||
private:
|
||||
|
|
@ -35,6 +39,60 @@ private:
|
|||
double _iou_thres;
|
||||
int _max_age;
|
||||
int _min_hits;
|
||||
|
||||
SORT* _sort_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;
|
||||
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);
|
||||
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);
|
||||
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;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -7,9 +7,13 @@ using namespace std;
|
|||
|
||||
int main(int argc, char *argv[]) {
|
||||
// 实例化
|
||||
sv::CommonObjectDetector cod;
|
||||
// 手动导入相机参数，如果使用Amov的G1等吊舱或相机，则可以忽略该步骤，将自动下载相机参数文件
|
||||
cod.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_640x480.yaml");
|
||||
sv::MultipleObjectTracker mot;
|
||||
// 手动导入相机参数，如果使用Amov的G1等吊舱或相机，则可以忽略该步骤，将自动下载相机参数文件
|
||||
mot.loadCameraParams(sv::get_home() + "/SpireCV/calib_webcam_640x480.yaml");
|
||||
mot.init(&cod);
|
||||
|
||||
// 打开摄像头
|
||||
sv::Camera cap;
|
||||
|
|
|
|||
Loading…
Reference in New Issue