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

原创

读书猿

发布于 2024-02-18 16:19:47

1670

发布于 2024-02-18 16:19:47

文章被收录于专栏：无人驾驶感知无人驾驶感知

一、前言

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

Tracking.h

#pragma once
#include "Hungarian.h"
#include "KalmanTracker.h"
#include "TrackingInfo.h"

class Tracking
{
public:
    Tracking(){}    
    
    // 初始化
    bool InitData(std::shared_ptr<DisInit> disInit);   

    // 反初始化
    void Uninit();

    // 输入接口             
    void SetInputTrackingMessage(std::shared_ptr<DetectInfo> objectMessage);

    // 目标跟踪计算
    void TargetTracking();

    // 输出接口             输出trackingmessage目标跟踪发布的消息
    std::shared_ptr<TrackerMessage> GetOutputTrackingMessage();

private:
    typedef struct TrackingBox
    {
        int label;                      // 目标标签
        float score;                    // 置信度
        int id;                         // 目标id
        cv::Rect_<float> box;           // 目标框
    }TrackingBox;   

    typedef struct StateBox
    {
        int id;                         // 目标id
        int label;                      // 目标标签
        float score;                    // 置信度
	    KalmanTracker kBox;             // 目标框 类型同cv::Rect_<float>
	    std::vector<float> state;       // 目标状态 x,y
    }StateBox;

    typedef struct PredictBox
    {
        int label;                      // 目标标签
	    cv::Rect_<float> box;           // 跟踪预测框
	    std::vector<float> state;       // 目标状态 x,y
    }PredictBox;

    std::vector<TrackingBox> SaveObjectMessage(std::shared_ptr<DetectInfo> objectMessage);                                          // 目标检测信息
    void ManageTrack( std::vector<TrackingBox> detectData, int index);                                                              // 1、管理航迹
    std::vector<PredictBox> PredictTrack();                                                                                         // 2、预测航迹
    void MatchUpdateTrack(std::vector<PredictBox> predictBox, std::vector<TrackingBox> detectData);                                 // 3、匹配 && 4、更新                                                         
    void UpdateTrack(std::vector<PredictBox> predictBox, std::vector<TrackingBox> detectData, std::vector<cv::Point> matchedPairs); // 4、更新
    void PublishTrackMessage();                                                                                                     // 5、内部获得目标跟踪消息
    float GetIOU(cv::Rect_<float> boxA, cv::Rect_<float> boxB);                                         // 获取两个框的iou:交并比
    std::vector<float> GetPosition(float x, float y);                                                   // 计算距离
    std::vector<float> GetWorldPosition(float pixeY, float pixeX, std::shared_ptr<DisInit> disInit);    // 距离计算公式 

private:
    std::shared_ptr<DisInit> mDisInit = std::make_shared<DisInit>();                          // 初始化参数
    std::shared_ptr<DetectInfo> mObjectMessage = std::make_shared<DetectInfo>();              // 需要输入目标检测信息
    std::shared_ptr<TrackerMessage> mTrackerMessage = std::make_shared<TrackerMessage>();     // 获得目标跟踪的信息
    std::vector<StateBox> trackers;                                                           // 航迹
    int frameCount = 0;                                                // 图像的帧数记录
    int maxAge = 1;                                                    // 允许跟踪连续未匹配到的最大帧数
    float iouThreshold = 0.35;                                         // iou匹配最小不能小于1-iouThreshold
    int imageWidth = 1920;                                             // 图片像素宽
    int imageHeight = 1080;                                            // 图片像素高
    int idCount = 0;                                                   // id 计数
    // 畸变校正后对应的像素点
    std::vector<std::vector<cv::Point2d>> mPoints;     
};

2.2、KalmanTracking

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

函数	解释
initKf	数据初始化。定义box状态、状态转移方程，中心点，宽高比，高。初始化。初始化方差、测量误差、噪声误差等
predict	状态预测，kf是opencv中的cv::KalmanFilter。
update	修正状态，跟新当前框状态
GetRectXYSR	转化目标框的状态，由cv::Rect_<float> 左上角坐标转换为中心点坐标

predict与update要结合理解。

mTimeSinceUpdate上次更新后的预测次数，通过这个参数可以舍弃一些长期未更新的框。

mAge 从出生到现在的年龄(帧数)

mHitStreak 连续更新次数

mHits 历史总更新次数

KalmanTracker.cpp

#include "KalmanTracker.h"

// initialize Kalman filter
void KalmanTracker::initKf(StateType stateMat)
{
	int stateNum = 8;	// 状态
	int measureNum = 4;	// 测量
	kf = cv::KalmanFilter(stateNum, measureNum, 0);
	measurement = cv::Mat::zeros(measureNum, 1, CV_32F);
	// 状态转移方程 中心点x,y,框的宽高比r,框的高h,vx,vy,vr,vh 
	kf.transitionMatrix = (cv::Mat_<float>(stateNum, stateNum) <<
		1, 0, 0, 0, 1, 0, 0, 0,
		0, 1, 0, 0, 0, 1, 0, 0,
		0, 0, 1, 0, 0, 0, 1, 0,
		0, 0, 0, 1, 0, 0, 0, 1,
		0, 0, 0, 0, 1, 0, 0, 0,
		0, 0, 0, 0, 0, 1, 0, 0,
		0, 0, 0, 0, 0, 0, 1, 0,
		0, 0, 0, 0, 0, 0, 0, 1);
	setIdentity(kf.measurementMatrix);
	setIdentity(kf.processNoiseCov, cv::Scalar::all(1e-2));
	setIdentity(kf.measurementNoiseCov, cv::Scalar::all(1e-1));
	setIdentity(kf.errorCovPost, cv::Scalar::all(1));
	
	// initialize state vector with bounding box in [cx,cy,r,h] style
	kf.statePost.at<float>(0, 0) = stateMat.x + stateMat.width / 2;		// 中心点x
	kf.statePost.at<float>(1, 0) = stateMat.y + stateMat.height / 2;	// 中心点y
	kf.statePost.at<float>(2, 0) = stateMat.width / stateMat.height;	// 框的宽高比
	kf.statePost.at<float>(3, 0) = stateMat.height;						// 框的高度
}

// 预测框的位置
StateType KalmanTracker::predict()
{
	// predict
	mUpdateOrPredict = 0;	// 预测的时候为0
	cv::Mat p = kf.predict();	// 预测
	mAge += 1;	// 历史预测次数+1
	// 当上次没更新时连续更新的次数清0	
	if (mTimeSinceUpdate > 0) {	
		mHitStreak = 0;	
	}
	mTimeSinceUpdate += 1;	// 从上一次更新起 连续预测次数+1
	StateType predictBox = GetRectXYSR(p.at<float>(0, 0), p.at<float>(1, 0), p.at<float>(2, 0), p.at<float>(3, 0));
	mHistory.push_back(predictBox);	// 存放历史的box
	return mHistory.back();
}

// 更新框的位置
void KalmanTracker::update(StateType stateMat)
{
	mTimeSinceUpdate = 0;	
	mUpdateOrPredict = 1;	// 更新的时候为1
	mHistory.clear();	// 清空历史的box
	mHits += 1;  // 历史更新次数+1
	mHitStreak += 1;
	// 当前测量值的中心点cx,cy,r,h
	measurement.at<float>(0, 0) = stateMat.x + stateMat.width / 2;
	measurement.at<float>(1, 0) = stateMat.y + stateMat.height / 2;
	measurement.at<float>(2, 0) = stateMat.width / stateMat.height;
	measurement.at<float>(3, 0) = stateMat.height;
	// update
	kf.correct(measurement);
}

StateType KalmanTracker::GetState(StateType stateMat)
{
	return stateMat;
}

// Return the current state vector
StateType KalmanTracker::GetState()
{	
	cv::Mat s = kf.statePost;
	return GetRectXYSR(s.at<float>(0, 0), s.at<float>(1, 0), s.at<float>(2, 0), s.at<float>(3, 0));
}

// Convert bounding box from [cx,cy,r,h] to [x,y,w,h] style.
StateType KalmanTracker::GetRectXYSR(float cx, float cy, float r, float h)
{
	// 返回原始类型cv::Rect_<float> x,y,w,h
	float w = r * h;
	float x = (cx - w / 2);
	float y = (cy - h / 2);
	if (x < 0 && cx > 0) {
		x = 0;
	}
	if (y < 0 && cy > 0) {
		y = 0;
	}
	return StateType(x, y, w, h);
}

KalmanTracker.h

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"

#define StateType cv::Rect_<float>	// 接收cv::Rect_<float>类型的box


class KalmanTracker
{
public:
	KalmanTracker()
	{
		initKf(StateType());
		mTimeSinceUpdate = 0;						// 从上一次更新起总预测次数 
		mHits = 0;									// 历史总更新次数
		mHitStreak = 0;								// 连续更新的次数
		mAge = 0;									// 历史总预测次数
	}
	KalmanTracker(StateType initRect)
	{
		initKf(initRect);
		mTimeSinceUpdate = 0;						// 从上一次更新起连续预测次数 
		mHits = 0;									// 历史总更新次数
		mHitStreak = 0; 							// 连续更新的次数
		mAge = 0;									// 历史总预测次数
	}
	~KalmanTracker()
	{
		mHistory.clear();
	}
	StateType predict();
	void update(StateType stateMat);
	StateType GetState();
	StateType GetState(StateType stateMat);
	StateType GetRectXYSR(float cx, float cy, float s, float r);

	int mTimeSinceUpdate;											// 离最近一次更新 连续预测的次数
	int mUpdateOrPredict; 											// 判断此框状态 update为1 predict为0
	int mHits;														// 历史总更新次数
	int mHitStreak;													// 连续更新的次数
	int mAge;														// 历史总预测次数
	cv::KalmanFilter kf;

private:
	void initKf(StateType stateMat);
	cv::Mat measurement;
	std::vector<StateType> mHistory;								// 存放历史的box	
};

2.3、Hungarian

这部分是匈牙利算法，简单来说就是根据权重选取全局最优的匹配结果。这部分原理不难理解,可以参考博主往期博客匈牙利算法

代码写起来其实还是稍微有点难度，这里直接借用开源已有代码。

Hungarian.cpp

#ifndef DBL_EPSILON
#define DBL_EPSILON      2.2204460492503131e-016
#endif

#ifndef DBL_MAX
#define DBL_MAX          1.7976931348623158e+308
#endif

#include "Hungarian.h"

HungarianAlgorithm::HungarianAlgorithm(){}
HungarianAlgorithm::~HungarianAlgorithm(){}


//********************************************************//
// A single function wrapper for solving assignment problem.
//********************************************************//
double HungarianAlgorithm::Solve(std::vector<std::vector<double>>& DistMatrix, std::vector<int>& Assignment)
{
	unsigned int nRows = DistMatrix.size();
	unsigned int nCols = DistMatrix[0].size();

	double *distMatrixIn = new double[nRows * nCols];
	int *assignment = new int[nRows];
	double cost = 0.0;

	for (unsigned int i = 0; i < nRows; i++)
		for (unsigned int j = 0; j < nCols; j++)
			distMatrixIn[i + nRows * j] = DistMatrix[i][j];
	
	// call solving function
	assignmentoptimal(assignment, &cost, distMatrixIn, nRows, nCols);

	Assignment.clear();
	for (unsigned int r = 0; r < nRows; r++)
		Assignment.push_back(assignment[r]);

	delete[] distMatrixIn;
	delete[] assignment;
	return cost;
}


//********************************************************//
// Solve optimal solution for assignment problem using Munkres algorithm, also known as Hungarian Algorithm.
//********************************************************//
void HungarianAlgorithm::assignmentoptimal(int *assignment, double *cost, double *distMatrixIn, int nOfRows, int nOfColumns)
{
	double *distMatrix, *distMatrixTemp, *distMatrixEnd, *columnEnd, value, minValue;
	bool *coveredColumns, *coveredRows, *starMatrix, *newStarMatrix, *primeMatrix;
	int nOfElements, minDim, row, col;

	/* initialization */
	*cost = 0;
	for (row = 0; row<nOfRows; row++)
		assignment[row] = -1;

	nOfElements = nOfRows * nOfColumns;
	distMatrix = (double *)malloc(nOfElements * sizeof(double));
	distMatrixEnd = distMatrix + nOfElements;

	for (row = 0; row < nOfElements; row++)
	{
		value = distMatrixIn[row];
		if (value < 0)
			std::cerr << "All matrix elements have to be non-negative." << std::endl;
		distMatrix[row] = value;
	}

	/* memory allocation */
	coveredColumns = (bool *)calloc(nOfColumns, sizeof(bool));
	coveredRows = (bool *)calloc(nOfRows, sizeof(bool));
	starMatrix = (bool *)calloc(nOfElements, sizeof(bool));
	primeMatrix = (bool *)calloc(nOfElements, sizeof(bool));
	newStarMatrix = (bool *)calloc(nOfElements, sizeof(bool)); /* used in step4 */

	/* preliminary steps */
	if (nOfRows <= nOfColumns)
	{
		minDim = nOfRows;

		for (row = 0; row < nOfRows; row++)
		{
			/* find the smallest element in the row */
			distMatrixTemp = distMatrix + row;
			minValue = *distMatrixTemp;
			distMatrixTemp += nOfRows;
			while (distMatrixTemp < distMatrixEnd)
			{
				value = *distMatrixTemp;
				if (value < minValue)
					minValue = value;
				distMatrixTemp += nOfRows;
			}

			/* subtract the smallest element from each element of the row */
			distMatrixTemp = distMatrix + row;
			while (distMatrixTemp < distMatrixEnd)
			{
				*distMatrixTemp -= minValue;
				distMatrixTemp += nOfRows;
			}
		}

		/* Steps 1 and 2a */
		for (row = 0; row < nOfRows; row++)
			for (col = 0; col < nOfColumns; col++)
				if (fabs(distMatrix[row + nOfRows * col]) < DBL_EPSILON)
					if (!coveredColumns[col])
					{
						starMatrix[row + nOfRows * col] = true;
						coveredColumns[col] = true;
						break;
					}
	}
	else /* if(nOfRows > nOfColumns) */
	{
		minDim = nOfColumns;

		for (col = 0; col < nOfColumns; col++)
		{
			/* find the smallest element in the column */
			distMatrixTemp = distMatrix + nOfRows*col;
			columnEnd = distMatrixTemp + nOfRows;

			minValue = *distMatrixTemp++;
			while (distMatrixTemp < columnEnd)
			{
				value = *distMatrixTemp++;
				if (value < minValue)
					minValue = value;
			}

			/* subtract the smallest element from each element of the column */
			distMatrixTemp = distMatrix + nOfRows*col;
			while (distMatrixTemp < columnEnd)
				*distMatrixTemp++ -= minValue;
		}

		/* Steps 1 and 2a */
		for (col = 0; col < nOfColumns; col++)
			for (row = 0; row < nOfRows; row++)
				if (fabs(distMatrix[row + nOfRows * col]) < DBL_EPSILON)
					if (!coveredRows[row])
					{
						starMatrix[row + nOfRows * col] = true;
						coveredColumns[col] = true;
						coveredRows[row] = true;
						break;
					}
		for (row = 0; row<nOfRows; row++)
			coveredRows[row] = false;

	}

	/* move to step 2b */
	step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);

	/* compute cost and remove invalid assignments */
	computeassignmentcost(assignment, cost, distMatrixIn, nOfRows);

	/* free allocated memory */
	free(distMatrix);
	free(coveredColumns);
	free(coveredRows);
	free(starMatrix);
	free(primeMatrix);
	free(newStarMatrix);
	return;
}

/********************************************************/
void HungarianAlgorithm::buildassignmentvector(int *assignment, bool *starMatrix, int nOfRows, int nOfColumns)
{
	int row, col;

	for (row = 0; row < nOfRows; row++)
		for (col = 0; col < nOfColumns; col++)
			if (starMatrix[row + nOfRows * col])
			{
#ifdef ONE_INDEXING
				assignment[row] = col + 1; /* MATLAB-Indexing */
#else
				assignment[row] = col;
#endif
				break;
			}
}

/********************************************************/
void HungarianAlgorithm::computeassignmentcost(int *assignment, double *cost, double *distMatrix, int nOfRows)
{
	int row, col;

	for (row = 0; row < nOfRows; row++)
	{
		col = assignment[row];
		if (col >= 0)
			*cost += distMatrix[row + nOfRows * col];
	}
}

/********************************************************/
void HungarianAlgorithm::step2a(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
{
	bool *starMatrixTemp, *columnEnd;
	int col;

	/* cover every column containing a starred zero */
	for (col = 0; col < nOfColumns; col++)
	{
		starMatrixTemp = starMatrix + nOfRows*col;
		columnEnd = starMatrixTemp + nOfRows;
		while (starMatrixTemp < columnEnd) {
			if (*starMatrixTemp++)
			{
				coveredColumns[col] = true;
				break;
			}
		}
	}

	/* move to step 3 */
	step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}

/********************************************************/
void HungarianAlgorithm::step2b(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
{
	int col, nOfCoveredColumns;

	/* count covered columns */
	nOfCoveredColumns = 0;
	for (col = 0; col < nOfColumns; col++)
		if (coveredColumns[col])
			nOfCoveredColumns++;

	if (nOfCoveredColumns == minDim)
	{
		/* algorithm finished */
		buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns);
	}
	else
	{
		/* move to step 3 */
		step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
	}

}

/********************************************************/
void HungarianAlgorithm::step3(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
{
	bool zerosFound;/* generate working copy of distance Matrix */
	/* check if all matrix elements are positive */
	int row, col, starCol;

	zerosFound = true;
	while (zerosFound)
	{
		zerosFound = false;
		for (col = 0; col < nOfColumns; col++)
			if (!coveredColumns[col])
				for (row = 0; row < nOfRows; row++)
					if ((!coveredRows[row]) && (fabs(distMatrix[row + nOfRows * col]) < DBL_EPSILON))
					{
						/* prime zero */
						primeMatrix[row + nOfRows*col] = true;

						/* find starred zero in current row */
						for (starCol = 0; starCol < nOfColumns; starCol++)
							if (starMatrix[row + nOfRows * starCol])
								break;

						if (starCol == nOfColumns) /* no starred zero found */
						{
							/* move to step 4 */
							step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim, row, col);
							return;
						}
						else
						{
							coveredRows[row] = true;
							coveredColumns[starCol] = false;
							zerosFound = true;
							break;
						}
					}
	}

	/* move to step 5 */
	step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}

/********************************************************/
void HungarianAlgorithm::step4(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim, int row, int col)
{
	int n, starRow, starCol, primeRow, primeCol;
	int nOfElements = nOfRows * nOfColumns;

	/* generate temporary copy of starMatrix */
	for (n = 0; n < nOfElements; n++)
		newStarMatrix[n] = starMatrix[n];

	/* star current zero */
	newStarMatrix[row + nOfRows * col] = true;

	/* find starred zero in current column */
	starCol = col;
	for (starRow = 0; starRow<nOfRows; starRow++)
		if (starMatrix[starRow + nOfRows * starCol])
			break;

	while (starRow < nOfRows)
	{
		/* unstar the starred zero */
		newStarMatrix[starRow + nOfRows * starCol] = false;

		/* find primed zero in current row */
		primeRow = starRow;
		for (primeCol = 0; primeCol < nOfColumns; primeCol++)
			if (primeMatrix[primeRow + nOfRows * primeCol])
				break;

		/* star the primed zero */
		newStarMatrix[primeRow + nOfRows * primeCol] = true;

		/* find starred zero in current column */
		starCol = primeCol;
		for (starRow = 0; starRow < nOfRows; starRow++)
			if (starMatrix[starRow + nOfRows * starCol])
				break;
	}

	/* use temporary copy as new starMatrix */
	/* delete all primes, uncover all rows */
	for (n = 0; n < nOfElements; n++)
	{
		primeMatrix[n] = false;
		starMatrix[n] = newStarMatrix[n];
	}
	for (n = 0; n < nOfRows; n++)
		coveredRows[n] = false;

	/* move to step 2a */
	step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}

/********************************************************/
void HungarianAlgorithm::step5(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
{
	double h, value;
	int row, col;

	/* find smallest uncovered element h */
	h = DBL_MAX;
	for (row = 0; row < nOfRows; row++)
		if (!coveredRows[row])
			for (col = 0; col < nOfColumns; col++)
				if (!coveredColumns[col])
				{
					value = distMatrix[row + nOfRows * col];
					if (value < h)
						h = value;
				}

	/* add h to each covered row */
	for (row = 0; row < nOfRows; row++)
		if (coveredRows[row])
			for (col = 0; col < nOfColumns; col++)
				distMatrix[row + nOfRows * col] += h;

	/* subtract h from each uncovered column */
	for (col = 0; col < nOfColumns; col++)
		if (!coveredColumns[col])
			for (row = 0; row < nOfRows; row++)
				distMatrix[row + nOfRows * col] -= h;

	/* move to step 3 */
	step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}

Hungarian.h

#pragma once
#include <iostream>
#include <vector>
#include <stdlib.h>
#include <math.h>


class HungarianAlgorithm
{
public:
	HungarianAlgorithm();
	~HungarianAlgorithm();
	double Solve(std::vector<std::vector<double>>& DistMatrix, std::vector<int>& Assignment);

private:
	void assignmentoptimal(int *assignment, double *cost, double *distMatrix, int nOfRows, int nOfColumns);
	void buildassignmentvector(int *assignment, bool *starMatrix, int nOfRows, int nOfColumns);
	void computeassignmentcost(int *assignment, double *cost, double *distMatrix, int nOfRows);
	void step2a(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
	void step2b(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
	void step3(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
	void step4(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim, int row, int col);
	void step5(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
};

2.4、TrackingInfo

TrackingInfo.h 文件数据格式。

TrackingInfo.h

#pragma once
#include <string>
#include <vector>
#include <memory>
#include <set>


/*
 * 目标检测信息
 */
typedef struct DetBox
{
	float x; 			// xy左上角坐标  
	float y;
	float w; 			// wh目标长宽（已复原到原图坐标）
	float h;
	int type; 			// 当前类别 "pedestrian","car", "bus","truck", "cyclist", "motorcyclist", "tricyclist", 
	float score; 		// score = ObjConf * ClsConf
}DetBox;

typedef struct DetectInfo
{
	std::vector<DetBox> boxes;
}DetectInfo;

/*
 * 目标跟踪初始化
 */
typedef struct DisInit
{
    float h;                    // 相机离地面距离
    float pitch;                // 俯仰角
    std::vector<double> mtx;    // 内参矩阵
    std::vector<double> dist;   // 畸变系数
    std::vector<double> r;      // 相机外参，相对于车体 旋转矩阵
    std::vector<double> t;      // 相机外参，相对于车体 平移矩阵
}DisInit;

/*
 * 目标跟踪信息
 */
typedef struct TrackerImageInfo
{
    std::string sensor;         // 关联那个传感器如：“head_camera”
    int framecnt;               // 图片的帧数
    double timestamp;           // 图片的时间戳
}TrackerImageInfo;

typedef struct TrackerResult
{
    int label;                              // 目标标签
    float score;                            // 置信度
    int id;                                 // 目标id
    std::vector<float> position;            // 目标的位置 x,y
    std::vector<float> box;                 // x1,y1,x2,y2
}TrackerResult;

typedef struct TrackerMessage
{
    std::vector<TrackerResult> trackerResults; 
}TrackerMessage;

三、调用示例

Tracking tracking;
tracking.InitData(std::make_shared<DisInit>(cameraParam)); // 初始化获取相机内外参
计算获取结果 for (int fi = 1; fi < FrameCount; fi++) { tracking.SetInputTrackingMessage(std::make_shared<DetectInfo>(detBox)); // 输入当前帧检测信息 tracking.TargetTracking(); // 计算 TrackerMessage messageResult = *tracking.GetOutputTrackingMessage(); // 获取当前帧跟踪输出结果 }