【目标跟踪】提供一种简单跟踪测距方法（c++）

文章目录

一、前言
二、c++代码
- 2.1、Tracking
- 2.2、KalmanTracking
- 2.3、Hungarian
- 2.4、TrackingInfo
三、调用示例
四、结果

一、前言

在许多目标检测应用场景中，完完全全依赖目标检测对下游是很难做出有效判断，如漏检。
检测后都会加入跟踪进行一些判断或者说补偿。而在智能驾驶中，还需要目标位置信息，所以还需要测距。
往期博客介绍了许多处理复杂问题的，而大部分时候我们算力有限（内存、耗时），所以很多时候只需要提供一种检测适用的方法。
本篇提供一种检测跟踪测距方法，根据博主提供的 c++ 代码来进行讲解。

二、c++代码

直接上代码，共7个文件，都在同一目录下。

Hungarian.cpp

Hungarian.h

KalmanTracker.cpp

kalmanTracker.h

Tracking.cpp

Tracking.h

TrackingInfo.h

2.1、Tracking

这部分代码就是整个跟踪代码的框架了，我已经对代码尽可能的做了简化。注释也算比较详细。

函数	解释
SetInputTrackingMessage	输入数据
TargetTracking	目标跟踪计算。当航迹为空时，分配管理。预测，匹配，更新，获取结果
SaveObjectMessage	1、转化目标检测数据。 2、可以适当过滤检测结果，如：置信度低的目标过滤掉等
ManageTrack	航迹管理，分配id、状态、box等
PredictTrack	预测。box预测、舍弃脱离范围的目标框
MatchUpdateTrack	匹配。匈牙利矩阵计算代码在 Hungarian.cpp。分情况讨论，检测框个数>预测框预测框个数>检测框
UpdateTrack	如果匹配上，利用检测的结果，会对预测的结果进行修正。卡尔曼代码在 KalmanTracking.cpp
PublishTrackMessage	控制信息的输出
GetWorldPosition	距离计算，简化计算，距离每次都更新。当然也可以添加状态进行预测

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;                      // 目标idstateBox.kBox = KalmanTracker(detectData[index].box);   // KalmanTracker所需的boxidCount++;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存放符合条件的boxit++;}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) {// 匈牙利算法没有匹配到 当前索引对应的值为-1if (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);            // 未匹配预测idunMatchedDetections.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); // 更新修正boxfloat 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;         // idtrackerResult.position = {rState[0], rState[1], 0}; // 世界坐标相对车位置,xyz z默认为0    单位mtrackerResult.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_zfloat 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;                         // 目标idcv::Rect_<float> box;           // 目标框}TrackingBox;   typedef struct StateBox{int id;                         // 目标idint 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-iouThresholdint imageWidth = 1920;                                             // 图片像素宽int imageHeight = 1080;                                            // 图片像素高int idCount = 0;                                                   // id 计数// 畸变校正后对应的像素点std::vector<std::vector<cv::Point2d>> mPoints;     
};

2.2、KalmanTracking

这部分主要是调用 opencv kalman代码。状态、状态转移方程可以自己设定。

函数	解释
initKf	数据初始化。定义box状态、状态转移方程，中心点，宽高比，高。初始化。初始化方差、测量误差、噪声误差等
predict	状态预测，kf是opencv中的cv::KalmanFilter。
update	修正状态，跟新当前框状态

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] stylekf.statePost.at<float>(0, 0) = stateMat.x + stateMat.width / 2;		// 中心点xkf.statePost.at<float>(1, 0) = stateMat.y + stateMat.height / 2;	// 中心点ykf.statePost.at<float>(2, 0) = stateMat.width / stateMat.height;	// 框的宽高比kf.statePost.at<float>(3, 0) = stateMat.height;						// 框的高度
}// 预测框的位置
StateType KalmanTracker::predict()
{// predictmUpdateOrPredict = 0;	// 预测的时候为0cv::Mat p = kf.predict();	// 预测mAge += 1;	// 历史预测次数+1// 当上次没更新时连续更新的次数清0	if (mTimeSinceUpdate > 0) {	mHitStreak = 0;	}mTimeSinceUpdate += 1;	// 从上一次更新起 连续预测次数+1StateType 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);	// 存放历史的boxreturn mHistory.back();
}// 更新框的位置
void KalmanTracker::update(StateType stateMat)
{mTimeSinceUpdate = 0;	mUpdateOrPredict = 1;	// 更新的时候为1mHistory.clear();	// 清空历史的boxmHits += 1;  // 历史更新次数+1mHitStreak += 1;// 当前测量值的中心点cx,cy,r,hmeasurement.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;// updatekf.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,hfloat 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>类型的boxclass 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为0int 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 functionassignmentoptimal(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_INDEXINGassignment[row] = col + 1; /* MATLAB-Indexing */
#elseassignment[row] = col;
#endifbreak;}
}/********************************************************/
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;                                 // 目标idstd::vector<float> position;            // 目标的位置 x,ystd::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(); // 获取当前帧跟踪输出结果
}