上一篇博客介紹了無人駕駛中的車輛檢測算法（YOLO模型），該檢測是基于圖像進行的2D目標檢測。在無人駕駛環境感知傳感器中還有另一種重要的傳感器：激光雷達。今天就介紹一篇無人駕駛中基于激光雷達目標檢測的3D多目標追蹤論文，使用的數據集也是KITTI數據集，使用的目標檢測算法是PointRCNN。該3D多目標追蹤系統不需要使用GPU且取得了最快的處理速度（214.7FPS）。

論文地址：A Baseline for 3D Multi-Object Tracking

Github地址：https://github.com/xinshuoweng/AB3DMOT

---

大家有時間可以看看這個視頻，這是論文作者此前分享的一個3D多目標追蹤報告，視頻地址：https://www.bilibili.com/video/BV1DD4y1Q7JG/

3D Multi-Object Tracking: A Baseline and New Evaluation Metrics

文章目錄

• • • 1.概述
    • 2.方法
    • • A. 3D 目標檢測
      • B. 3D 卡爾曼濾波---狀態預測
      • C. 數據關聯
      • D. 3D卡爾曼濾波-狀態更新
      • E. Birth and Death內存
    • 3. 測試結果

1.概述

無論是在自動駕駛或輔助機器人等實時應用中，3D多目標追蹤（MOT）都是非常重要的技術環節。然而，在近期的一些3D多目標追蹤研究當中更多的都是關注于追蹤系統的準確率而忽略了其計算成本和復雜性。

在本文中作者提出了一種簡單而又準確的實時3D MOT基線系統。本文使用了現成的3D目標檢測器從LiDAR點云數據中獲取對應目標3D bounding box。 然后，將3D卡爾曼濾波器和匈牙利算法結合起來進行目標狀態估計和數據關聯。為了評估提出的基線系統，論文在原有的KITTI官方2D MOT評價指標上進行了擴展，提出了三個新的3D MOT評價指標，本文主要貢獻有：

• 設計了一種簡單而又準確的實時3D MOT系統；
• 對KITTI官方2D MOT評價指標進行擴展，提出了新的3D MOT評價指標；
• 在KITTI評價工具上獲得了最優的3D MOT性能，在2D MOT評價工具上獲得了最快的處理速度（如下圖所示）。

---

2.方法

3D MOT的目的是對一段連續數據中的3D 檢測目標進行關聯。在本系統中，需要使用當前幀中的檢測結果以及上一幀中的關聯軌跡。系統結構如下圖所示，由5個部分組成：

（A）3D檢測模塊用于對LiDAR點云數據進行目標檢測；

（B）3D卡爾曼濾波模塊用于預測關聯軌跡當前幀的狀態；

（C）數據關聯模塊用于對檢測模塊輸出結果和卡爾曼濾波模塊輸出的軌跡進行匹配關聯；

（D）3D卡爾曼濾波根據檢測結果更新匹配軌跡的狀態；

（E）birth和death內存模塊用于生成新的軌跡和刪除未匹配的目標軌跡。

除了3D目標檢測塊以外，3D MOT系統不需要進行訓練，可直接使用。

---

A. 3D 目標檢測

在這里，直接使用在KITTI 3D數據及上訓練好的3D 檢測模型（即：PointRCNN）作為目標檢測模塊。

下面是一些術語規定，假設當前為$t$時刻，3D檢測模塊輸出是：$D_t=\{D_t^1,D_t^2,...,D_t^{n_t}\}$，($n_t$為$t$時刻中檢測目標數量)。每一個檢測目標向量表示形式為：$(x,y,z,\theta ,l,w,h,s)$，即目標中心坐標$(x,y,z)$，目標尺寸$(l,w,h)$，偏航角$\theta$以及置信度$s$

---

B. 3D 卡爾曼濾波—狀態預測

為了預測目標從上一幀到當前幀的軌跡狀態，這里選擇使用的運動模型為恒速運動模型，忽略了相鄰幀內的目標位置移動。

具體地，我們使用11維向量來表示目標軌跡的狀態$T=(x,y,z,\theta ,l,w,h,s,v_x,v_{y,}{v}_z)$，附加的3個變量表示目標在3個維度上的運動速度。

在每一幀，所有t-1幀的關聯軌跡為$T_{t?1}=\left\{T_{t?1}^1,T_{t?1}^2,?,T_{t?1}^{m_{t?1}}\right\}$（$m_{t?1}$是t-1時刻的軌跡數量）將會被傳播到下一幀$t$，用$T_{est}$表示。由恒速運動模型可以推出：
$$x_{est}=x+v_x{y}_{est}=y+v_y{z}_{est}=z+v_z$$
因此，$T_{t?1}$中的每個軌跡$T_{t?1}^i$傳播到t幀中的預測狀態將是$T_{\mathrm{e}\mathrm{s}\mathrm{t}}^i=\left(x_{\mathrm{e}\mathrm{s}\mathrm{t}},y_{\mathrm{e}\mathrm{s}\mathrm{t}},z_{\mathrm{e}\mathrm{s}\mathrm{t}},\theta ,l,w,h,s,v_x,v_y,v_z\right)$，軌跡數據將會用于之后的關聯模塊。

3D 卡爾曼濾波的代碼如下：

class KalmanBoxTracker(object):"""This class represents the internel state of individual tracked objects observed as bbox."""count = 0def __init__(self, bbox3D, info):"""Initialises a tracker using initial bounding box."""# define constant velocity modelself.kf = KalmanFilter(dim_x=10, dim_z=7) self.kf.F = np.array([[1,0,0,0,0,0,0,1,0,0], # state transition matrix[0,1,0,0,0,0,0,0,1,0],[0,0,1,0,0,0,0,0,0,1],[0,0,0,1,0,0,0,0,0,0], [0,0,0,0,1,0,0,0,0,0],[0,0,0,0,0,1,0,0,0,0],[0,0,0,0,0,0,1,0,0,0],[0,0,0,0,0,0,0,1,0,0],[0,0,0,0,0,0,0,0,1,0],[0,0,0,0,0,0,0,0,0,1]]) self.kf.H = np.array([[1,0,0,0,0,0,0,0,0,0], # measurement function,[0,1,0,0,0,0,0,0,0,0],[0,0,1,0,0,0,0,0,0,0],[0,0,0,1,0,0,0,0,0,0],[0,0,0,0,1,0,0,0,0,0],[0,0,0,0,0,1,0,0,0,0],[0,0,0,0,0,0,1,0,0,0]])# # with angular velocity# self.kf = KalmanFilter(dim_x=11, dim_z=7) # self.kf.F = np.array([[1,0,0,0,0,0,0,1,0,0,0], # state transition matrix# [0,1,0,0,0,0,0,0,1,0,0],# [0,0,1,0,0,0,0,0,0,1,0],# [0,0,0,1,0,0,0,0,0,0,1], # [0,0,0,0,1,0,0,0,0,0,0],# [0,0,0,0,0,1,0,0,0,0,0],# [0,0,0,0,0,0,1,0,0,0,0],# [0,0,0,0,0,0,0,1,0,0,0],# [0,0,0,0,0,0,0,0,1,0,0],# [0,0,0,0,0,0,0,0,0,1,0],# [0,0,0,0,0,0,0,0,0,0,1]]) # self.kf.H = np.array([[1,0,0,0,0,0,0,0,0,0,0], # measurement function,# [0,1,0,0,0,0,0,0,0,0,0],# [0,0,1,0,0,0,0,0,0,0,0],# [0,0,0,1,0,0,0,0,0,0,0],# [0,0,0,0,1,0,0,0,0,0,0],# [0,0,0,0,0,1,0,0,0,0,0],# [0,0,0,0,0,0,1,0,0,0,0]])# self.kf.R[0:,0:] *= 10. # measurement uncertainty# state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrixself.kf.P[7:, 7:] *= 1000. self.kf.P *= 10.# self.kf.Q[-1,-1] *= 0.01 # process uncertaintyself.kf.Q[7:, 7:] *= 0.01self.kf.x[:7] = bbox3D.reshape((7, 1))self.time_since_update = 0self.id = KalmanBoxTracker.countKalmanBoxTracker.count += 1self.history = []self.hits = 1 # number of total hits including the first detectionself.hit_streak = 1 # number of continuing hit considering the first detectionself.first_continuing_hit = 1self.still_first = Trueself.age = 0self.info = info # other info associateddef update(self, bbox3D, info): """ Updates the state vector with observed bbox."""self.time_since_update = 0self.history = []self.hits += 1self.hit_streak += 1 # number of continuing hitif self.still_first:self.first_continuing_hit += 1 # number of continuing hit in the fist time######################### orientation correctionif self.kf.x[3] >= np.pi: self.kf.x[3] -= np.pi * 2 # make the theta still in the rangeif self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi * 2new_theta = bbox3D[3]if new_theta >= np.pi: new_theta -= np.pi * 2 # make the theta still in the rangeif new_theta < -np.pi: new_theta += np.pi * 2bbox3D[3] = new_thetapredicted_theta = self.kf.x[3]if abs(new_theta - predicted_theta) > np.pi / 2.0 and abs(new_theta - predicted_theta) < np.pi * 3 / 2.0: # if the angle of two theta is not acute angleself.kf.x[3] += np.pi if self.kf.x[3] > np.pi: self.kf.x[3] -= np.pi * 2 # make the theta still in the rangeif self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi * 2# now the angle is acute: < 90 or > 270, convert the case of > 270 to < 90if abs(new_theta - self.kf.x[3]) >= np.pi * 3 / 2.0:if new_theta > 0: self.kf.x[3] += np.pi * 2else: self.kf.x[3] -= np.pi * 2######################### # flipself.kf.update(bbox3D)if self.kf.x[3] >= np.pi: self.kf.x[3] -= np.pi * 2 # make the theta still in the rageif self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi * 2self.info = infodef predict(self): """Advances the state vector and returns the predicted bounding box estimate."""self.kf.predict() if self.kf.x[3] >= np.pi: self.kf.x[3] -= np.pi * 2if self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi * 2self.age += 1if (self.time_since_update > 0):self.hit_streak = 0self.still_first = Falseself.time_since_update += 1self.history.append(self.kf.x)return self.history[-1]def get_state(self):"""Returns the current bounding box estimate."""return self.kf.x[:7].reshape((7, ))

---

C. 數據關聯

為了將檢測結果$D_t$與預測軌跡$T_{est}$進行匹配，我們將使用匈牙利算法。

維度為$(m_{t?1},n_t)$的矩陣將用來計算每對$D_t$和$T_{est}$的$3DIoU$或目標中心距離。然后，使用匈牙利算法來處理二分圖匹配問題。除此之外，當$3DIoU$小于$Io{U}_{min}$時或目標中心距離大于$dis{t}_{max}$時，將拒絕進行匹配。數據關聯模塊的輸出將分為4類：

$$\begin{gathered} T_{match}=\{T_{match}^1,T_{match}^2,..,T_{match}^{wt}\} \\ {D}_{match}=\{D_{match}^1,D_{match}^2,..,D_{match}^{wt}\} \\ {T}_{unmatch}=\{T_{unmatch}^1,T_{unmatch}^2,..,T_{unmatch}^{m_{t?1}?wt}\} \\ {D}_{unmatch}=\{D_{unmatch}^1,D_{unmatch}^2,..,D_{unmatch}^{nt?wt}\} \end{gathered}$$
這里$w_t$是匹配目標數量。

def associate_detections_to_trackers(detections, trackers, iou_threshold=0.01): """Assigns detections to tracked object (both represented as bounding boxes)detections: N x 8 x 3trackers: M x 8 x 3Returns 3 lists of matches, unmatched_detections and unmatched_trackers"""if (len(trackers)==0): return np.empty((0, 2), dtype=int), np.arange(len(detections)), np.empty((0, 8, 3), dtype=int) iou_matrix = np.zeros((len(detections), len(trackers)), dtype=np.float32)for d, det in enumerate(detections):for t, trk in enumerate(trackers):iou_matrix[d, t] = iou3d(det, trk)[0] # det: 8 x 3, trk: 8 x 3# matched_indices = linear_assignment(-iou_matrix) # hougarian algorithm, compatible to linear_assignment in sklearn.utilsrow_ind, col_ind = linear_sum_assignment(-iou_matrix) # hougarian algorithmmatched_indices = np.stack((row_ind, col_ind), axis=1)unmatched_detections = []for d, det in enumerate(detections):if (d not in matched_indices[:, 0]): unmatched_detections.append(d)unmatched_trackers = []for t, trk in enumerate(trackers):if (t not in matched_indices[:, 1]): unmatched_trackers.append(t)#filter out matched with low IOUmatches = []for m in matched_indices:if (iou_matrix[m[0], m[1]] < iou_threshold):unmatched_detections.append(m[0])unmatched_trackers.append(m[1])else: matches.append(m.reshape(1, 2))if (len(matches) == 0): matches = np.empty((0, 2),dtype=int)else: matches = np.concatenate(matches, axis=0)return matches, np.array(unmatched_detections), np.array(unmatched_trackers)

---

D. 3D卡爾曼濾波-狀態更新

由于$T_{match}$的不確定性，我們根據檢測結果$D_{match}$對$T_{match}$整個狀態空間的每個軌跡進行更新，最后獲得$t$幀時刻的關聯軌跡$T_t=\left\{T_t^1,T_t^2,?,T_t^{w_t}\right\}$。

根據貝葉斯規則，更新后的每一條軌跡$T_t^k=(x^{\prime },y^{\prime },z^{\prime },{\theta }^{\prime },l^{\prime },w^{\prime },h^{\prime },s^{\prime },v_x^{\prime },v_{y,}^{\prime }{v}_z^{\prime })$是狀態空間$T_{match}^k$和$D_{match}^k$的加權和，這的權重是由軌跡和檢測的不確定性所決定的。

除此之外，我們發現原始的加權和對確定目標方向效果一般。對于匹配目標$T_t^k$，其檢測結果上的方向和運動軌跡方向可能完全相反，相差$\pi$個相位。但事實上，目標都是平緩移動，不可能在一幀中改變移動方向。從結果上來看，$T_t^k$目標的加權軌跡方向應該位于$D_{match}^k$和$T_{match}^k$之間。這往往將會導致比較低的$3DIoU$值。為此，我們提出一個方向修正技術，當兩者相位相差超過$\pi /2$時，我們在$T_{match}^k$上添加$\pi$個相位，以便$T_{match}^k$和$D_{match}^k$方向能大致一致。

---

E. Birth and Death內存

因為現存的目標可能會消失以及新的目標可能會進入畫面，因此需要一個新的模塊來管理出現和消失的軌跡。一方面，我們認為所有不匹配的檢測可能會進入幀中，為了避免假陽性追蹤，當在$Bi{r}_{min}$中持續檢測到$D_{unmatch}^p$時，將會生成 一條新的軌跡$T_{new}^p$。一旦新的軌跡產生，我們將初始化$T_{new}^p$作為最新的測量$D_{unmatch}^p$，三個方向的速度初始化為0。

另一方面，我們認為所有不匹配的軌跡可能會離開，為了避免刪除真陽性的軌跡，我們在$Ag{e}_{max}$幀中持續追蹤不匹配軌跡$T_{unmatch}^q$直到從關聯軌跡刪除。

---

3. 測試結果

在KITTI上的追蹤結果如下，這里只給出了可視化結果，詳細結果可查閱論文。

總結

以上是生活随笔為你收集整理的动手学无人驾驶（3）：基于激光雷达3D多目标追踪的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。