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

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

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

3D目標檢測跟蹤

3D目標檢測

目標檢測是自動駕駛/機器人環境感知系統中的一項基本任務，常見的目標檢測算法可以分為兩大類:

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

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

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

框架流程圖

點云3D目標檢測

自2017年以來，點云3D目標檢測涌現了很多經典算法，下圖是按時間排序的檢測算法:

算法一覽

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

LSD基于原版PointPillar網絡結構，對模型結構進行了優化:

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

SSD backbone替換為基于CSPNet結構的backbone

Anchor based檢測頭替換為CenterPoint網絡的Anchor free檢測頭，并添加了IOU aware分支

Voxelize

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

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

voxel坐標，維度為[m, 3]

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

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

其中m為計算得到的voxel個數，上述4個變量作為后續神經網絡的輸入。

Pillar VFE

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

ONNX格式的Pillar VFE

CSP Backbone

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

參考的YOLOV4網絡結構

CenterPoint Head

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

CenterPoint網絡

單目3D目標檢測

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

RTM3D

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

RTM3D

在LSD中，我們對RTM3D的結構做了部分微調：

由ImageNet預訓練的Resnet18 backbone替換為Darknet53

檢測頭增加heatmap輸出，用于后續的目標融合。

模型訓練和推理

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

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

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

后融合

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

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

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

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

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

合并matched, unmatch_camera和unmatch_lidar得到最終的目標列表輸出

多目標跟蹤

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

根據相似度對相鄰幀目標進行關聯匹配

對目標進行觀測并估計最優狀態

LSD采用了傳統的基于規則的目標匹配和狀態估計算法，算法基于AB3DMOT(A Baseline for 3D Multi-Object Tracking and New Evaluation Metrics)實現，并在其基礎上進行了優化，能夠輸出更穩定的目標列表。

AB3DMOT

目標匹配

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

相似度計算

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

針對車輛目標，計算兩兩之間的BEV IOU

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

最優匹配求解

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

matched: 前后幀匹配上的目標

unmatch_old: 在前一幀跟蹤列表中未匹配上的目標

unmatch_new: 當前幀檢測輸出但未匹配上的目標

卡爾曼濾波跟蹤

卡爾曼濾波是一種狀態估計算法，在LSD中，我們使用卡爾曼濾波算法對目標進行建模和狀態估計:

針對unmatch_new目標，作為當前幀新出現的目標，添加進跟蹤列表

針對unmatch_old目標，表示當前幀該目標未檢測到，通過運動學模型對其進行預測

針對matched目標，將當前幀作為觀測值，利用卡爾曼濾波器對其狀態更新

實車測試

我們只使用了公開數據集進行訓練，在未訓練過的城市道路環境中實車測試，檢驗模型和跟蹤算法的魯棒性。