AllinToyou

这个屌丝很懒，什么也没留下！

热门标签

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

作者：AllinToyou | 2024-02-22 22:30:29

踩

文章目录

一、前言
二、c++代码
三、调用示例
四、结果

一、前言

在许多目标检测应用场景中，完完全全依赖目标检测对下游是很难做出有效判断，如漏检。
检测后都会加入跟踪进行一些判断或者说补偿。而在智能驾驶中，还需要目标位置信息，所以还需要测距。
往期博客介绍了许多处理复杂问题的，而大部分时候我们算力有限（内存、耗时），所以很多时候只需要提供一种检测适用的方法。
本篇提供一种检测跟踪测距方法，根据博主提供的 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;                      // 目标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}; 
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241

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;     
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74

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] 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);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92

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	
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47

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);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381

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);
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

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;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61

三、调用示例

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(); // 获取当前帧跟踪输出结果
}

四、结果

在这里插入图片描述

在对一些目标做一些跟踪定位，或者对单个目标，在不需要严格跟踪的场景下，效果还是不错。关键是简单实用。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/AllinToyou/article/detail/131586