LSD (LiDAR SLAM & Detection) 是一個(gè)開(kāi)源的面向自動(dòng)駕駛/機(jī)器人的環(huán)境感知算法框架，能夠完成數(shù)據(jù)采集回放、多傳感器標(biāo)定、SLAM建圖定位和障礙物檢測(cè)等多種感知任務(wù)。

本文將詳細(xì)講解LSD中的激光點(diǎn)云/單目3D目標(biāo)檢測(cè)和多目標(biāo)跟蹤算法。

LSD（LiDAR SLAM & Detection） github.com/w111liang222/lidar-slam-detection

3D目標(biāo)檢測(cè)跟蹤

3D目標(biāo)檢測(cè)

目標(biāo)檢測(cè)是自動(dòng)駕駛/機(jī)器人環(huán)境感知系統(tǒng)中的一項(xiàng)基本任務(wù)，常見(jiàn)的目標(biāo)檢測(cè)算法可以分為兩大類(lèi):

基于圖像的2D目標(biāo)檢測(cè)，輸出圖像坐標(biāo)下的2D Box(x, y, l, w, c)，由于缺少目標(biāo)的深度和朝向信息，難以用于機(jī)器人的規(guī)劃和控制

基于激光點(diǎn)云/相機(jī)的3D目標(biāo)檢測(cè)算法，輸出以自車(chē)為原點(diǎn)坐標(biāo)系下的3D Box (x, y, z, l, w, h, yaw, c)

LSD采用了改進(jìn)的PointPillar和RTM3D算法，能夠?qū)崿F(xiàn)激光點(diǎn)云/單目相機(jī)的3D目標(biāo)檢測(cè)功能，在推理時(shí)轉(zhuǎn)換成TensorRT FP16引擎運(yùn)算，支持在x86 PC, Xavier NX, Xavier AGX和Orin平臺(tái)上實(shí)時(shí)運(yùn)行。相關(guān)實(shí)現(xiàn)代碼位于sensor_inference目錄。整體框架如下圖所示:

框架流程圖

點(diǎn)云3D目標(biāo)檢測(cè)

自2017年以來(lái)，點(diǎn)云3D目標(biāo)檢測(cè)涌現(xiàn)了很多經(jīng)典算法，下圖是按時(shí)間排序的檢測(cè)算法:

算法一覽

跟圖像2D目標(biāo)檢測(cè)算法類(lèi)似，3D目標(biāo)檢測(cè)算法也可以分類(lèi)為"單階段"/"多階段"、"Anchor base"/"Anchor free"、"Point base"/"Voxel base"等，其中APTIV提出的PointPillar由于其部署簡(jiǎn)單、計(jì)算效率和檢測(cè)精度較高等優(yōu)勢(shì)，在實(shí)際的工程項(xiàng)目中應(yīng)用廣泛。

LSD基于原版PointPillar網(wǎng)絡(luò)結(jié)構(gòu)，對(duì)模型結(jié)構(gòu)進(jìn)行了優(yōu)化:

在PillarVFE結(jié)構(gòu)中刪除了xyz的均值特征，保留了每個(gè)點(diǎn)的x,y,z,i及其voxel中心offset特征

SSD backbone替換為基于CSPNet結(jié)構(gòu)的backbone

Anchor based檢測(cè)頭替換為CenterPoint網(wǎng)絡(luò)的Anchor free檢測(cè)頭，并添加了IOU aware分支

Voxelize

由于PointPillar網(wǎng)絡(luò)的輸入是Voxel(z軸方向只有1維，也叫作Pillar)，因此我們需要將原始點(diǎn)云[n, 4]體素化，其中n為點(diǎn)云個(gè)數(shù)和每個(gè)點(diǎn)的特征(x,y,z,intensity)。在LSD中，我們?cè)O(shè)置每個(gè)voxel的尺寸為[0.32, 0.32, 6]，單位為米，且每個(gè)voxel最多包含32個(gè)點(diǎn)，通過(guò)將點(diǎn)云投影到其對(duì)應(yīng)的voxel中，可以得到:

voxel特征，維度為[m, 32, 4]

voxel坐標(biāo)，維度為[m, 3]

每個(gè)voxel中點(diǎn)的個(gè)數(shù)，維度為[1, m]

每個(gè)voxel中點(diǎn)的mask，維度為[m, 32]

其中m為計(jì)算得到的voxel個(gè)數(shù)，上述4個(gè)變量作為后續(xù)神經(jīng)網(wǎng)絡(luò)的輸入。

Pillar VFE

Pillar VFE結(jié)構(gòu)主要是對(duì)每個(gè)voxel中的點(diǎn)特征進(jìn)行提取(由7維特征[x, y, z, i, xp, yp, zp]經(jīng)過(guò)卷積得到64維)，并根據(jù)每個(gè)voxel的坐標(biāo)投影到2D BEV網(wǎng)格得到[64，448，448]大小的特征(最大檢測(cè)范圍設(shè)置為71.68m)

ONNX格式的Pillar VFE

CSP Backbone

PointPillar原版采用的是SSD backbone，其結(jié)構(gòu)較為簡(jiǎn)單、參數(shù)量小，在實(shí)際測(cè)試時(shí)發(fā)現(xiàn)當(dāng)訓(xùn)練數(shù)據(jù)量增大后檢測(cè)精度提升幅度較小，因此，我們參考了YOLO V4的backbone，其主要由CSP Net基礎(chǔ)block組成，通過(guò)權(quán)衡檢測(cè)精度和實(shí)時(shí)性，我們構(gòu)建了4個(gè)下采樣block和3個(gè)上采樣block網(wǎng)絡(luò)來(lái)對(duì)多尺度特征進(jìn)行提取和融合，最后輸出[48，224，224]和[192, 56, 56]兩種尺度特征，詳細(xì)的網(wǎng)絡(luò)結(jié)構(gòu)可以參見(jiàn)CSPDarknet53Small.py。

參考的YOLOV4網(wǎng)絡(luò)結(jié)構(gòu)

CenterPoint Head

LSD采用了多檢測(cè)頭結(jié)構(gòu)，針對(duì)backbone輸出的2個(gè)尺度特征，分別連接了兩個(gè)CenterPoint檢測(cè)頭，相比于原版的Anchor based檢測(cè)頭，Anchor free在檢測(cè)小目標(biāo)時(shí)精度更高。另外，我們還參考了CIA-SSD網(wǎng)絡(luò)模型，添加了IOU-Aware預(yù)測(cè)分支，進(jìn)一步提高了檢測(cè)精度。

CenterPoint網(wǎng)絡(luò)

單目3D目標(biāo)檢測(cè)

大多數(shù)激光雷達(dá)由于發(fā)射結(jié)構(gòu)的原因，其點(diǎn)云具有近處密集，遠(yuǎn)處稀疏的特點(diǎn)，從而導(dǎo)致激光雷達(dá)在檢測(cè)距離較遠(yuǎn)的目標(biāo)時(shí)精度較低，而圖像在距離較遠(yuǎn)時(shí)相比激光雷達(dá)更容易分辨目標(biāo)，因此LSD采用了單目3D目標(biāo)檢測(cè)算法來(lái)提高遠(yuǎn)距離目標(biāo)的檢測(cè)精度以適應(yīng)高速場(chǎng)景。

RTM3D

RTM3D (Real-time Monocular 3D Detection from Object Keypoints for Autonomous Driving)是一個(gè)單目3D目標(biāo)檢測(cè)網(wǎng)絡(luò)，其主要思想是通過(guò)網(wǎng)絡(luò)預(yù)測(cè)目標(biāo)框的8個(gè)頂點(diǎn)、質(zhì)心、深度和朝向信息，并求解偽逆方程得到目標(biāo)的3DBox。

RTM3D

在LSD中，我們對(duì)RTM3D的結(jié)構(gòu)做了部分微調(diào)：

由ImageNet預(yù)訓(xùn)練的Resnet18 backbone替換為Darknet53

檢測(cè)頭增加heatmap輸出，用于后續(xù)的目標(biāo)融合。

模型訓(xùn)練和推理

模型訓(xùn)練基于OpenPCDet框架實(shí)現(xiàn)，將公開(kāi)的WOD、Nuscences、Lyft、Panda Set和DeepRoute等數(shù)據(jù)集進(jìn)行了合并，并且將不同數(shù)據(jù)集的各個(gè)類(lèi)別重新分類(lèi)為“車(chē)輛”、“行人”、“騎行車(chē)”和“三角錐”4種類(lèi)別。服務(wù)器采用4張2080Ti顯卡訓(xùn)練50個(gè)epoch。

在模型推理階段，我們將Pytorch模型導(dǎo)出成ONNX文件，并分別在Jetpack5.0.2 Xavier NX、 AGX、 Orin平臺(tái)上生成了TensorRT FP16引擎進(jìn)行推理（單目3D模型則運(yùn)行在DLA上），在Xavier NX平臺(tái)上運(yùn)行耗時(shí)約80ms，能夠滿(mǎn)足實(shí)時(shí)性要求。

ONNX模型文件可以從這里獲取。

后融合

數(shù)據(jù)融合用于融合多傳感器數(shù)據(jù)來(lái)提高檢測(cè)精度，按實(shí)施階段可以分為前融合和后融合，前融合通常處理傳感器的原始數(shù)據(jù)，而后融合則處理各傳感器分別計(jì)算后的目標(biāo)數(shù)據(jù)。在LSD中，我們采用了后融合的方式，將激光點(diǎn)云輸出的3D目標(biāo)列表和單目視覺(jué)輸出的3D目標(biāo)列表融合為一個(gè)目標(biāo)列表并輸出給目標(biāo)跟蹤模塊。我們實(shí)現(xiàn)了一個(gè)相對(duì)簡(jiǎn)單的基于規(guī)則的融合算法：

將激光點(diǎn)云輸出的3DBox根據(jù)激光-相機(jī)外參投影到圖像坐標(biāo)系下得到2DBox，計(jì)算與視覺(jué)目標(biāo)的2D IOU

利用匈牙利算法進(jìn)行匹配，得到matched、unmatch_camera和unmatch_lidar三個(gè)目標(biāo)列表

針對(duì)matched列表，我們將激光點(diǎn)云輸出的3DBox直接作為輸出，對(duì)于confidence屬性求激光和相機(jī)目標(biāo)的均值加上0.2 * IOU

針對(duì)unmatch_lidar列表，利用RTM3D輸出的heat map對(duì)confidence屬性求均值

合并matched, unmatch_camera和unmatch_lidar得到最終的目標(biāo)列表輸出

多目標(biāo)跟蹤

3D目標(biāo)檢測(cè)和融合算法處理的是傳感器的單幀數(shù)據(jù)，單幀檢測(cè)難以獲取目標(biāo)的運(yùn)動(dòng)信息，并且容易出現(xiàn)檢測(cè)結(jié)果跳變等問(wèn)題，在時(shí)間序列上對(duì)目標(biāo)進(jìn)行跟蹤，可以估算出目標(biāo)的運(yùn)動(dòng)速度、角速度，同時(shí)能夠輸出較為穩(wěn)定的目標(biāo)列表給下游規(guī)劃控制模塊。目標(biāo)跟蹤算法通?？梢苑纸獬蓛蓚€(gè)步驟：

根據(jù)相似度對(duì)相鄰幀目標(biāo)進(jìn)行關(guān)聯(lián)匹配

對(duì)目標(biāo)進(jìn)行觀測(cè)并估計(jì)最優(yōu)狀態(tài)

LSD采用了傳統(tǒng)的基于規(guī)則的目標(biāo)匹配和狀態(tài)估計(jì)算法，算法基于AB3DMOT(A Baseline for 3D Multi-Object Tracking and New Evaluation Metrics)實(shí)現(xiàn)，并在其基礎(chǔ)上進(jìn)行了優(yōu)化，能夠輸出更穩(wěn)定的目標(biāo)列表。

AB3DMOT

目標(biāo)匹配

匈牙利算法是一種求解二分圖最小權(quán)值匹配問(wèn)題的方法

相似度計(jì)算

針對(duì)不同類(lèi)別，我們采用了不同的方法來(lái)計(jì)算前后幀目標(biāo)相似度(權(quán)值):

針對(duì)車(chē)輛目標(biāo)，計(jì)算兩兩之間的BEV IOU

針對(duì)行人、騎行者和三角錐，由于目標(biāo)較小，前后幀的IOU大部分為0，因此計(jì)算兩兩中心點(diǎn)的歐式距離

最優(yōu)匹配求解

得到相似度矩陣后，我們利用scipy.optimize.linear_sum_assignment函數(shù)求解得到三個(gè)輸出:

matched: 前后幀匹配上的目標(biāo)

unmatch_old: 在前一幀跟蹤列表中未匹配上的目標(biāo)

unmatch_new: 當(dāng)前幀檢測(cè)輸出但未匹配上的目標(biāo)

卡爾曼濾波跟蹤

卡爾曼濾波是一種狀態(tài)估計(jì)算法，在LSD中，我們使用卡爾曼濾波算法對(duì)目標(biāo)進(jìn)行建模和狀態(tài)估計(jì):

針對(duì)unmatch_new目標(biāo)，作為當(dāng)前幀新出現(xiàn)的目標(biāo)，添加進(jìn)跟蹤列表

針對(duì)unmatch_old目標(biāo)，表示當(dāng)前幀該目標(biāo)未檢測(cè)到，通過(guò)運(yùn)動(dòng)學(xué)模型對(duì)其進(jìn)行預(yù)測(cè)

針對(duì)matched目標(biāo)，將當(dāng)前幀作為觀測(cè)值，利用卡爾曼濾波器對(duì)其狀態(tài)更新

實(shí)車(chē)測(cè)試

我們只使用了公開(kāi)數(shù)據(jù)集進(jìn)行訓(xùn)練，在未訓(xùn)練過(guò)的城市道路環(huán)境中實(shí)車(chē)測(cè)試，檢驗(yàn)?zāi)Ｐ秃透櫵惴ǖ聂敯粜浴?/p>