企业网站优化是什么,香河县做网站,济宁做网站哪家比较好,帝国cms调用网站名称文章目录 一、前言二、c代码2.1、Tracking2.2、KalmanTracking2.3、Hungarian2.4、TrackingInfo 三、调用示例四、结果 一、前言
在许多目标检测应用场景中#xff0c;完完全全依赖目标检测对下游是很难做出有效判断#xff0c;如漏检。检测后都会加入跟踪进行一些判断或者说… 文章目录 一、前言二、c代码2.1、Tracking2.2、KalmanTracking2.3、Hungarian2.4、TrackingInfo 三、调用示例四、结果 一、前言
在许多目标检测应用场景中完完全全依赖目标检测对下游是很难做出有效判断如漏检。检测后都会加入跟踪进行一些判断或者说补偿。而在智能驾驶中还需要目标位置信息所以还需要测距。往期博客介绍了许多处理复杂问题的而大部分时候我们算力有限内存、耗时所以很多时候只需要提供一种检测适用的方法。本篇提供一种检测跟踪测距方法根据博主提供的 c 代码来进行讲解。 二、c代码
直接上代码共7个文件都在同一目录下。
Hungarian.cpp
Hungarian.h
KalmanTracker.cpp
kalmanTracker.h
Tracking.cpp
Tracking.h
TrackingInfo.h
2.1、Tracking
这部分代码就是整个跟踪代码的框架了我已经对代码尽可能的做了简化。注释也算比较详细。
函数解释SetInputTrackingMessage输入数据TargetTracking目标跟踪计算。当航迹为空时分配管理。预测匹配更新获取结果SaveObjectMessage1、转化目标检测数据。 2、可以适当过滤检测结果如置信度低的目标过滤掉等ManageTrack航迹管理分配id、状态、box等PredictTrack预测。box预测、舍弃脱离范围的目标框MatchUpdateTrack匹配。匈牙利矩阵计算代码在 Hungarian.cpp。分情况讨论检测框个数预测框 预测框个数检测框UpdateTrack如果匹配上利用检测的结果会对预测的结果进行修正。卡尔曼代码在 KalmanTracking.cppPublishTrackMessage控制信息的输出GetWorldPosition距离计算简化计算距离每次都更新。当然也可以添加状态进行预测
Tracking.cpp Tracking.h 这部分代码虽然简短但是基本运算都具备麻雀虽小五脏俱全。代码思路也很清晰可以结合我的注释理解。代码如下:
Tracking.cpp
#include Tracking.h// 初始化
bool Tracking::InitData(std::shared_ptrDisInit disInit)
{ mDisInit disInit; // disInit:相机参数内外参return true;
}// 反初始化
void Tracking::Uninit()
{
}void Tracking::SetInputTrackingMessage(std::shared_ptrDetectInfo objectMessage)
{mObjectMessage objectMessage; // 私有变量mObjectMessage存放 目标检测消息
}// 目标跟踪计算
void Tracking::TargetTracking()
{frameCount; // 每次调用frameCount1, 判断处理了几帧std::vectorTrackingBox 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::vectorPredictBox 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_ptrTrackerMessage Tracking::GetOutputTrackingMessage()
{return mTrackerMessage; // 提供外部获取目标跟踪消息接口
}std::vectorTracking::TrackingBox Tracking::SaveObjectMessage(std::shared_ptrDetectInfo objectMessage)
{std::vectorTrackingBox 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::vectorTrackingBox 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::vectorTracking::PredictBox Tracking::PredictTrack()
{std::vectorPredictBox 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::vectorPredictBox predictBox, std::vectorTrackingBox detectData)
{// trackers:当前所有航迹, predictBox:当前所有预测box、state, detectData:当前帧检测信息unsigned int trkNum predictBox.size(); // 上一帧预测框得个数unsigned int detNum detectData.size(); // 当前检测框得个数std::vectorstd::vectordouble iouMatrix; // 关联矩阵-匈牙利匹配iouMatrix.resize(trkNum, std::vectordouble(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::vectorint assignment; hungAlgo.Solve(iouMatrix, assignment); // 匈牙利匹配计算std::setint unMatchedDetections; // 存放未匹配的检测框std::setint allItems;std::setint 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_iteratorstd::setint(unMatchedDetections, unMatchedDetections.begin()));}std::setint unMatchedTrajectories; // 存放未匹配的跟踪框// 检测框个数 预测框个数if (detNum trkNum) { for (unsigned int i 0; i trkNum; i) {// 匈牙利算法没有匹配到 当前索引对应的值为-1if (assignment[i] -1) { unMatchedTrajectories.insert(i);}}}std::vectorcv::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::vectorPredictBox predictBox, std::vectorTrackingBox detectData, std::vectorcv::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::vectorTrackerResult trackerResults;for (auto it trackers.begin(); it ! trackers.end();) { cv::Rect_float kBox (*it).kBox.GetState();std::vectorfloat 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_sharedTrackerMessage(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::vectorfloat Tracking::GetPosition(float x, float y)
{std::vectorfloat position GetWorldPosition(y, x, mDisInit); // 根据图像像素获取世界位置 x,y相对于车体return position;
}std::vectorfloat Tracking::GetWorldPosition(float pixeY, float pixeX, std::shared_ptrDisInit 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.hclass Tracking
{
public:Tracking(){} // 初始化bool InitData(std::shared_ptrDisInit disInit); // 反初始化void Uninit();// 输入接口 void SetInputTrackingMessage(std::shared_ptrDetectInfo objectMessage);// 目标跟踪计算void TargetTracking();// 输出接口 输出trackingmessage目标跟踪发布的消息std::shared_ptrTrackerMessage 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_floatstd::vectorfloat state; // 目标状态 x,y}StateBox;typedef struct PredictBox{int label; // 目标标签cv::Rect_float box; // 跟踪预测框std::vectorfloat state; // 目标状态 x,y}PredictBox;std::vectorTrackingBox SaveObjectMessage(std::shared_ptrDetectInfo objectMessage); // 目标检测信息void ManageTrack( std::vectorTrackingBox detectData, int index); // 1、管理航迹std::vectorPredictBox PredictTrack(); // 2、预测航迹void MatchUpdateTrack(std::vectorPredictBox predictBox, std::vectorTrackingBox detectData); // 3、匹配 4、更新 void UpdateTrack(std::vectorPredictBox predictBox, std::vectorTrackingBox detectData, std::vectorcv::Point matchedPairs); // 4、更新void PublishTrackMessage(); // 5、内部获得目标跟踪消息float GetIOU(cv::Rect_float boxA, cv::Rect_float boxB); // 获取两个框的iou:交并比std::vectorfloat GetPosition(float x, float y); // 计算距离std::vectorfloat GetWorldPosition(float pixeY, float pixeX, std::shared_ptrDisInit disInit); // 距离计算公式 private:std::shared_ptrDisInit mDisInit std::make_sharedDisInit(); // 初始化参数std::shared_ptrDetectInfo mObjectMessage std::make_sharedDetectInfo(); // 需要输入目标检测信息std::shared_ptrTrackerMessage mTrackerMessage std::make_sharedTrackerMessage(); // 获得目标跟踪的信息std::vectorStateBox trackers; // 航迹int frameCount 0; // 图像的帧数记录int maxAge 1; // 允许跟踪连续未匹配到的最大帧数float iouThreshold 0.35; // iou匹配最小不能小于1-iouThresholdint imageWidth 1920; // 图片像素宽int imageHeight 1080; // 图片像素高int idCount 0; // id 计数// 畸变校正后对应的像素点std::vectorstd::vectorcv::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.atfloat(0, 0) stateMat.x stateMat.width / 2; // 中心点xkf.statePost.atfloat(1, 0) stateMat.y stateMat.height / 2; // 中心点ykf.statePost.atfloat(2, 0) stateMat.width / stateMat.height; // 框的宽高比kf.statePost.atfloat(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.atfloat(0, 0), p.atfloat(1, 0), p.atfloat(2, 0), p.atfloat(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.atfloat(0, 0) stateMat.x stateMat.width / 2;measurement.atfloat(1, 0) stateMat.y stateMat.height / 2;measurement.atfloat(2, 0) stateMat.width / stateMat.height;measurement.atfloat(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.atfloat(0, 0), s.atfloat(1, 0), s.atfloat(2, 0), s.atfloat(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::vectorStateType mHistory; // 存放历史的box
};2.3、Hungarian
这部分是匈牙利算法简单来说就是根据权重选取全局最优的匹配结果。这部分原理不难理解,可以参考博主往期博客 匈牙利算法
代码写起来其实还是稍微有点难度这里直接借用开源已有代码。
Hungarian.cpp
#ifndef DBL_EPSILON
#define DBL_EPSILON 2.2204460492503131e-016
#endif#ifndef DBL_MAX
#define DBL_MAX 1.7976931348623158e308
#endif#include Hungarian.hHungarianAlgorithm::HungarianAlgorithm(){}
HungarianAlgorithm::~HungarianAlgorithm(){}//********************************************************//
// A single function wrapper for solving assignment problem.
//********************************************************//
double HungarianAlgorithm::Solve(std::vectorstd::vectordouble DistMatrix, std::vectorint 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; rownOfRows; 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; rownOfRows; 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; starRownOfRows; 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.hclass HungarianAlgorithm
{
public:HungarianAlgorithm();~HungarianAlgorithm();double Solve(std::vectorstd::vectordouble DistMatrix, std::vectorint 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::vectorDetBox boxes;
}DetectInfo;/** 目标跟踪初始化*/
typedef struct DisInit
{float h; // 相机离地面距离float pitch; // 俯仰角std::vectordouble mtx; // 内参矩阵std::vectordouble dist; // 畸变系数std::vectordouble r; // 相机外参相对于车体 旋转矩阵std::vectordouble 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::vectorfloat position; // 目标的位置 x,ystd::vectorfloat box; // x1,y1,x2,y2
}TrackerResult;typedef struct TrackerMessage
{std::vectorTrackerResult 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(); // 获取当前帧跟踪输出结果 }
四、结果 在对一些目标做一些跟踪定位或者对单个目标在不需要严格跟踪的场景下效果还是不错。关键是简单实用。
