一、前言
- 在许多目标检测应用场景中，完完全全依赖目标检测对下游是很难做出有效判断，如漏检。
- 检测后都会加入跟踪进行一些判断或者说补偿。而在智能驾驶中，还需要目标位置信息，所以还需要测距。
- 往期博客介绍了许多处理复杂问题的，而大部分时候我们算力有限（内存、耗时），所以很多时候只需要提供一种检测适用的方法。
- 本篇提供一种检测跟踪测距方法，根据博主提供的 c++ 代码来进行讲解。
二、c++代码
直接上代码，共7个文件，都在同一目录下。
Hungarian.cpp
Hungarian.h
KalmanTracker.cpp
kalmanTracker.h
Tracking.cpp
Tracking.h
TrackingInfo.h
2.1、Tracking
这部分代码就是整个跟踪代码的框架了，我已经对代码尽可能的做了简化。注释也算比较详细。
Tracking.cpp Tracking.h 这部分代码虽然简短，但是基本运算都具备，麻雀虽小五脏俱全。代码思路也很清晰，可以结合我的注释理解。代码如下:
- Tracking.cpp
#include "Tracking.h"
// 初始化
bool Tracking::InitData(std::shared_ptr<DisInit> disInit)
{
mDisInit = disInit; // disInit:相机参数内外参
return true;
}
// 反初始化
void Tracking::Uninit()
{
}
void Tracking::SetInputTrackingMessage(std::shared_ptr<DetectInfo> objectMessage)
{
mObjectMessage = objectMessage; // 私有变量mObjectMessage存放 目标检测消息
}
// 目标跟踪计算
void Tracking::TargetTracking()
{
frameCount++; // 每次调用frameCount+1, 判断处理了几帧
std::vector<TrackingBox> detData = SaveObjectMessage(mObjectMessage); // 存放目标检测信息
if (trackers.size() == 0) {
if (detData.size() != 0) {
for (unsigned int i = 0; i < detData.size(); i++) {
ManageTrack(detData, i); // 1、管理航迹信息
}
}
return ; // 当trackers.size()为0时直接跳出函数,
}
std::vector<PredictBox> predictBox = PredictTrack(); // PredictTrack 2、预测航迹
MatchUpdateTrack(predictBox, detData); // MatchUpdateTrack 3、匹配 && 4、更新 UpdateTrack
// 管理航迹 a、长时间未更新 b、框已经超出图片
for (auto it = trackers.begin(); it != trackers.end();) {
cv::Rect_<float> box = (*it).kBox.GetState();
if ((*it).kBox.mTimeSinceUpdate > maxAge || (box.x + box.width < 0 || box.y + box.height < 0
|| box.x > imageWidth || box.y > imageHeight || box.height < 0 || box.width < 0)){
it = trackers.erase(it);
}
else {
it++;
}
}
PublishTrackMessage(); // 5、 内部得到跟踪消息、跟踪图片
}
std::shared_ptr<TrackerMessage> Tracking::GetOutputTrackingMessage()
{
return mTrackerMessage; // 提供外部获取目标跟踪消息接口
}
std::vector<Tracking::TrackingBox> Tracking::SaveObjectMessage(std::shared_ptr<DetectInfo> objectMessage)
{
std::vector<TrackingBox> detData; // 存放目标检测信息
for(auto message:objectMessage->boxes) {
TrackingBox tb;
tb.id = 0; // 默认值
tb.box = cv::Rect_<float>(cv::Point_<float>(message.x, message.y), cv::Point_<float>(message.x + message.w, message.y + message.h)); // 检测框
tb.label = message.type; // 保存检测类别
tb.score = message.score; // 保存置信度
detData.push_back(tb); // detData存放目标检测信息
}
return detData; // 用TrackingBox结构体存放目标检测消息 方便后续计算
}
// 1、管理航迹信息
void Tracking::ManageTrack(std::vector<TrackingBox> detectData, int index)
{
// trackers:跟踪航迹, detectData:目标检测消息, index:索引
StateBox stateBox;
stateBox.label = detectData[index].label; // 目标标签
stateBox.score = detectData[index].score; // 目标置信度
stateBox.id = idCount; // 目标id
stateBox.kBox = KalmanTracker(detectData[index].box); // KalmanTracker所需的box
idCount++;
float pixeX = detectData[index].box.x + detectData[index].box.width / 2, pixeY = detectData[index].box.y + detectData[index].box.height;
stateBox.state = GetPosition(pixeX, pixeY); // x,y相对于车体
trackers.push_back(stateBox);
}
// 2、预测航迹
std::vector<Tracking::PredictBox> Tracking::PredictTrack()
{
std::vector<PredictBox> predictBox;
for (auto it = trackers.begin(); it != trackers.end();) {
PredictBox pBox;
pBox.label = (*it).label; // 类别
pBox.box = (*it).kBox.predict(); // box预测;
pBox.state = (*it).state;
if (pBox.box.x + pBox.box.width >= 0 && pBox.box.y + pBox.box.height >= 0 && pBox.box.x <= imageWidth && pBox.box.y <= imageHeight) {
predictBox.push_back(pBox); // predictBox存放符合条件的box
it++;
}
else {
it = trackers.erase(it); // 舍弃不符合条件航迹
}
}
return predictBox; // 返回所有预测后的box、state
}
// 3、匹配
void Tracking::MatchUpdateTrack(std::vector<PredictBox> predictBox, std::vector<TrackingBox> detectData)
{
// trackers:当前所有航迹, predictBox:当前所有预测box、state, detectData:当前帧检测信息
unsigned int trkNum = predictBox.size(); // 上一帧预测框得个数
unsigned int detNum = detectData.size(); // 当前检测框得个数
std::vector<std::vector<double>> iouMatrix; // 关联矩阵->匈牙利匹配
iouMatrix.resize(trkNum, std::vector<double>(detNum, 1)); // resize关联矩阵大小
if (trkNum != 0 && detNum != 0) {
for (unsigned int i = 0; i < trkNum; i++) {
cv::Rect_<float> box = predictBox[i].box;
for (unsigned int j = 0; j < detNum; j++) {
float iouBox = GetIOU(box, detectData[j].box);
iouMatrix[i][j] = 1 - iouBox; // 使用1 - weight * iou匈牙利算法匹配最小的权重.
}
}
HungarianAlgorithm hungAlgo;
std::vector<int> assignment;
hungAlgo.Solve(iouMatrix, assignment); // 匈牙利匹配计算
std::set<int> unMatchedDetections; // 存放未匹配的检测框
std::set<int> allItems;
std::set<int> matchedItems;
// 检测框个数>预测框个数 detNum:当前帧框个数,trknum:预测框个数
if (detNum > trkNum) {
for (unsigned int n = 0; n < detNum; n++) {
allItems.insert(n);
}
for (unsigned int i = 0; i < trkNum; ++i) {
matchedItems.insert(assignment[i]);
}
std::set_difference(allItems.begin(), allItems.end(), matchedItems.begin(), matchedItems.end(),
std::insert_iterator<std::set<int>>(unMatchedDetections, unMatchedDetections.begin()));
}
std::set<int> unMatchedTrajectories; // 存放未匹配的跟踪框
// 检测框个数 < 预测框个数
if (detNum < trkNum) {
for (unsigned int i = 0; i < trkNum; ++i) {
// 匈牙利算法没有匹配到 当前索引对应的值为-1
if (assignment[i] == -1) {
unMatchedTrajectories.insert(i);
}
}
}
std::vector<cv::Point> matchedPairs; // 存放匹配到的跟踪框与检测框
for (unsigned int i = 0; i < trkNum; ++i) {
if (assignment[i] == -1) {
continue; // assignment[i] == -1 过滤掉无效的值
}
if (1 - iouMatrix[i][assignment[i]] < iouThreshold) {
unMatchedTrajectories.insert(i); // 未匹配预测id
unMatchedDetections.insert(assignment[i]); // 未匹配检测id
}
else {
matchedPairs.push_back(cv::Point(i, assignment[i]));
}
}
// 4、更新修正
UpdateTrack(predictBox, detectData, matchedPairs);
// 管理未匹配的检测框航迹
for (auto umd : unMatchedDetections) {
ManageTrack(detectData, umd); // 重新管理航迹信息
}
}
}
// 4、更新修正
void Tracking::UpdateTrack(std::vector<PredictBox> predictBox, std::vector<TrackingBox> detectData, std::vector<cv::Point> matchedPairs)
{
// trackers:当前所有航迹, predictBox:当前所有预测box、state, detectData:当前帧检测信息, matchedPairs:匹配完成后得到的索引
int trkIdx, detIdx; //trkIdx:对应的预测框索引 detIdx:对应的检测框索引
for (unsigned int i = 0; i < matchedPairs.size(); i++) {
trkIdx = matchedPairs[i].x; // 预测索引
detIdx = matchedPairs[i].y; // 检测索引
trackers[trkIdx].kBox.update(detectData[detIdx].box); // 更新修正box
float pixeX = detectData[detIdx].box.x + detectData[detIdx].box.width / 2, pixeY = detectData[detIdx].box.y + detectData[detIdx].box.height;
trackers[trkIdx].state = GetPosition(pixeX, pixeY);
}
}
// 5、内部获得跟踪消息
void Tracking::PublishTrackMessage()
{
std::vector<TrackerResult> trackerResults;
for (auto it = trackers.begin(); it != trackers.end();) {
cv::Rect_<float> kBox = (*it).kBox.GetState();
std::vector<float> rState = (*it).state; // 状态值 x,y
// 此区间的目标才发布
if (rState[0] > 0 && rState[0] < 50 && rState[1] > -20 && rState[1] < 20) {
TrackerResult trackerResult;
trackerResult.label = (*it).label; // 标签
trackerResult.score = (*it).score; // 置信度
trackerResult.id = (*it).id; // id
trackerResult.position = {rState[0], rState[1], 0}; // 世界坐标相对车位置,xyz z默认为0 单位m
trackerResult.box = {kBox.x, kBox.y, kBox.x + kBox.width, kBox.y + kBox.height};
trackerResults.push_back(trackerResult);
}
it++;
}
TrackerMessage trackerMessage;
trackerMessage.trackerResults = trackerResults;
mTrackerMessage = std::make_shared<TrackerMessage>(trackerMessage); // 得到跟踪信息
}
float Tracking::GetIOU(cv::Rect_<float> boxA, cv::Rect_<float> boxB)
{
// boxA:A图像框, boxB:B图像框
float in = (boxA & boxB).area(); // A框与B框交集面积
float un = boxA.area() + boxB.area() - in; // A框与B框并集面积
if (un < DBL_EPSILON) {
return 0;
}
float result = in / un; // 获取iou 交并比
return result;
}
// 计算距离
std::vector<float> Tracking::GetPosition(float x, float y)
{
std::vector<float> position = GetWorldPosition(y, x, mDisInit); // 根据图像像素获取世界位置 x,y相对于车体
return position;
}
std::vector<float> Tracking::GetWorldPosition(float pixeY, float pixeX, std::shared_ptr<DisInit> disInit)
{
// pixeY:像素坐标y, pixeX:像素坐标x, disInit:相机参数内外参
float sigma = atan((pixeY - disInit->mtx[5]) / disInit->mtx[4]); // 计算目标与相机的夹角 纵向
float z = disInit->h * cos(sigma) / sin(sigma + disInit->pitch); // 计算目标到相机的深度
float newX = 2 * disInit->mtx[2] - pixeX;
float newY = 2 * disInit->mtx[5] - pixeY;
float cameraX = z * (newX / disInit->mtx[0] - disInit->mtx[2] / disInit->mtx[0]),
cameraY = z * (newY / disInit->mtx[4] - disInit->mtx[5] / disInit->mtx[4]),
cameraZ = z; // 相机坐标系下的camera_x,camera_y,caemra_z
float x = disInit->r[0] * cameraX + disInit->r[1] * cameraY + disInit->r[2] * cameraZ + disInit->t[0]; // 相对车体x方向距离
float y = disInit->r[3] * cameraX + disInit->r[4] * cameraY + disInit->r[5] * cameraZ + disInit->t[1]; // 相对车体y方向距离
return {x, y};
}
- Tracking.h
#pragma once
#include "Hungarian.h"
#include "KalmanTracker.h"
#include "TrackingInfo.h"
class Tracking
{
public:
Tracking(){}
// 初始化
bool InitData(std::shared_ptr<DisInit> disInit);
// 反初始化
void Uninit();
// 输入接口
void SetInputTrackingMessage(std::shared_ptr<DetectInfo> objectMessage);
// 目标跟踪计算
void TargetTracking();
// 输出接口 输出trackingmessage目标跟踪发布的消息
std::shared_ptr<TrackerMessage> GetOutputTrackingMessage();
private:
typedef struct TrackingBox
{
int label; // 目标标签
float score; // 置信度
int id; // 目标id
cv::Rect_<float> box; // 目标框
}TrackingBox;
typedef struct StateBox
{
int id; // 目标id
int label; // 目标标签
float score; // 置信度
KalmanTracker kBox; // 目标框 类型同cv::Rect_<float>
std::vector<float> state; // 目标状态 x,y
}StateBox;
typedef struct PredictBox
{
int label; // 目标标签
cv::Rect_<float> box; // 跟踪预测框
std::vector<float> state; // 目标状态 x,y
}PredictBox;
std::vector<TrackingBox> SaveObjectMessage(std::shared_ptr<DetectInfo> objectMessage); // 目标检测信息
void ManageTrack( std::vector<TrackingBox> detectData, int index); // 1、管理航迹
std::vector<PredictBox> PredictTrack(); // 2、预测航迹
void MatchUpdateTrack(std::vector<PredictBox> predictBox, std::vector<TrackingBox> detectData); // 3、匹配 && 4、更新
void UpdateTrack(std::vector<PredictBox> predictBox, std::vector<TrackingBox> detectData, std::vector<cv::Point> matchedPairs); // 4、更新
void PublishTrackMessage(); // 5、内部获得目标跟踪消息
float GetIOU(cv::Rect_<float> boxA, cv::Rect_<float> boxB); // 获取两个框的iou:交并比
std::vector<float> GetPosition(float x, float y); // 计算距离
std::vector<float> GetWorldPosition(float pixeY, float pixeX, std::shared_ptr<DisInit> disInit); // 距离计算公式
private:
std::shared_ptr<DisInit> mDisInit = std::make_shared<DisInit>(); // 初始化参数
std::shared_ptr<DetectInfo> mObjectMessage = std::make_shared<DetectInfo>(); // 需要输入目标检测信息
std::shared_ptr<TrackerMessage> mTrackerMessage = std::make_shared<TrackerMessage>(); // 获得目标跟踪的信息
std::vector<StateBox> trackers; // 航迹
int frameCount = 0; // 图像的帧数记录
int maxAge = 1; // 允许跟踪连续未匹配到的最大帧数
float iouThreshold = 0.35; // iou匹配最小不能小于1-iouThreshold
int imageWidth = 1920; // 图片像素宽
int imageHeight = 1080; // 图片像素高
int idCount = 0; // id 计数
// 畸变校正后对应的像素点
std::vector<std::vector<cv::Point2d>> mPoints;
};
2.2、KalmanTracking
这部分主要是调用 opencv kalman代码。状态、状态转移方程可以自己设定。
predict与update要结合理解。
mTimeSinceUpdate上次更新后的预测次数，通过这个参数可以舍弃一些长期未更新的框。
mAge 从出生到现在的年龄(帧数)
mHitStreak 连续更新次数
mHits 历史总更新次数
- KalmanTracker.cpp
#include "KalmanTracker.h"
// initialize Kalman filter
void KalmanTracker::initKf(StateType stateMat)
{
int stateNum = 8; // 状态
int measureNum = 4; // 测量
kf = cv::KalmanFilter(stateNum, measureNum, 0);
measurement = cv::Mat::zeros(measureNum, 1, CV_32F);
// 状态转移方程 中心点x,y,框的宽高比r,框的高h,vx,vy,vr,vh
kf.transitionMatrix = (cv::Mat_<float>(stateNum, stateNum) <<
1, 0, 0, 0, 1, 0, 0, 0,
0, 1, 0, 0, 0, 1, 0, 0,
0, 0, 1, 0, 0, 0, 1, 0,
0, 0, 0, 1, 0, 0, 0, 1,
0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 0, 0, 1);
setIdentity(kf.measurementMatrix);
setIdentity(kf.processNoiseCov, cv::Scalar::all(1e-2));
setIdentity(kf.measurementNoiseCov, cv::Scalar::all(1e-1));
setIdentity(kf.errorCovPost, cv::Scalar::all(1));
// initialize state vector with bounding box in [cx,cy,r,h] style
kf.statePost.at<float>(0, 0) = stateMat.x + stateMat.width / 2; // 中心点x
kf.statePost.at<float>(1, 0) = stateMat.y + stateMat.height / 2; // 中心点y
kf.statePost.at<float>(2, 0) = stateMat.width / stateMat.height; // 框的宽高比
kf.statePost.at<float>(3, 0) = stateMat.height; // 框的高度
}
// 预测框的位置
StateType KalmanTracker::predict()
{
// predict
mUpdateOrPredict = 0; // 预测的时候为0
cv::Mat p = kf.predict(); // 预测
mAge += 1; // 历史预测次数+1
// 当上次没更新时连续更新的次数清0
if (mTimeSinceUpdate > 0) {
mHitStreak = 0;
}
mTimeSinceUpdate += 1; // 从上一次更新起 连续预测次数+1
StateType predictBox = GetRectXYSR(p.at<float>(0, 0), p.at<float>(1, 0), p.at<float>(2, 0), p.at<float>(3, 0));
mHistory.push_back(predictBox); // 存放历史的box
return mHistory.back();
}
// 更新框的位置
void KalmanTracker::update(StateType stateMat)
{
mTimeSinceUpdate = 0;
mUpdateOrPredict = 1; // 更新的时候为1
mHistory.clear(); // 清空历史的box
mHits += 1; // 历史更新次数+1
mHitStreak += 1;
// 当前测量值的中心点cx,cy,r,h
measurement.at<float>(0, 0) = stateMat.x + stateMat.width / 2;
measurement.at<float>(1, 0) = stateMat.y + stateMat.height / 2;
measurement.at<float>(2, 0) = stateMat.width / stateMat.height;
measurement.at<float>(3, 0) = stateMat.height;
// update
kf.correct(measurement);
}
StateType KalmanTracker::GetState(StateType stateMat)
{
return stateMat;
}
// Return the current state vector
StateType KalmanTracker::GetState()
{
cv::Mat s = kf.statePost;
return GetRectXYSR(s.at<float>(0, 0), s.at<float>(1, 0), s.at<float>(2, 0), s.at<float>(3, 0));
}
// Convert bounding box from [cx,cy,r,h] to [x,y,w,h] style.
StateType KalmanTracker::GetRectXYSR(float cx, float cy, float r, float h)
{
// 返回原始类型cv::Rect_<float> x,y,w,h
float w = r * h;
float x = (cx - w / 2);
float y = (cy - h / 2);
if (x < 0 && cx > 0) {
x = 0;
}
if (y < 0 && cy > 0) {
y = 0;
}
return StateType(x, y, w, h);
}
- KalmanTracker.h
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#define StateType cv::Rect_<float> // 接收cv::Rect_<float>类型的box
class KalmanTracker
{
public:
KalmanTracker()
{
initKf(StateType());
mTimeSinceUpdate = 0; // 从上一次更新起总预测次数
mHits = 0; // 历史总更新次数
mHitStreak = 0; // 连续更新的次数
mAge = 0; // 历史总预测次数
}
KalmanTracker(StateType initRect)
{
initKf(initRect);
mTimeSinceUpdate = 0; // 从上一次更新起连续预测次数
mHits = 0; // 历史总更新次数
mHitStreak = 0; // 连续更新的次数
mAge = 0; // 历史总预测次数
}
~KalmanTracker()
{
mHistory.clear();
}
StateType predict();
void update(StateType stateMat);
StateType GetState();
StateType GetState(StateType stateMat);
StateType GetRectXYSR(float cx, float cy, float s, float r);
int mTimeSinceUpdate; // 离最近一次更新 连续预测的次数
int mUpdateOrPredict; // 判断此框状态 update为1 predict为0
int mHits; // 历史总更新次数
int mHitStreak; // 连续更新的次数
int mAge; // 历史总预测次数
cv::KalmanFilter kf;
private:
void initKf(StateType stateMat);
cv::Mat measurement;
std::vector<StateType> mHistory; // 存放历史的box
};
2.3、Hungarian
这部分是匈牙利算法，简单来说就是根据权重选取全局最优的匹配结果。这部分原理不难理解,可以参考博主往期博客 匈牙利算法
代码写起来其实还是稍微有点难度，这里直接借用开源已有代码。
- Hungarian.cpp
#ifndef DBL_EPSILON
#define DBL_EPSILON 2.2204460492503131e-016
#endif
#ifndef DBL_MAX
#define DBL_MAX 1.7976931348623158e+308
#endif
#include "Hungarian.h"
HungarianAlgorithm::HungarianAlgorithm(){}
HungarianAlgorithm::~HungarianAlgorithm(){}
//********************************************************//
// A single function wrapper for solving assignment problem.
//********************************************************//
double HungarianAlgorithm::Solve(std::vector<std::vector<double>>& DistMatrix, std::vector<int>& Assignment)
{
unsigned int nRows = DistMatrix.size();
unsigned int nCols = DistMatrix[0].size();
double *distMatrixIn = new double[nRows * nCols];
int *assignment = new int[nRows];
double cost = 0.0;
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, double *cost, double *distMatrixIn, int nOfRows, int nOfColumns)
{
double *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;
nOfElements = nOfRows * nOfColumns;
distMatrix = (double *)malloc(nOfElements * sizeof(double));
distMatrixEnd = distMatrix + nOfElements;
for (row = 0; row < nOfElements; row++)
{
value = distMatrixIn[row];
if (value < 0)
std::cerr << "All matrix elements have to be non-negative." << std::endl;
distMatrix[row] = value;
}
/* 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, double *cost, double *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, double *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, double *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, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
{
bool zerosFound;/* generate working copy of distance Matrix */
/* check if all matrix elements are positive */
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, double *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, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
{
double 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);
}
- Hungarian.h
#pragma once
#include <iostream>
#include <vector>
#include <stdlib.h>
#include <math.h>
class HungarianAlgorithm
{
public:
HungarianAlgorithm();
~HungarianAlgorithm();
double Solve(std::vector<std::vector<double>>& DistMatrix, std::vector<int>& Assignment);
private:
void assignmentoptimal(int *assignment, double *cost, double *distMatrix, int nOfRows, int nOfColumns);
void buildassignmentvector(int *assignment, bool *starMatrix, int nOfRows, int nOfColumns);
void computeassignmentcost(int *assignment, double *cost, double *distMatrix, int nOfRows);
void step2a(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
void step2b(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
void step3(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
void step4(int *assignment, double *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, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
};
2.4、TrackingInfo
TrackingInfo.h 文件数据格式。
- TrackingInfo.h
#pragma once
#include <string>
#include <vector>
#include <memory>
#include <set>
/*
* 目标检测信息
*/
typedef struct DetBox
{
float x; // xy左上角坐标
float y;
float w; // wh目标长宽（已复原到原图坐标）
float h;
int type; // 当前类别 "pedestrian","car", "bus","truck", "cyclist", "motorcyclist", "tricyclist",
float score; // score = ObjConf * ClsConf
}DetBox;
typedef struct DetectInfo
{
std::vector<DetBox> boxes;
}DetectInfo;
/*
* 目标跟踪初始化
*/
typedef struct DisInit
{
float h; // 相机离地面距离
float pitch; // 俯仰角
std::vector<double> mtx; // 内参矩阵
std::vector<double> dist; // 畸变系数
std::vector<double> r; // 相机外参，相对于车体 旋转矩阵
std::vector<double> t; // 相机外参，相对于车体 平移矩阵
}DisInit;
/*
* 目标跟踪信息
*/
typedef struct TrackerImageInfo
{
std::string sensor; // 关联那个传感器如：“head_camera”
int framecnt; // 图片的帧数
double timestamp; // 图片的时间戳
}TrackerImageInfo;
typedef struct TrackerResult
{
int label; // 目标标签
float score; // 置信度
int id; // 目标id
std::vector<float> position; // 目标的位置 x,y
std::vector<float> box; // x1,y1,x2,y2
}TrackerResult;
typedef struct TrackerMessage
{
std::vector<TrackerResult> trackerResults;
}TrackerMessage;
三、调用示例
- Tracking tracking;
- tracking.InitData(std::make_shared(cameraParam)); // 初始化获取相机内外参
- 计算获取结果
for (int fi = 1; fi < FrameCount; fi++) {
tracking.SetInputTrackingMessage(std::make_shared(detBox)); // 输入当前帧检测信息
tracking.TargetTracking(); // 计算
TrackerMessage messageResult = *tracking.GetOutputTrackingMessage(); // 获取当前帧跟踪输出结果
}
四、结果
在对一些目标做一些跟踪定位，或者对单个目标，在不需要严格跟踪的场景下，效果还是不错。关键是简单实用。