hungarian.h
#ifndef HUNGARIAN_H
#define HUNGARIAN_H
#include
#include
class hungarian
{
public:
hungarian();
~hungarian();
float Solve(std::vector>& DistMatrix, std::vector& Assignment);
private:
void assignmentoptimal(int *assignment, float *cost, float *distMatrix, int nOfRows, int nOfColumns);
void buildassignmentvector(int *assignment, bool *starMatrix, int nOfRows, int nOfColumns);
void computeassignmentcost(int *assignment, float *cost, float *distMatrix, int nOfRows);
void step2a(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
void step2b(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
void step3(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
void step4(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim, int row, int col);
void step5(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
};
#endif // HUNGARIAN_H
hungarian.cpp
#include "hungarian.h"
#include
#include // for DBL_MAX
#include // for fabs()
hungarian::hungarian(){}
hungarian::~hungarian(){}
//********************************************************//
// A single function wrapper for solving assignment problem.
//********************************************************//
float hungarian::Solve(std::vector >& DistMatrix, std::vector& Assignment)
{
unsigned int nRows = DistMatrix.size();
unsigned int nCols = DistMatrix[0].size();
float *distMatrixIn = new float[nRows * nCols];
int *assignment = new int[nRows];
float cost = 0.0;
// Fill in the distMatrixIn. Mind the index is "i + nRows * j".
// Here the cost matrix of size MxN is defined as a float precision array of N*M elements.
// In the solving functions matrices are seen to be saved MATLAB-internally in row-order.
// (i.e. the matrix [1 2; 3 4] will be stored as a vector [1 3 2 4], NOT [1 2 3 4]).
for (unsigned int i = 0; i < nRows; i++)
for (unsigned int j = 0; j < nCols; j++)
distMatrixIn[i + nRows * j] = DistMatrix[i][j];
// call solving function
assignmentoptimal(assignment, &cost, distMatrixIn, nRows, nCols);
Assignment.clear();
for (unsigned int r = 0; r < nRows; r++)
Assignment.push_back(assignment[r]);
delete[] distMatrixIn;
delete[] assignment;
return cost;
}
//********************************************************//
// Solve optimal solution for assignment problem using Munkres algorithm, also known as Hungarian Algorithm.
//********************************************************//
void hungarian::assignmentoptimal(int *assignment, float *cost, float *distMatrixIn, int nOfRows, int nOfColumns)
{
float *distMatrix, *distMatrixTemp, *distMatrixEnd, *columnEnd, value, minValue;
bool *coveredColumns, *coveredRows, *starMatrix, *newStarMatrix, *primeMatrix;
int nOfElements, minDim, row, col;
/* initialization */
*cost = 0;
for (row = 0; row nOfColumns) */
{
minDim = nOfColumns;
for (col = 0; col= 0)
*cost += distMatrix[row + nOfRows*col];
}
}
/********************************************************/
void hungarian::step2a(int *assignment, float *distMatrix, bool *starMatrix,
bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns,
bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
{
bool *starMatrixTemp, *columnEnd;
int col;
/* cover every column containing a starred zero */
for (col = 0; col
kalmanboxtracker.h
#ifndef KALMANBOXTRACKER_H
#define KALMANBOXTRACKER_H
#include
#include
class kalmanboxtracker
{
public:
kalmanboxtracker(std::vector bbox);
void update(std::vector bbox);
std::vector predict();
std::vector get_state();
public:
cv::KalmanFilter* m_kf;
int m_time_since_update;
static int count;
int m_id;
std::vector> m_history;
int m_hits;
int m_hit_streak;
int m_age;
};
#endif // KALMANBOXTRACKER_H
kalmanboxtracker.cpp
#include "kalmanboxtracker.h"
#include "utils.h"
int kalmanboxtracker::count = 0;
kalmanboxtracker::kalmanboxtracker(std::vector bbox)
{
m_kf = new cv::KalmanFilter(7, 4);
m_kf->transitionMatrix = cv::Mat::eye(7, 7, CV_32F);
m_kf->transitionMatrix.at(0, 4) = 1;
m_kf->transitionMatrix.at(1, 5) = 1;
m_kf->transitionMatrix.at(2, 6) = 1;
m_kf->measurementMatrix = cv::Mat::eye(4, 7, CV_32F);
m_kf->measurementNoiseCov = cv::Mat::eye(4, 4, CV_32F);
m_kf->measurementNoiseCov.at(2, 2) *= 10;
m_kf->measurementNoiseCov.at(3, 3) *= 10;
m_kf->errorCovPost = cv::Mat::eye(7, 7, CV_32F);
m_kf->errorCovPost.at(4, 4) *= 1000;
m_kf->errorCovPost.at(5, 5) *= 1000;
m_kf->errorCovPost.at(6, 6) *= 1000;
m_kf->errorCovPost *= 10;
m_kf->processNoiseCov = cv::Mat::eye(7, 7, CV_32F);
m_kf->processNoiseCov.at(4, 4) *= 0.01;
m_kf->processNoiseCov.at(5, 5) *= 0.01;
m_kf->processNoiseCov.at(6, 6) *= 0.001;
m_kf->statePost = cv::Mat::eye(7, 1, CV_32F);
m_kf->statePost.at(0, 0) = convert_bbox_to_z(bbox)[0];
m_kf->statePost.at(1, 0) = convert_bbox_to_z(bbox)[1];
m_kf->statePost.at(2, 0) = convert_bbox_to_z(bbox)[2];
m_kf->statePost.at(3, 0) = convert_bbox_to_z(bbox)[3];
m_time_since_update = 0;
m_id = count;
count++;
m_history = {};
m_hits = 0;
m_hit_streak = 0;
m_age = 0;
//std::cout << m_kf->transitionMatrix << std::endl;
//std::cout << m_kf->measurementMatrix << std::endl;
//std::cout << m_kf->measurementNoiseCov << std::endl;
//std::cout << m_kf->errorCovPost << std::endl;
//std::cout << m_kf->processNoiseCov << std::endl;
//std::cout << m_kf->statePost << std::endl;
}
void kalmanboxtracker::update(std::vector bbox)
{
m_time_since_update = 0;
m_history = {};
m_hits++;
m_hit_streak++;
cv::Mat measurement(4, 1, CV_32F);
for (size_t i = 0; i < 4; i++)
measurement.at(i) = convert_bbox_to_z(bbox)[i];
//std::cout << measurement << std::endl;
m_kf->correct(measurement);
}
std::vector kalmanboxtracker::predict()
{
//std::cout << m_kf->statePost << std::endl;
if (m_kf->statePost.at(2, 0) + m_kf->statePost.at(6, 0) <= 0)
m_kf->statePost.at(6, 0) = 0;
m_kf->predict();
m_age++;
if (m_time_since_update > 0)
m_hit_streak = 0;
m_time_since_update++;
m_history.push_back(convert_x_to_bbox(m_kf->statePost));
return m_history.back();
}
std::vector kalmanboxtracker::get_state()
{
//std::cout << m_kf->statePost << std::endl;
return convert_x_to_bbox(m_kf->statePost);
}
utils.h
#ifndef UTILS_H
#define UTILS_H
#include
#include
#include
/**
* @brief draw_label 画检测框
* @param input_image 输入图像
* @param label 标签名称
* @param left 标签距图像左侧距离
* @param top 标签距图像上侧距离
*/
void draw_label(cv::Mat& input_image, std::string label, int left, int top);
/**
* @brief get_center 计算检测框中心
* @param detections 输入检测框
*/
std::vector get_center(std::vector detections);
/**
* @brief get_center 计算两点间距离
* @param p1 点1
* @param p2 点2
*/
float get_distance(cv::Point p1, cv::Point p2);
/**
* @brief get_center 计算两检测框中心
* @param p1 检测框1
* @param p2 检测框2
*/
float get_center_distance(std::vector bbox1, std::vector bbox2);
/**
* @brief convert_bbox_to_z 将检测框由[x1,y1,x2,y2]的形式转化为[x,y,s,r]的形式
* @param bbox 检测框
*/
std::vector convert_bbox_to_z(std::vector bbox);
/**
* @brief convert_x_to_bbox 将检测框由[x,y,s,r]的形式转化为[x1,y1,x2,y2]的形式
* @param x 检测框
*/
std::vector convert_x_to_bbox(std::vector x);
/**
* @brief iou 计算两检测框的iou
* @param box1 检测框1
* @param box2 检测框2
*/
float iou(std::vector box1, std::vector box2);
/**
* @brief associate_detections_to_tracks 将检测结果和跟踪结果关联
* @param detections 检测结果
* @param trackers 跟踪结果
* @param iou_threshold iou矩阵
*/
std::tuple>, std::vector, std::vector>
associate_detections_to_tracks(std::vector detections,
std::vector> trackers, float iou_threshold = 0.3);
#endif // UTILS_H
utils.cpp
#include "utils.h"
#include "hungarian.h"
void draw_label(cv::Mat& input_image, std::string label, int left, int top)
{
int baseLine;
cv::Size label_size = cv::getTextSize(label, 1, 1, 2, &baseLine);
top = std::max(top, label_size.height);
cv::Point tlc = cv::Point(left, top);
cv::Point brc = cv::Point(left, top + label_size.height + baseLine);
cv::putText(input_image, label, cv::Point(left, top + label_size.height), cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0, 255, 255), 1);
}
std::vector get_center(std::vector detections)
{
std::vector detections_center(detections.size());
for (size_t i = 0; i < detections.size(); i++)
{
detections_center[i] = cv::Point(detections[i].x + detections[i].width / 2, detections[i].y + detections[i].height / 2);
}
return detections_center;
}
float get_distance(cv::Point p1, cv::Point p2)
{
return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2));
}
float get_center_distance(std::vector bbox1, std::vector bbox2)
{
float x1 = bbox1[0], x2 = bbox2[0];
float y1 = bbox1[1], y2 = bbox2[1];
float w1 = bbox1[2] - bbox1[0], w2 = bbox2[2] - bbox2[0];
float h1 = bbox1[3] - bbox1[1], h2 = bbox2[3] - bbox2[1];
cv::Point p1(x1 + w1 / 2, y1 + h1 / 2), p2(x2 + w2 / 2, y2 + h2 / 2);
return get_distance(p1, p2);
}
std::vector convert_bbox_to_z(std::vector bbox)
{
float w = bbox[2] - bbox[0];
float h = bbox[3] - bbox[1];
float x = bbox[0] + w / 2;
float y = bbox[1] + h / 2;
float s = w * h;
float r = w / h;
return { x, y, s, r };
}
std::vector convert_x_to_bbox(std::vector x)
{
float w = sqrt(x[2] * x[3]);
float h = x[2] / w;
return { x[0] - w / 2, x[1] - h / 2, x[0] + w / 2, x[1] + h / 2 };
}
float iou(std::vector box1, std::vector box2)
{
int x1 = std::max(box1[0], box2[0]);
int y1 = std::max(box1[1], box2[1]);
int x2 = std::min(box1[2], box2[2]);
int y2 = std::min(box1[3], box2[3]);
int w = std::max(0, x2 - x1);
int h = std::max(0, y2 - y1);
int inter_area = w * h;
float iou = inter_area * 1.0 / ((box1[2] - box1[0]) * (box1[3] - box1[1]) + (box2[2] - box2[0]) * (box2[3] - box2[1]) - inter_area);
return iou;
}
template
std::vector sort_indices(const std::vector& v)
{
std::vector idx(v.size());
std::iota(idx.begin(), idx.end(), 0);
std::sort(idx.begin(), idx.end(), [&v](size_t i1, size_t i2) { return v[i1] < v[i2]; });
return idx;
}
std::vector> linear_assignment(cv::Mat iou_matrix)
{
std::vector> costMatrix(iou_matrix.cols, std::vector(iou_matrix.rows));
for (size_t i = 0; i < iou_matrix.cols; i++)
for (size_t j = 0; j < iou_matrix.rows; j++)
costMatrix[i][j] = iou_matrix.at(j, i);
hungarian HungAlgo;
std::vector assignment;
HungAlgo.Solve(costMatrix, assignment);
std::vector> tmp(2);
for (size_t i = 0; i < assignment.size(); i++)
{
if (assignment[i] >= 0)
{
tmp[0].push_back(assignment[i]);
tmp[1].push_back(i);
}
}
std::vector indices = sort_indices(tmp[0]);
std::sort(tmp[0].begin(), tmp[0].end());
std::vector> ret(2);
ret[0] = tmp[0];
for (size_t i = 0; i < ret[0].size(); i++)
ret[1].push_back(tmp[1][indices[i]]);
return ret;
}
std::tuple>, std::vector, std::vector>
associate_detections_to_tracks(std::vector detections, std::vector> trackers, float iou_threshold)
{
std::vector unmatched_trackers;
std::vector> matches;
if (trackers.size() == 0)
{
std::vector unmatched_detections(detections.size());
std::iota(unmatched_detections.begin(), unmatched_detections.end(), 0);
//return { {}, unmatched_detections, {} };
return {matches, unmatched_detections, unmatched_trackers};
}
cv::Mat iou_matrix(detections.size(), trackers.size(), CV_32F);
for (size_t i = 0; i < iou_matrix.rows; i++)
{
for (size_t j = 0; j < iou_matrix.cols; j++)
{
std::vector detection{ detections[i].x, detections[i].y, detections[i].x + detections[i].width, detections[i].y + detections[i].height };
std::vector tracker = trackers[j];
iou_matrix.at(i, j) = iou(detection, tracker);
}
}
//std::cout << iou_matrix << std::endl;
std::vector> matched_indices(2);
if (std::min(iou_matrix.rows, iou_matrix.cols) > 0)
{
cv::Mat a(iou_matrix.rows, iou_matrix.cols, CV_32F, cv::Scalar(0));
for (size_t i = 0; i < a.rows; i++)
{
for (size_t j = 0; j < a.cols; j++)
{
if (iou_matrix.at(i, j) > iou_threshold)
a.at(i, j) = 1;
}
}
//std::cout << a << std::endl;
cv::Mat a_sum0(iou_matrix.cols, 1, CV_32F, cv::Scalar(0));
cv::reduce(a, a_sum0, 0, cv::REDUCE_SUM);
std::vector sum0(iou_matrix.cols);
for (size_t i = 0; i < sum0.size(); i++)
sum0[i] = a_sum0.at(0, i);
float a_sum0_max = *std::max_element(sum0.begin(), sum0.end());
cv::Mat a_sum1(1, iou_matrix.rows, CV_32F, cv::Scalar(0));
cv::reduce(a, a_sum1, 1, cv::REDUCE_SUM);
std::vector sum1(iou_matrix.rows);
for (size_t i = 0; i < sum1.size(); i++)
sum1[i] = a_sum1.at(i, 0);
float a_sum1_max = *std::max_element(sum1.begin(), sum1.end());
if (int(a_sum0_max) == 1 && int(a_sum1_max) == 1)
{
std::vector nonZeroCoordinates;
cv::findNonZero(a, nonZeroCoordinates);
std::sort(nonZeroCoordinates.begin(), nonZeroCoordinates.end(), [](cv::Point p1, cv::Point p2) {return p1.y < p2.y; });
for (int i = 0; i < nonZeroCoordinates.size(); i++)
{
matched_indices[0].push_back(nonZeroCoordinates[i].y);
matched_indices[1].push_back(nonZeroCoordinates[i].x);
}
}
else
{
matched_indices = linear_assignment(-iou_matrix);
}
}
std::vector unmatched_detections;
for (size_t i = 0; i < detections.size(); i++)
{
if (std::find(matched_indices[0].begin(), matched_indices[0].end(), i) == matched_indices[0].end())
unmatched_detections.push_back(i);
}
//std::vector unmatched_trackers;
for (size_t i = 0; i < trackers.size(); i++)
{
if (std::find(matched_indices[1].begin(), matched_indices[1].end(), i) == matched_indices[1].end())
unmatched_trackers.push_back(i);
}
//std::vector> matches;
for (size_t i = 0; i < matched_indices[0].size(); i++)
{
//std::cout << matched_indices[0][i] << " " << matched_indices[1][i] << std::endl;
if (iou_matrix.at(matched_indices[0][i], matched_indices[1][i]) < iou_threshold)
{
unmatched_detections.push_back(matched_indices[0][i]);
unmatched_trackers.push_back(matched_indices[1][i]);
}
else
matches.push_back({ matched_indices[0][i], matched_indices[1][i] });
}
return { matches, unmatched_detections, unmatched_trackers };
}
sort.h
#ifndef SORT_H
#define SORT_H
#include
#include
#include "utils.h"
#include "kalmanboxtracker.h"
class sort
{
public:
/**
* @brief Sort 构造函数
* @param max_age 未命中时间上限
* @param min_hits 未命中时间下限
* @param iou_threshold iou阈值
*/
sort(int max_age, int min_hits, float iou_threshold);
/**
* @brief update 更新检测框
* @param dets 检测框
*/
std::vector> update(std::vector dets);
private:
/**
* @param m_max_age 未命中时间上限
*/
int m_max_age;
/**
* @param m_min_hits 未命中时间下限
*/
int m_min_hits;
/**
* @param m_iou_threshold iou阈值
*/
float m_iou_threshold;
/**
* @param m_trackers 卡尔曼跟踪器的集合
*/
std::vector m_trackers;
/**
* @param m_frame_count 帧数
*/
int m_frame_count;
};
#endif // SORT_H
sort.cpp
#include "sort.h"
#include
#include
#include "utils.h"
#include "kalmanboxtracker.h"
#include "sort.h"
/*
max_age:每个track可以存活的帧数
min_hits:最小击中帧数
frame_count:总的帧数,也就是第几帧*/
sort::sort(int max_age = 1, int min_hits = 3, float iou_threshold = 0.3)
{
m_max_age = max_age;
m_min_hits = min_hits;
m_iou_threshold = iou_threshold;
m_trackers = {};
m_frame_count = 0;
}
std::vector> sort::update(std::vector dets)
{
m_frame_count++;
std::vector> trks(m_trackers.size(), std::vector(5, 0));
std::vector to_del;
std::vector> ret;
for (size_t i = 0; i < trks.size(); i++)
{
std::vector pos = m_trackers[i].predict();
std::vector trk{ (int)pos[0], (int)pos[1], (int)pos[2], (int)pos[3], 0 };
trks[i] = trk;
}
for (int i = to_del.size() - 1; i >= 0; i--)
m_trackers.erase(m_trackers.begin() + i);
auto [matched, unmatched_dets, unmatched_trks] = associate_detections_to_tracks(dets, trks, m_iou_threshold);
for (size_t i = 0; i < matched.size(); i++)
{
int id = matched[i].first;
std::vector vec{ dets[id].x, dets[id].y, dets[id].x + dets[id].width, dets[id].y + dets[id].height };
m_trackers[matched[i].second].update(vec);
}
for (auto i : unmatched_dets)
{
std::vector vec{ dets[i].x, dets[i].y, dets[i].x + dets[i].width, dets[i].y + dets[i].height };
kalmanboxtracker trk(vec);
m_trackers.push_back(trk);
}
int n = m_trackers.size();
for (int i = m_trackers.size() - 1; i >= 0; i--)
{
auto trk = m_trackers[i];
//std::cout << KalmanBoxTracker::count << std::endl;
std::vector d = trk.get_state();
if ((trk.m_time_since_update < 1) && (trk.m_hit_streak >= m_min_hits || m_frame_count <= m_min_hits))
{
std::vector tmp = d;
tmp.push_back(trk.m_id + 1);
ret.push_back(tmp);
}
n--;
if (trk.m_time_since_update > m_max_age)
m_trackers.erase(m_trackers.begin() + n);
}
if (ret.size() > 0)
return ret;
return {};
}
sort *mot_tracker;
std::vector colors;
mot_tracker = new sort(3, 3, 0.25);
myyolo->Infer(tempmat.cols, tempmat.rows, tempmat.channels(), tempmat.data, Boxes, ClassIndexs, BboxNum,Scores);
std::vector detections;
for (int i =0; i< newBboxNum[0]; i++){
cv::Rect rect(newBoxes[i * 4], newBoxes[i * 4 + 1], newBoxes[i * 4 + 2], newBoxes[i * 4 + 3]);
//cv::rectangle(tempmat, rect, cv::Scalar(0x27, 0xC1, 0x36), 2);
detections.push_back(rect);
}
std::vector> trackers = mot_tracker->update(detections);
//qDebug()< colors(32);
//srand(time(0));
//int size_row = trackers.size();
//int size_col = trackers[0].size();
for (auto tracker : trackers)
{
cv::rectangle(tempmat, cv::Rect(tracker[0], tracker[1],
tracker[2]- tracker[0], tracker[3]- tracker[1]),
colors[int(tracker[4])%100], 2);
cv::putText(tempmat,std::to_string(int(tracker[4])),
cv::Point(tracker[0], tracker[1]),
cv::FONT_HERSHEY_PLAIN,1.5,
colors[int(tracker[4])%100],
2);
}
}
QImage outimg = Mat2QImage(tempmat);
你可能感兴趣的:(C++,c++,算法,开发语言)