【目标跟踪】多相机环视跟踪

文章目录

一、前言
二、流程图
三、实现原理
- 3.1、初始化
- 3.2、输入
- 3.3、初始航迹
- 3.4、航迹预测
- 3.5、航迹匹配
- 3.6、输出结果
四、c++ 代码
五、总结

一、前言

多相机目标跟踪主要是为了实现 360 度跟踪。单相机检测存在左右后的盲区视野。
在智能驾驶领域，要想靠相机实现无人驾驶，相机必须 360 度无死角全覆盖。
博主提供一种非深度学习方法，采用kalman滤波+匈牙利匹配方式实现环视跟踪。有兴趣可以参考往期【目标跟踪】系列博客。
本文干货满满，可以先点赞关注收藏，以免下次找不到。欢迎各位吴彦祖私信交流学习。

二、流程图

干货怎么会少了流程图，上图!上图!上图!。流程图用 processon 制作的，不是付费软件买不起，而是免费的更有性价比。

在这里插入图片描述

这里最重要的就是航迹管理，对于四路相机(front, left, right, back) 检测的目标都需要管理。这里面不仅要对单相机目标进行跟踪，且对跨相机的目标也需要进行跟踪，还有各种匹配，初始化，开始消亡等到。想想都头皮发麻，那到底怎么做？别慌，让我娓娓道来。

街上熙熙攘攘的人群交头接耳，果然雨过天晴就会有聊不完的话题。三只白鹭停泊在河边享受着阳光的沐浴，不时煽动自己羽毛，引起阵阵涟漪，如此惬意无不让打工人憧憬。远处的山峰层峦叠嶂，仿佛去到世界尽头。命运的齿轮正在转动，会当凌绝顶，一览纵山下。

wc !!!思绪差点飘离银河系，还是敲键盘要紧。下面看看实现原理。

三、实现原理

3.1、初始化

保存四路相机参数：4路相机内参与外参。

制定表格 1920 * 1080 ,用于后续判断计算校正后的像素点。对整张图片进行校正会占用大量 CPU，我们采用对原图进行计算。

在这里插入图片描述

3.2、输入

nuscenes 是六路相机。我们这里是四路相机，全部都是 190° 鱼眼相机。

输入四路相机检测结果。当前相机当前帧的所有目标信息

二维 box 检测框 (x,y,w,h)
目标列表 label。如person、car等
目标的置信度 score

演示这里采取的是 2D 检测。如果是 3D 检测也是类似计算，且 3D 检测定位更加精准。

3.3、初始航迹

(1) 划分 4 象限

在这里插入图片描述

(2) 划分 8 区域

前相机第4象限、前相机第1象限、右相机第3象限、右相机第4象限、后相机第2象限、后相机第3象限、左相机第1象限、左相机第2象限。

在这里插入图片描述

(3) 划分 2 类

前相机第4象限、右相机第3象限、后相机第2象限、左相机第1象限为一类 firstBox。

右相机第4象限、后相机第3象限、左相机第2象限、前相机第1象限为一类 secondBox。

(4) 分配 id

当目标是属于 firstBox 一类的目标时，此时直接分配 id 。分配一个 id ，则 idCount ++。

(5) 匹配

sencondBox 中的目标需要与 firstBox 目标匹配。计算距离与角度。

如果匹配上时，id = firstBox 目标 id

如果未匹配上时，id = idCount , idCount ++

3.4、航迹预测

这部分比较简单, 正常预测就行。

对每个目标的box进行预测; 对每个目标的状态，距离、速度、加速度进行预测; 得到预测后的目标状态、box、相应传感器（枚举）。

3.5、航迹匹配

(1) 预测框与检测框

预测框是上一帧所获得的预测框;检测框是当前检测信息的检测框。同时对目标有预测的距离与测量距离

(2) 匈牙利匹配矩阵

如果属于不同相机检测的目标，则设为默认最大值;如果属于同一相机检测目标，计算iou。这个是与单相机跟踪类似。

(3) 匹配修正

匹配上的目标，修正预测的状态与box。此时idCount 不变。

(4) 未匹配的检测框

对于未匹配的检测框，寻找航迹中在其他相机的目标，进行再次匹配。如果匹配上，则未匹配检测框id = 其他相机目标id。否则赋予id = idCount，idCount++。

3.6、输出结果

输出俯视角下所有目标的位置(x, y)。

四、c++ 代码

这部分代码比较多，展示划分象限初始航迹代码。其他代码放到资源自行下载。

// 1、管理航迹信息
void Tracking::ManageTrack(std::vector<TrackingBox> detectData) 
{
    std::vector<StateBox> firstBox;                 // 目标所处相机与第几象限 front 4, right 3, back 2, left 1 
    std::vector<StateBox> secondBox;                // 目标所处相机与第几象限 right 4, back 3, left 2, front 1 
    // 1.1 分配id
    for (unsigned int i = 0; i < detectData.size(); i++) {
        StateBox stateBox;
        stateBox.sensor = detectData[i].sensor;                                       // 相机传感器的方位
        stateBox.label = detectData[i].label;                                         // 目标标签
        stateBox.score = detectData[i].score;                                         // 目标置信度
        stateBox.box = detectData[i].box;                                             // 检测框
        std::vector<float> p = detectData[i].position;                                // 根据图像像素获取世界位置 x,y相对于车体
        stateBox.position = p;
        stateBox.state = {(cv::Mat_<float>(6, 1) << p[0], 0, 0, p[1], 0, 0), pInit};  // 初始化distanceKalman的状态与协方差
        if ((stateBox.sensor == 0 && p[1] < 0) || (stateBox.sensor == 1 && p[0] < 0) || 
                (stateBox.sensor == 2 && p[1] > 0) || (stateBox.sensor == 3 && p[0] > 0) ) {
            stateBox.kBox = KalmanTracker(detectData[i].box);                         // KalmanTracker所需的box
            stateBox.id = countId;
            countId++;
            firstBox.push_back(stateBox);
        }
        else {
            secondBox.push_back(stateBox);
        }
    }
    for (auto box:firstBox) {
        trackers.push_back(box);
    }
    // 1.2 匹配id
    unsigned int firstNum = firstBox.size();                             // 前相机在右侧个数
	unsigned int secondNum = secondBox.size();                           // 右相机在前侧个数
    std::vector<std::vector<double>> matrix;                             // 关联矩阵->匈牙利匹配
    matrix.resize(firstNum, std::vector<double>(secondNum, 1));          // resize关联矩阵大小
    std::vector<cv::Point> matchedPairs;  
    if (firstNum != 0 && secondNum != 0) {
        for (unsigned int i = 0; i < firstNum; i++) {
            for (unsigned int j = 0; j < secondNum; j++) {
                matrix[i][j] = GetWeight(firstBox[i].position, secondBox[j].position); // 计算角度权重
            }
        }
        HungarianAlgorithm hungAlgo;
        std::vector<int> assignment; 
        hungAlgo.Solve(matrix, assignment);     // 匈牙利匹配计算
        std::set<int> unMatchedSecondBox;       // 存放未匹配的sencond 类型的框
        for (unsigned int i = 0; i < firstNum; ++i) {
            if (assignment[i] == -1) { 
                continue;                       // 过滤掉无效的值
            }
            matchedPairs.push_back(cv::Point(i, assignment[i]));
        }
    }
    // 1.3 修正id
    int firstId, secondId;
    for (unsigned int i = 0; i < matchedPairs.size(); i++) {
        firstId = matchedPairs[i].x; 
        secondId = matchedPairs[i].y; 
        if (matrix[firstId][secondId] < hungarianThreshold) {
            secondBox[secondId].kBox = KalmanTracker(detectData[i].box);
            secondBox[secondId].id = firstBox[firstId].id;
            trackers.push_back(secondBox[secondId]);
        }
    }
    for (auto secBox:secondBox) {
        if (secBox.id == -1) {
            secBox.kBox = KalmanTracker(secBox.box);
            secBox.id = countId;
            countId++;
            trackers.push_back(secBox);
        }
    }
}