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

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

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

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

3D目標(biāo)檢測

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

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

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

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

框架流程圖

點云3D目標(biāo)檢測

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

算法一覽

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

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

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

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

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

Voxelize

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

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

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

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

每個voxel中點的mask，維度為[m, 32]

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

Pillar VFE

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

ONNX格式的Pillar VFE

CSP Backbone

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

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

CenterPoint Head

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

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

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

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

RTM3D

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

RTM3D

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

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

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

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

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

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

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

后融合

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

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

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

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

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

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

多目標(biāo)跟蹤

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

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

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

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

AB3DMOT

目標(biāo)匹配

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

相似度計算

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

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

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

最優(yōu)匹配求解

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

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

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

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

卡爾曼濾波跟蹤

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

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

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

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

實車測試

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