基于激光雷达3D多目标追踪
无人驾驶领域的目标检测技术主要分为二维（2D）和三维（3D）两类。其中，在二维场景中常用的一种主流解决方案是YOLO模型常被用作二维目标探测的主流解决方案。本文的研究则聚焦于无人驾驶场景下利用激光雷达进行三维（3D）多物体跟踪的技术方案，并采用了KITTI数据集作为实验基准。其中的目标检测算法基于POINTRCNN架构，并在此基础上设计了一种无需依赖GPU即可实现高处理效率的方法，在硬件资源受限的情况下仍实现了令人瞩目的处理效率（达到每秒214.7帧）。
1.概述
在自动驾驶或辅助机器人等实时应用中，3D多目标追踪（MOT）都是非常重要的组件技术。然而，在近期的一些3D多目标追踪研究当中更多的都是关注于追踪系统的准确率而忽略了其计算成本和复杂性。在本文中提出了一种简单而又准确的实时3D MOT基线系统。**本文使用了已有的3D目标检测器从LiDAR点云数据中获取对应目标3D bounding box。 然后，将3D卡尔曼滤波器和匈牙利算法结合起来用于状态估计和数据关联。**为了评估提出的基线系统，本文在原有的KITTI官方2D MOT评估指标上进行了扩展，提出了3D MOT评价指标，此外还提出了两个新的评价指标。本文主要贡献有：
- 开发了一种高效可靠的实时3D motivate基础系统；
- 将 official 2d mot指标进行了扩展，并成功开发了相应的三维mot评估标准。
- 开发了两项新的三维mot评估标准。
- 在 kitti 平台上实现了最优的三维mot性能，并在该平台上实现了处理速度最快的二维mot评估。
2.方法
3D MOT的目标是将序列中的3D 检测框关联起来。由于系统的实时性，需要当前帧中的检测结果以及上一帧中的关联轨迹 。系统示意图如下图所示，由5部分组成：
（1）3D检测模块用于从LiDAR点云中提取出检测框；
（2）3D卡尔曼滤波预测当前帧中的目标状态；
（3）数据关联模块用于对检测结果和预测轨迹进行匹配；
（4）3D卡尔曼滤波根据测量结果更新目标状态；
（5）birth和death内存控制着新出现的目标轨迹和消失的目标轨迹。
通常采用以下八项参数来表示三维检测框：目标中心坐标(x, y, z)、目标尺寸(l, w, h)、偏航角θ以及置信度s（confidence s）。值得注意的是，在不依赖于三维目标检测块的情况下（也就是说无需额外训练步骤），我们所使用的三维目标追踪系统能够直接应用现有模型进行运算
在此处进行说明的是，在KITTI 3D数据集上经过预先训练并引入到本研究中的三维目标检测模型（称为PointRCNN）作为基准系统。具体而言，在本节中我们将详细阐述一些基本术语的规定方式。在时间刻度t上所述的三维目标检测模型输出的结果将被记作Dt=D_{t1}, D_{t2}, D_{t3},..., D_{tn_t}（其中n_t表示当前帧中的目标数量）。每个三维目标将被一个八维元组表示为{x,y,z,l,w,h,θ,s}的形式
基于运动模型推导出下一时刻的目标状态T。假设目标遵循恒速运动模型，在此过程中未考虑同一帧内目标位置的变化。通过10维状态向量_T=(x,y,z,θ,l,w,h,vx,vy,vz)_来描述目标运动轨迹，其中额外的三个分量分别代表了物体沿x、y、z轴方向的速度变化。**
对于每一帧，在某一时间点上存在的所有属于该时间点的关联轨迹集合T_{i,t-1}（其中i表示第i个轨迹），它们会按照某种传播机制被传递至下一个时间点t的位置，并记为Test。在恒速模型下可得：预测位置坐标(x_{est}, y_{est}, z_{est})等于当前位置(x, y, z)加上速度增量(v_x, v_y, v_z)；预测姿态参数\theta, 长宽高l, w, h保持不变；速度分量v_x, v_y, v_z也会被传递至下一时刻t的位置估计中。由此可见，在这种情况下每个原始轨迹T_{i,t-1}会被传播至当前时间点的状态估计，并将被表示为Test_j = (x_{est}, y_{est}, z_{est}, \theta, l, w, h, v_x, v_y, v_z)的形式；这些预测信息将作为后续关联模块的重要输入依据。此外，在3D卡尔曼滤波算法中会用到这些预估值作为初始值进行迭代更新处理。
class KalmanBoxTracker(object):
"""
This class represents the internel state of individual tracked objects observed as bbox.
"""
count = 0
def __init__(self, bbox3D, info):
"""
Initialises a tracker using initial bounding box.
"""
#定义恒速模型
self.kf = KalmanFilter(dim_x=10, dim_z=7)
self.kf.F = np.array([[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,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], # 测量矩阵
[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]])
self.kf.P[7:,7:] *= 1000. #state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrix
self.kf.P *= 10.
# self.kf.Q[-1,-1] *= 0.01 # process uncertainty
self.kf.Q[7:,7:] *= 0.01
self.kf.x[:7] = bbox3D.reshape((7, 1))
self.time_since_update = 0
self.id = KalmanBoxTracker.count
KalmanBoxTracker.count += 1
self.history = []
self.hits = 1 # number of total hits including the first detection
self.hit_streak = 1 # number of continuing hit considering the first detection
self.first_continuing_hit = 1
self.still_first = True
self.age = 0
self.info = info # other info
def update(self, bbox3D, info):
"""
Updates the state vector with observed bbox.
"""
self.time_since_update = 0
self.history = []
self.hits += 1
self.hit_streak += 1 # number of continuing hit
if self.still_first:
self.first_continuing_hit += 1 # number of continuing hit in the fist time
######################### orientation correction
if self.kf.x[3] >= np.pi: self.kf.x[3] -= np.pi * 2 # make the theta still in the range
if self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi
new_theta = bbox3D[3]
if new_theta >= np.pi: new_theta -= np.pi * 2 # make the theta still in the range
if new_theta < -np.pi: new_theta += np.pi
bbox3D[3] = new_theta
predicted_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 angle
self.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 range
if self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi
# now the angle is acute: < 90 or > 270, convert the case of > 270 to < 90
if abs(new_theta - self.kf.x[3]) >= np.pi * 3 / 2.0:
if new_theta > 0: self.kf.x[3] += np.pi
else: self.kf.x[3] -= np.pi
#########################
self.kf.update(bbox3D)
if self.kf.x[3] >= np.pi: self.kf.x[3] -= np.pi * 2 # make the theta still in the range
if self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi
self.info = info
def 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
if self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi
self.age += 1
if(self.time_since_update>0):
self.hit_streak = 0
self.still_first = False
self.time_since_update += 1
self.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, ))为了实现检测结果Dt与预测轨迹Test之间的匹配关系, 应用匈牙利算法进行求解. 计算每对Dt和Test之间的三维交集体积（3DIoU）所需构建的矩阵维度为(nt×(mt−1), mt−1). 接着应用匈牙利算法处理相应的二分图匹配问题. 此外, 在3DIoU小于IoUmin的情况下, 则拒绝进行匹配操作. 数据关联模块输出结果分为两类:
一类是：检测集合D_{\text{match}} = D_{\text{match}_1}, D_{\text{match}_2}, \dots, D_{\text{match}_{wt}}及其对应的运动轨迹T_{\text{match}} = T_{\text{match}_1}, T_{\text{matching}_{12}}, \dots, \tilde{T}_{\text{matching}_{wt}}^{\wedge}
另一类是：未匹配检测集合D由多个元素组成：D = \{d_1, d_2, \dots, d_{m_t - w_t}\}；同时还有未被配对的运动轨迹T = \{t_1^{(1)}, t_2^{(1)}, \dots, t_{m_t - w_t}^{(1)}\}；其中w_t表示配对的目标数量
def associate_detections_to_trackers(detections,trackers,iou_threshold=0.1):
# def associate_detections_to_trackers(detections,trackers,iou_threshold=0.01): # ablation study
# def associate_detections_to_trackers(detections,trackers,iou_threshold=0.25):
"""
Assigns detections to tracked object (both represented as bounding boxes)
detections: N x 8 x 3
trackers: M x 8 x 3
Returns 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
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 IOU
matches = []
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)2.4 3D卡尔曼滤波-状态更新
基于Tmatcg的存在带来的不确定性影响，在对应测量值Dmatch的基础上对Tmatch的状态空间中所有轨迹进行更新操作，从而最终得到t帧时刻与之相关的关联轨迹集合Tt={Tt1,Tt2,⋯,Ttwt}。通过贝叶斯定理计算更新后的每一条轨迹Tit=(x′,y′,z′,θ′,l′,w′,h′,vx',vy',vz')作为状态空间中第i个匹配项与测量值Dmatchi结合后的加权综合结果。这个加权系数由对应轨迹及其检测结果的相关不确定性共同决定。（详见卡尔曼滤波论文）
此外进一步发现，在确定目标方向方面，默认加权方法表现平平。对于被匹配的目标i而言其检测结果的方向与运动轨迹可能存在反向现象这一现象可能源于以下假设各目标在运动过程中通常呈现平稳状态其在单帧时间段内难以突变其运动趋势从实验结果来看 Tti 目标的加权轨迹方向应位于 Dmatching 和 Tmatching 之间可能导致较低的三维交并率（3DIoU）数值为此提出了一个基于相位调整的方向修正技术当两者的相位差异超过 π/2 时 在 Tmatching 的时间段上增加一个完整的周期（即添加 2π 弧度）从而使得两者能够在空间上更加接近
2.5 出生死亡内存 由于现有目标可能消亡或新目标可能加入画面的原因, 我们需要一个新的模块来处理内存的增删变化。每当Fmin持续检测到Dunmatchi时, 我们将生成一个新的跟踪标记Tnewi, 并初始化其三个方向的速度参数设为零值。一旦生成新的跟踪标记Tnewi, 我们将立即启动其跟踪机制, 直至其从关联对象中被删除为止。另一方面, 我们认为所有不匹配的跟踪项可能会逐渐离开画面, 因此在Agemax帧中我们需要持续追踪这些不匹配项直至它们被正确删除为止。
3. 测试结果
在KITTI上的bounding box结果如下：
AB3DMOT
介绍
三维多目标跟踪（MOT）已成为实现自动驾驶或辅助机器人等实时应用的关键组成部分。然而，在当前的研究中对三维 MOT 的关注却逐渐转向了开发高精度系统而非效率问题。针对这一现状，《AB3DMOT》提出了一种简单而高效的实时三维 MOT 系统基准方法。我们基于现有的 3D 物体检测技术，并从 LiDAR 点云数据中提取定向边界框信息。通过将 3D 卡尔曼滤波器与匈牙利算法相结合的方式完成了状态估计与数据关联过程。尽管我们的基准方法采用了简单的整合方式，《AB3DMOT》依然实现了比现有同类技术更好的性能表现。为了验证我们的基准方法，《AB3DMOT》不仅提出了新的 三维 MOT 评估指标体系，在 KITTI 数据集上也实现了多项创新指标的突破性进展。具体而言，在 KITTI 数据集中，《AB3DMOT》所建立的方法首次实现了对三维 MOT 标准指标 MOTA 的显著提升——将现有技术中的 72.23% 提升至 76.47% 的新高水平。值得注意的是，在将三维跟踪结果投影至二维图像空间后进行测试时，《AB3DMOT》的表现依然令人瞩目——其在官方排行榜上的综合得分位居第二名的同时运行效率更是达到了惊人的 214.7 FPS ，较最新二维 MOT 方法快出 65 倍之多
注
3D目标检测：
为了方便起见，我们在“ ./data/KITTI/”的KITTI MOT数据集上提供了PointRCNN的3D检测，用于汽车，行人和骑自行车的人。 我们的检测结果遵循KITTI 3D对象检测挑战的格式（格式定义可在此处的对象开发工具包中找到：http://www.cvlibs.net/datasets/kitti/eval_object.php?obj_benchmark=3d），除了顺序已切换。 我们显示一个检测示例，如下所示：
为了在KITTI MOT验证集中测试我们的3D多目标追踪器，请部署所给的检测并按照指示进行操作。
$ python main.py car_3d_det_val
$ python main.py ped_3d_det_val
$ python main.py cyc_3d_det_val要在具有提供的检测功能的KITTI MOT测试仪上运行跟踪器：
$ python main.py car_3d_det_test
$ python main.py ped_3d_det_test
$ python main.py cyc_3d_det_test随后
3D MOT评估
为了实现基于推荐的KITTI-3DMOT平台验证我们系统的性能，请按照以下步骤操作：
$ python evaluation/evaluate_kitti3dmot.py car_3d_det_val
$ python evaluation/evaluate_kitti3dmot.py ped_3d_det_val
$ python evaluation/evaluate_kitti3dmot.py cyc_3d_det_val随后, 结果应基本一致, 而且除了FPS这一指标外（因为不同机器上表现可能有所差异）之外
评估
为了实现本文中展示的3D MOT系统的定性结果，请按照以下步骤操作：
通过适当选择阈值来处理轨迹。
在图像上绘制剩余的三维轨迹。（请注意：此步骤需依赖OpenCV 3库；如出现错误，请确认OpenCV版本。）
$ python trk_conf_threshold.py car_3d_det_test
$ python visualization.py car_3d_det_test_thres为了将可视化结果保存至指定路径"./results/car_3d_det_test_thres/trk_image_vis"。
请访问该链接http://www.cvlibs.net/datasets/kitti/eval_tracking.php以便下载KITTI MOT数据集，并按照指示将image_02移动至"./data/KITTI/resources"文件夹。
另外，在full_demo.mp4中您可以观看我们的演示视频以深入了解虚拟化效果。
2D MOT评估
请使用本文所展示的基于汽车类别的官方KITTI 2D MOT评估服务器来验证我们的3D MOT系统的定量效果；将以下文件夹进行压缩并上传至http://www.cvlibs.net/datasets/kitti/user_submit的指定位置（注：此处应有相应的说明）。
$ ./results/car_3d_det_test_thres/data然后，结果应类似于我们在KITTI 2D MOT排行榜上的输入：
