代码复制粘贴直接可用
版本介绍：
yoloV5 6.2
opencv 4.6.0
参考：
参考：taifyang/yolov5-sort: sort+yolov5 implemented with python and C++. (github.com)
参考原理：多目标跟踪入门篇(1)：SORT算法详解_sort多目标跟踪-CSDN博客
大致概述一下sort 就是将卡尔曼滤波预测到的信息与目标检测算法的得到的信息用匈牙利算法进行匹配，再用匹配到的结果更新当前状态，得到新的状态作为追踪的结果。
代码片段：
include 头文件
hungarian.h
///
// Hungarian.h: Header file for Class HungarianAlgorithm.
//
// This is a C++ wrapper with slight modification of a hungarian algorithm implementation by Markus Buehren.
// The original implementation is a few mex-functions for use in MATLAB, found here:
// http://www.mathworks.com/matlabcentral/fileexchange/6543-functions-for-the-rectangular-assignment-problem
//
// Both this code and the orignal code are published under the BSD license.
// by Cong Ma, 2016
//
///
#pragma once
#ifndef HUNGARIAN_H
#define HUNGARIAN_H
#include <iostream>
#include <vector>
class HungarianAlgorithm
{
public:
HungarianAlgorithm();
~HungarianAlgorithm();
float Solve(std::vector<std::vector<float> >& DistMatrix, std::vector<int>& 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);
};
#endifkalmanboxtracker.h
/********************************************************************************
** @auth： taify
** @date： 2023/04/21
** @Ver : V1.0.0
** @desc： kalmanboxtracker头文件
*********************************************************************************/
#pragma once
#include <iostream>
#include <opencv2/opencv.hpp>
/**
* @brief KalmanBoxTracker 卡尔曼跟踪器
*/
class KalmanBoxTracker
{
public:
/**
* @brief KalmanBoxTracker 卡尔曼跟踪器类构造函数
* @param bbox 检测框
*/
KalmanBoxTracker(std::vector<int> bbox);
/**
* @brief update 通过观测的检测框更新系统状态
* @param bbox 检测框
*/
void update(std::vector<int> bbox);
/**
* @brief predict 估计预测的边界框
*/
std::vector<float> predict();
/**
* @brief get_state 返回当前检测框状态
*/
std::vector<float> get_state();
public:
/**
* @param m_kf 卡尔曼滤波器
*/
cv::KalmanFilter* m_kf;
/**
* @param m_time_since_update 从更新开始的时间（帧数）
*/
int m_time_since_update;
/**
* @param count 卡尔曼跟踪器的个数
*/
static int count;
/**
* @param m_id 卡尔曼跟踪器的id
*/
int m_id;
/**
* @param m_history 历史检测框的储存
*/
std::vector<std::vector<float>> m_history;
/**
* @param m_hits
*/
int m_hits;
/**
* @param m_hit_streak 忽略目标初始的若干帧
*/
int m_hit_streak;
/**
* @param m_age predict被调用的次数计数
*/
int m_age;
};sort.h
/********************************************************************************
** @auth： taify
** @date： 2023/04/21
** @Ver : V1.0.0
** @desc： sort源文件
*********************************************************************************/
#pragma once
#include <iostream>
#include <opencv2/opencv.hpp>
#include "utils.h"
#include "kalmanboxtracker.h"
/**
* @brief Sort Sort算法
*/
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<std::vector<float>> update(std::vector<cv::Rect> 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<KalmanBoxTracker> m_trackers;
/**
* @param m_frame_count 帧数
*/
int m_frame_count;
};utils.h
#pragma once
/********************************************************************************
** @auth： taify
** @date： 2023/04/21
** @Ver : V1.0.0
** @desc： utils源文件
*********************************************************************************/
#include <iostream>
#include <numeric>
#include <opencv2/opencv.hpp>
/**
* @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<cv::Point> get_center(std::vector<cv::Rect> 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<float> bbox1, std::vector<float> bbox2);
/**
* @brief convert_bbox_to_z 将检测框由[x1,y1,x2,y2]的形式转化为[x,y,s,r]的形式
* @param bbox 检测框
*/
std::vector<float> convert_bbox_to_z(std::vector<int> bbox);
/**
* @brief convert_x_to_bbox 将检测框由[x,y,s,r]的形式转化为[x1,y1,x2,y2]的形式
* @param x 检测框
*/
std::vector<float> convert_x_to_bbox(std::vector<float> x);
/**
* @brief iou 计算两检测框的iou
* @param box1 检测框1
* @param box2 检测框2
*/
float iou(std::vector<float> box1, std::vector<float> box2);
/**
* @brief associate_detections_to_tracks 将检测结果和跟踪结果关联
* @param detections 检测结果
* @param trackers 跟踪结果
* @param iou_threshold iou矩阵
*/
std::tuple<std::vector<std::pair<int, int>>, std::vector<int>, std::vector<int>>
associate_detections_to_tracks(std::vector<cv::Rect> detections, std::vector<std::vector<int>> trackers, float iou_threshold = 0.3);yolo.h
#pragma once
#include<iostream>
#include<opencv2/opencv.hpp>
#define YOLO_P6 false //是否使用P6模型
struct Output {
int id; //结果类别id
float confidence; //结果置信度
cv::Rect box; //矩形框
};
class Yolo {
public:
Yolo() {
}
~Yolo() {}
bool readModel(cv::dnn::Net& net, std::string& netPath, bool isCuda);
bool Detect(cv::Mat& SrcImg, cv::dnn::Net& net, std::vector<Output>& output);
void drawPred(cv::Mat& img, std::vector<Output> result, std::vector<cv::Scalar> color);
std::vector<std::string> className = { "person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light",
"fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow",
"elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee",
"skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard",
"tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple",
"sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch",
"potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone",
"microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear",
"hair drier", "toothbrush"}; //COCO数据集类别
private:
#if(defined YOLO_P6 && YOLO_P6==true)
const float netAnchors[4][6] = { { 19,27, 44,40, 38,94 },{ 96,68, 86,152, 180,137 },{ 140,301, 303,264, 238,542 },{ 436,615, 739,380, 925,792 } };
const int netWidth = 1280; //ONNX图片输入宽度
const int netHeight = 1280; //ONNX图片输入高度
const int strideSize = 4; //stride size
#else
const float netAnchors[3][6] = { { 10,13, 16,30, 33,23 },{ 30,61, 62,45, 59,119 },{ 116,90, 156,198, 373,326 } };
const int netWidth = 640; //ONNX图片输入宽度
const int netHeight = 640; //ONNX图片输入高度
const int strideSize = 3; //stride size
#endif // YOLO_P6
const float netStride[4] = { 8, 16.0,32,64 };
float boxThreshold = 0.25;
float classThreshold = 0.25;
float nmsThreshold = 0.45;
float nmsScoreThreshold = boxThreshold * classThreshold;
//std::vector<std::string> className = { "element"}; //
};scr 源文件
hungarian.cpp
///
// Hungarian.cpp: Implementation file for Class HungarianAlgorithm.
//
// This is a C++ wrapper with slight modification of a hungarian algorithm implementation by Markus Buehren.
// The original implementation is a few mex-functions for use in MATLAB, found here:
// http://www.mathworks.com/matlabcentral/fileexchange/6543-functions-for-the-rectangular-assignment-problem
//
// Both this code and the orignal code are published under the BSD license.
// by Cong Ma, 2016
//
#include <stdlib.h>
#include <cfloat> // for DBL_MAX
#include <cmath> // for fabs()
#include "Hungarian.h"
HungarianAlgorithm::HungarianAlgorithm() {}
HungarianAlgorithm::~HungarianAlgorithm() {}
//********************************************************//
// A single function wrapper for solving assignment problem.
//********************************************************//
float HungarianAlgorithm::Solve(std::vector<std::vector<float> >& DistMatrix, std::vector<int>& 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 HungarianAlgorithm::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 < nOfRows; row++)
assignment[row] = -1;
/* generate working copy of distance Matrix */
/* check if all matrix elements are positive */
nOfElements = nOfRows * nOfColumns;
distMatrix = (float*)malloc(nOfElements * sizeof(float));
distMatrixEnd = distMatrix + nOfElements;
for (row = 0; row < nOfElements; row++)
distMatrix[row] = distMatrixIn[row];;
/* memory allocation */
coveredColumns = (bool*)calloc(nOfColumns, sizeof(bool));
coveredRows = (bool*)calloc(nOfRows, sizeof(bool));
starMatrix = (bool*)calloc(nOfElements, sizeof(bool));
primeMatrix = (bool*)calloc(nOfElements, sizeof(bool));
newStarMatrix = (bool*)calloc(nOfElements, sizeof(bool)); /* used in step4 */
/* preliminary steps */
if (nOfRows <= nOfColumns)
{
minDim = nOfRows;
for (row = 0; row < nOfRows; row++)
{
/* find the smallest element in the row */
distMatrixTemp = distMatrix + row;
minValue = *distMatrixTemp;
distMatrixTemp += nOfRows;
while (distMatrixTemp < distMatrixEnd)
{
value = *distMatrixTemp;
if (value < minValue)
minValue = value;
distMatrixTemp += nOfRows;
}
/* subtract the smallest element from each element of the row */
distMatrixTemp = distMatrix + row;
while (distMatrixTemp < distMatrixEnd)
{
*distMatrixTemp -= minValue;
distMatrixTemp += nOfRows;
}
}
/* Steps 1 and 2a */
for (row = 0; row < nOfRows; row++)
for (col = 0; col < nOfColumns; col++)
if (fabs(distMatrix[row + nOfRows * col]) < DBL_EPSILON)
if (!coveredColumns[col])
{
starMatrix[row + nOfRows * col] = true;
coveredColumns[col] = true;
break;
}
}
else /* if(nOfRows > nOfColumns) */
{
minDim = nOfColumns;
for (col = 0; col < nOfColumns; col++)
{
/* find the smallest element in the column */
distMatrixTemp = distMatrix + nOfRows * col;
columnEnd = distMatrixTemp + nOfRows;
minValue = *distMatrixTemp++;
while (distMatrixTemp < columnEnd)
{
value = *distMatrixTemp++;
if (value < minValue)
minValue = value;
}
/* subtract the smallest element from each element of the column */
distMatrixTemp = distMatrix + nOfRows * col;
while (distMatrixTemp < columnEnd)
*distMatrixTemp++ -= minValue;
}
/* Steps 1 and 2a */
for (col = 0; col < nOfColumns; col++)
for (row = 0; row < nOfRows; row++)
if (fabs(distMatrix[row + nOfRows * col]) < DBL_EPSILON)
if (!coveredRows[row])
{
starMatrix[row + nOfRows * col] = true;
coveredColumns[col] = true;
coveredRows[row] = true;
break;
}
for (row = 0; row < nOfRows; row++)
coveredRows[row] = false;
}
/* move to step 2b */
step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
/* compute cost and remove invalid assignments */
computeassignmentcost(assignment, cost, distMatrixIn, nOfRows);
/* free allocated memory */
free(distMatrix);
free(coveredColumns);
free(coveredRows);
free(starMatrix);
free(primeMatrix);
free(newStarMatrix);
return;
}
/********************************************************/
void HungarianAlgorithm::buildassignmentvector(int* assignment, bool* starMatrix, int nOfRows, int nOfColumns)
{
int row, col;
for (row = 0; row < nOfRows; row++)
for (col = 0; col < nOfColumns; col++)
if (starMatrix[row + nOfRows * col])
{
#ifdef ONE_INDEXING
assignment[row] = col + 1; /* MATLAB-Indexing */
#else
assignment[row] = col;
#endif
break;
}
}
/********************************************************/
void HungarianAlgorithm::computeassignmentcost(int* assignment, float* cost, float* distMatrix, int nOfRows)
{
int row, col;
for (row = 0; row < nOfRows; row++)
{
col = assignment[row];
if (col >= 0)
*cost += distMatrix[row + nOfRows * col];
}
}
/********************************************************/
void HungarianAlgorithm::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 < nOfColumns; col++)
{
starMatrixTemp = starMatrix + nOfRows * col;
columnEnd = starMatrixTemp + nOfRows;
while (starMatrixTemp < columnEnd) {
if (*starMatrixTemp++)
{
coveredColumns[col] = true;
break;
}
}
}
/* move to step 3 */
step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}
/********************************************************/
void HungarianAlgorithm::step2b(int* assignment, float* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, int nOfRows, int nOfColumns, int minDim)
{
int col, nOfCoveredColumns;
/* count covered columns */
nOfCoveredColumns = 0;
for (col = 0; col < nOfColumns; col++)
if (coveredColumns[col])
nOfCoveredColumns++;
if (nOfCoveredColumns == minDim)
{
/* algorithm finished */
buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns);
}
else
{
/* move to step 3 */
step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}
}
/********************************************************/
void HungarianAlgorithm::step3(int* assignment, float* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, int nOfRows, int nOfColumns, int minDim)
{
bool zerosFound;
int row, col, starCol;
zerosFound = true;
while (zerosFound)
{
zerosFound = false;
for (col = 0; col < nOfColumns; col++)
if (!coveredColumns[col])
for (row = 0; row < nOfRows; row++)
if ((!coveredRows[row]) && (fabs(distMatrix[row + nOfRows * col]) < DBL_EPSILON))
{
/* prime zero */
primeMatrix[row + nOfRows * col] = true;
/* find starred zero in current row */
for (starCol = 0; starCol < nOfColumns; starCol++)
if (starMatrix[row + nOfRows * starCol])
break;
if (starCol == nOfColumns) /* no starred zero found */
{
/* move to step 4 */
step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim, row, col);
return;
}
else
{
coveredRows[row] = true;
coveredColumns[starCol] = false;
zerosFound = true;
break;
}
}
}
/* move to step 5 */
step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}
/********************************************************/
void HungarianAlgorithm::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)
{
int n, starRow, starCol, primeRow, primeCol;
int nOfElements = nOfRows * nOfColumns;
/* generate temporary copy of starMatrix */
for (n = 0; n < nOfElements; n++)
newStarMatrix[n] = starMatrix[n];
/* star current zero */
newStarMatrix[row + nOfRows * col] = true;
/* find starred zero in current column */
starCol = col;
for (starRow = 0; starRow < nOfRows; starRow++)
if (starMatrix[starRow + nOfRows * starCol])
break;
while (starRow < nOfRows)
{
/* unstar the starred zero */
newStarMatrix[starRow + nOfRows * starCol] = false;
/* find primed zero in current row */
primeRow = starRow;
for (primeCol = 0; primeCol < nOfColumns; primeCol++)
if (primeMatrix[primeRow + nOfRows * primeCol])
break;
/* star the primed zero */
newStarMatrix[primeRow + nOfRows * primeCol] = true;
/* find starred zero in current column */
starCol = primeCol;
for (starRow = 0; starRow < nOfRows; starRow++)
if (starMatrix[starRow + nOfRows * starCol])
break;
}
/* use temporary copy as new starMatrix */
/* delete all primes, uncover all rows */
for (n = 0; n < nOfElements; n++)
{
primeMatrix[n] = false;
starMatrix[n] = newStarMatrix[n];
}
for (n = 0; n < nOfRows; n++)
coveredRows[n] = false;
/* move to step 2a */
step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}
/********************************************************/
void HungarianAlgorithm::step5(int* assignment, float* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, int nOfRows, int nOfColumns, int minDim)
{
float h, value;
int row, col;
/* find smallest uncovered element h */
h = DBL_MAX;
for (row = 0; row < nOfRows; row++)
if (!coveredRows[row])
for (col = 0; col < nOfColumns; col++)
if (!coveredColumns[col])
{
value = distMatrix[row + nOfRows * col];
if (value < h)
h = value;
}
/* add h to each covered row */
for (row = 0; row < nOfRows; row++)
if (coveredRows[row])
for (col = 0; col < nOfColumns; col++)
distMatrix[row + nOfRows * col] += h;
/* subtract h from each uncovered column */
for (col = 0; col < nOfColumns; col++)
if (!coveredColumns[col])
for (row = 0; row < nOfRows; row++)
distMatrix[row + nOfRows * col] -= h;
/* move to step 3 */
step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}
kalmanboxtracker.cpp
/********************************************************************************
** @auth： taify
** @date： 2023/04/21
** @Ver : V1.0.0
** @desc： kalmanboxtracker源文件
*********************************************************************************/
#include "utils.h"
#include "kalmanboxtracker.h"
int KalmanBoxTracker::count = 0;
KalmanBoxTracker::KalmanBoxTracker(std::vector<int> bbox)
{
m_kf = new cv::KalmanFilter(7, 4);
m_kf->transitionMatrix = cv::Mat::eye(7, 7, CV_32F);
m_kf->transitionMatrix.at<float>(0, 4) = 1;
m_kf->transitionMatrix.at<float>(1, 5) = 1;
m_kf->transitionMatrix.at<float>(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<float>(2, 2) *= 10;
m_kf->measurementNoiseCov.at<float>(3, 3) *= 10;
m_kf->errorCovPost = cv::Mat::eye(7, 7, CV_32F);
m_kf->errorCovPost.at<float>(4, 4) *= 1000;
m_kf->errorCovPost.at<float>(5, 5) *= 1000;
m_kf->errorCovPost.at<float>(6, 6) *= 1000;
m_kf->errorCovPost *= 10;
m_kf->processNoiseCov = cv::Mat::eye(7, 7, CV_32F);
m_kf->processNoiseCov.at<float>(4, 4) *= 0.01;
m_kf->processNoiseCov.at<float>(5, 5) *= 0.01;
m_kf->processNoiseCov.at<float>(6, 6) *= 0.001;
m_kf->statePost = cv::Mat::eye(7, 1, CV_32F);
m_kf->statePost.at<float>(0, 0) = convert_bbox_to_z(bbox)[0];
m_kf->statePost.at<float>(1, 0) = convert_bbox_to_z(bbox)[1];
m_kf->statePost.at<float>(2, 0) = convert_bbox_to_z(bbox)[2];
m_kf->statePost.at<float>(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<int> 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<float>(i) = convert_bbox_to_z(bbox)[i];
//std::cout << measurement << std::endl;
m_kf->correct(measurement);
}
std::vector<float> KalmanBoxTracker::predict()
{
//std::cout << m_kf->statePost << std::endl;
if (m_kf->statePost.at<float>(2, 0) + m_kf->statePost.at<float>(6, 0) <= 0)
m_kf->statePost.at<float>(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<float> KalmanBoxTracker::get_state()
{
//std::cout << m_kf->statePost << std::endl;
return convert_x_to_bbox(m_kf->statePost);
}main.cpp
#include "yolo.h"
#include "kalmanboxtracker.h"
#include <iostream>
#include <opencv2/opencv.hpp>
#include <vector>
#include "sort.h"
using namespace std;
using namespace cv;
using namespace cv::dnn;
int main() {
// 初始化Yolo模型和跟踪器
Yolo yolo;
Net net;
vector<Scalar> colors; // 用于绘制不同目标的颜色
Sort mot_tracker = Sort(1, 3, 0.3); // 创建Sort跟踪器
// 读取Yolo模型
string model_path = "./yolov5s.onnx";
if (!yolo.readModel(net, model_path, true)) {
cout << "无法加载Yolo模型" << endl;
return -1;
}
// 生成随机颜色
srand(time(0));
for (int i = 0; i < 80; i++) {
int b = rand() % 256;
int g = rand() % 256;
int r = rand() % 256;
colors.push_back(Scalar(b, g, r));
}
VideoCapture cap("./test.mp4");
if (!cap.isOpened()) {
cout << "无法打开视频文件" << endl;
return -1;
}
while (true) {
Mat frame;
cap >> frame;
if (frame.empty()) {
cout << "视频已结束" << endl;
break;
}
// 进行目标检测
vector<Output> result;
if (yolo.Detect(frame, net, result)) {
// 跟踪已检测到的目标
vector<Rect> detection_rects; // 存储检测结果的矩形框
for (int i = 0; i < result.size(); i++) {
int x = result[i].box.x;
int y = result[i].box.y;
int w = result[i].box.width;
int h = result[i].box.height;
Rect rect(x, y, w, h);
detection_rects.push_back(rect);
}
// 更新目标跟踪器
vector<vector<float>> trackers = mot_tracker.update(detection_rects);//x,y,w,h,id
// 绘制跟踪结果
for (int i = 0; i < trackers.size(); i++) {
Rect rect(trackers[i][0], trackers[i][1], trackers[i][2] - trackers[i][0], trackers[i][3] - trackers[i][1]);
//cout << "0:" << trackers[i][0] << endl; //跟踪目标的 x 坐标（左上角的 x 坐标）。
//cout << "1:" << trackers[i][1] << endl; //跟踪目标的 y 坐标（左上角的 y 坐标）。
//cout << "2:" << trackers[i][2] << endl; //跟踪目标的宽度。
//cout << "3:" << trackers[i][3] << endl; //跟踪目标的高度。
//cout << "4:" << trackers[i][4] << endl; //跟踪目标的 ID。
int id = static_cast<int>(trackers[i][4]);
//cout << "id:" << id << endl;
//string label = yolo.className[result[id].id] + ":" + to_string(result[id].confidence);
rectangle(frame, rect, Scalar(0, 255, 0), 2);
cout << trackers[i][5] << endl;
putText(frame, to_string(result[i].id)+"id:" + to_string(int(trackers[i][4])), Point(rect.x, rect.y), FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 255, 0), 2);
}
}
imshow("img", frame);
if (waitKey(10) == 27) {
break; // 退出循环
}
}
cap.release();
destroyAllWindows();
return 0;
}
sort.cpp
/********************************************************************************
** @auth： taify
** @date： 2023/04/21
** @Ver : V1.0.0
** @desc： sort头文件
*********************************************************************************/
//#include "utils.h"
//#include "kalmanboxtracker.h"
#include "sort.h"
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<std::vector<float>> Sort::update(std::vector<cv::Rect> dets)
{
m_frame_count++;
std::vector<std::vector<int>> trks(m_trackers.size(), std::vector<int>(5, 0));
std::vector<int> to_del;
std::vector<std::vector<float>> ret;
for (size_t i = 0; i < trks.size(); i++)
{
std::vector<float> pos = m_trackers[i].predict();
std::vector<int> 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<int> 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<int> 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<float> 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<float> 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 {};
}utils.cpp
/********************************************************************************
** @auth： taify
** @date： 2023/04/21
** @Ver : V1.0.0
** @desc： utils头文件
*********************************************************************************/
#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<cv::Point> get_center(std::vector<cv::Rect> detections)
{
std::vector<cv::Point> 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<float> bbox1, std::vector<float> 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<float> convert_bbox_to_z(std::vector<int> 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<float> convert_x_to_bbox(std::vector<float> 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<int> box1, std::vector<int> 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 <typename T>
std::vector<size_t> sort_indices(const std::vector<T>& v)
{
std::vector<size_t> 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<std::vector<int>> linear_assignment(cv::Mat iou_matrix)
{
std::vector<std::vector<float>> costMatrix(iou_matrix.cols, std::vector<float>(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<float>(j, i);
HungarianAlgorithm HungAlgo;
std::vector<int> assignment;
HungAlgo.Solve(costMatrix, assignment);
std::vector<std::vector<int>> 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<size_t> indices = sort_indices(tmp[0]);
std::sort(tmp[0].begin(), tmp[0].end());
std::vector<std::vector<int>> 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::pair<int, int>>, std::vector<int>, std::vector<int>>
associate_detections_to_tracks(std::vector<cv::Rect> detections, std::vector<std::vector<int>> trackers, float iou_threshold)
{
if (trackers.size() == 0)
{
std::vector<int> unmatched_detections(detections.size());
std::iota(unmatched_detections.begin(), unmatched_detections.end(), 0);
return { {}, unmatched_detections, {} };
}
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<int> detection{ detections[i].x, detections[i].y, detections[i].x + detections[i].width, detections[i].y + detections[i].height };
std::vector<int> tracker = trackers[j];
iou_matrix.at<float>(i, j) = iou(detection, tracker);
}
}
//std::cout << iou_matrix << std::endl;
std::vector<std::vector<int>> 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<float>(i, j) > iou_threshold)
a.at<float>(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<float> sum0(iou_matrix.cols);
for (size_t i = 0; i < sum0.size(); i++)
sum0[i] = a_sum0.at<float>(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<float> sum1(iou_matrix.rows);
for (size_t i = 0; i < sum1.size(); i++)
sum1[i] = a_sum1.at<float>(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<cv::Point> 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<int> 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<int> 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<std::pair<int, int>> 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<float>(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 };
}yolo.cpp
#include"yolo.h"
#include "sort.h"
#include "kalmanboxtracker.h"
using namespace std;
using namespace cv;
using namespace cv::dnn;
bool Yolo::readModel(Net& net, string& netPath, bool isCuda = false) {
try {
net = readNet(netPath);
}
catch (const std::exception&) {
return false;
}
//cuda
if (isCuda) {
net.setPreferableBackend(cv::dnn::DNN_BACKEND_CUDA);
net.setPreferableTarget(cv::dnn::DNN_TARGET_CUDA_FP16);
}
//cpu
else {
net.setPreferableBackend(cv::dnn::DNN_BACKEND_DEFAULT);
net.setPreferableTarget(cv::dnn::DNN_TARGET_CPU);
}
return true;
}
bool Yolo::Detect(Mat& SrcImg, Net& net, vector<Output>& output) {
Mat blob;
int col = SrcImg.cols;
int row = SrcImg.rows;
int maxLen = MAX(col, row);
Mat netInputImg = SrcImg.clone();
if (maxLen > 1.2 * col || maxLen > 1.2 * row) {
Mat resizeImg = Mat::zeros(maxLen, maxLen, CV_8UC3);
SrcImg.copyTo(resizeImg(Rect(0, 0, col, row)));
netInputImg = resizeImg;
}
blobFromImage(netInputImg, blob, 1 / 255.0, cv::Size(netWidth, netHeight), cv::Scalar(0, 0, 0), true, false);
//如果在其他设置没有问题的情况下但是结果偏差很大，可以尝试下用下面两句语句
//blobFromImage(netInputImg, blob, 1 / 255.0, cv::Size(netWidth, netHeight), cv::Scalar(104, 117, 123), true, false);
//blobFromImage(netInputImg, blob, 1 / 255.0, cv::Size(netWidth, netHeight), cv::Scalar(114, 114,114), true, false);
net.setInput(blob);
std::vector<cv::Mat> netOutputImg;
net.forward(netOutputImg, net.getUnconnectedOutLayersNames());
std::vector<int> classIds;//结果id数组
std::vector<float> confidences;//结果每个id对应置信度数组
std::vector<cv::Rect> boxes;//每个id矩形框
float ratio_h = (float)netInputImg.rows / netHeight;
float ratio_w = (float)netInputImg.cols / netWidth;
int net_width = className.size() + 5; //输出的网络宽度是类别数+5
float* pdata = (float*)netOutputImg[0].data;
for (int stride = 0; stride < strideSize; stride++) { //stride
int grid_x = (int)(netWidth / netStride[stride]);
int grid_y = (int)(netHeight / netStride[stride]);
for (int anchor = 0; anchor < 3; anchor++) { //anchors
const float anchor_w = netAnchors[stride][anchor * 2];
const float anchor_h = netAnchors[stride][anchor * 2 + 1];
for (int i = 0; i < grid_y; i++) {
for (int j = 0; j < grid_x; j++) {
float box_score = pdata[4]; ;//获取每一行的box框中含有某个物体的概率
if (box_score >= boxThreshold) {
cv::Mat scores(1, className.size(), CV_32FC1, pdata + 5);
Point classIdPoint;
double max_class_socre;
minMaxLoc(scores, 0, &max_class_socre, 0, &classIdPoint);
max_class_socre = (float)max_class_socre;
if (max_class_socre >= classThreshold) {
//rect [x,y,w,h]
float x = pdata[0]; //x
float y = pdata[1]; //y
float w = pdata[2]; //w
float h = pdata[3]; //h
int left = (x - 0.5 * w) * ratio_w;
int top = (y - 0.5 * h) * ratio_h;
classIds.push_back(classIdPoint.x);
confidences.push_back(max_class_socre * box_score);
boxes.push_back(Rect(left, top, int(w * ratio_w), int(h * ratio_h)));
}
}
pdata += net_width;//下一行
}
}
}
}
//执行非最大抑制以消除具有较低置信度的冗余重叠框（NMS）
vector<int> nms_result;
NMSBoxes(boxes, confidences, nmsScoreThreshold, nmsThreshold, nms_result);
for (int i = 0; i < nms_result.size(); i++) {
int idx = nms_result[i];
Output result;
result.id = classIds[idx];
result.confidence = confidences[idx];
result.box = boxes[idx];
output.push_back(result);
}
if (output.size())
return true;
else
return false;
}
void Yolo::drawPred(Mat& img, vector<Output> result, vector<Scalar> color) {
for (int i = 0; i < result.size(); i++) {
int left, top;
left = result[i].box.x;
top = result[i].box.y;
int color_num = i;
rectangle(img, result[i].box, color[result[i].id], 2, 8);
string label = className[result[i].id] + ":" + to_string(result[i].confidence);
int baseLine;
Size labelSize = getTextSize(label, FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseLine);
top = max(top, labelSize.height);
//rectangle(frame, Point(left, top - int(1.5 * labelSize.height)), Point(left + int(1.5 * labelSize.width), top + baseLine), Scalar(0, 255, 0), FILLED);
putText(img, label, Point(left, top), FONT_HERSHEY_SIMPLEX, 1, color[result[i].id], 2);
}
//imshow("1", img);
//imwrite("out.bmp", img);
//waitKey();
//destroyAllWindows();
}
视频效果展示:
d0096a1982f48a4b7dd91bfee3936df5_raw_哔哩哔哩_bilibili
结语：
昨天某鱼找代做，给我要6k，一气之下自己搞定了，现在把6k的项目分享给大家。知识是无价的，但是我是廉价的！！！！！！！！
=================================================
物体经过区域内计数+1
#include "yolo.h"
#include "kalmanboxtracker.h"
#include <iostream>
#include <opencv2/opencv.hpp>
#include <vector>
#include "sort.h"
#include <unordered_set>
using namespace std;
using namespace cv;
using namespace cv::dnn;
int main() {
// 初始化Yolo模型和跟踪器
Yolo yolo;
Net net;
vector<Scalar> colors; // 用于绘制不同目标的颜色
Sort mot_tracker = Sort(1, 3, 0.1); // 创建Sort跟踪器
// 读取Yolo模型
string model_path = "./yolov5s.onnx";
if (!yolo.readModel(net, model_path, true)) {
cout << "无法加载Yolo模型" << endl;
return -1;
}
// 生成随机颜色
srand(time(0));
for (int i = 0; i < 80; i++) {
int b = rand() % 256;
int g = rand() % 256;
int r = rand() % 256;
colors.push_back(Scalar(b, g, r));
}
VideoCapture cap("./test.mp4");
if (!cap.isOpened()) {
cout << "无法打开视频文件" << endl;
return -1;
}
Rect monitoring_area(700, 500, 200, 200); // 定义监测区域
int counter = 0; // 初始化计数器
std::unordered_set<int> counted_trackers; // 用于跟踪已计数的目标 id
while (true) {
Mat frame;
cap >> frame;
if (frame.empty()) {
cout << "视频已结束" << endl;
break;
}
// 进行目标检测
vector<Output> result;
if (yolo.Detect(frame, net, result)) {
// 跟踪已检测到的目标
vector<Rect> detection_rects; // 存储检测结果的矩形框
for (int i = 0; i < result.size(); i++) {
int x = result[i].box.x;
int y = result[i].box.y;
int w = result[i].box.width;
int h = result[i].box.height;
Rect rect(x, y, w, h);
detection_rects.push_back(rect);
}
// 更新目标跟踪器
vector<vector<float>> trackers = mot_tracker.update(detection_rects); // x, y, w, h, id
// 绘制跟踪结果
for (int i = 0; i < trackers.size(); i++) {
Rect rect(trackers[i][0], trackers[i][1], trackers[i][2] - trackers[i][0], trackers[i][3] - trackers[i][1]);
int id = static_cast<int>(trackers[i][4]);
rectangle(frame, rect, Scalar(0, 255, 0), 2); //目标预测框
rectangle(frame, monitoring_area, Scalar(0, 0, 255), 2);//OIR区域画框
putText(frame, "id: " + to_string(id), Point(rect.x, rect.y), FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 255, 0), 2);
// 判断目标是否穿过监测区域
Point center(rect.x + rect.width / 2, rect.y + rect.height / 2);
if (monitoring_area.contains(center) && !counted_trackers.count(id)) { //判断一个点是否在这个区域内 monitoring_area.contains(center)
counter++;
counted_trackers.insert(id);
}
}
}
// 在画面上显示计数
putText(frame, "Count: " + to_string(counter), Point(10, 30), FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 255), 2);
imshow("img", frame);
if (waitKey(10) == 27) {
break; // 退出循环
}
}
cap.release();
destroyAllWindows();
return 0;
}
