Stereo Vision-based Semantic 3D Object and Ego-motion Tracking for Autonomous Driving 论文阅读

1 速读

1.1 论文试图解决什么问题？这是否是一个新的问题？

动态自动驾驶场景下的3D语义目标追踪

1.2 有哪些相关研究？如何归类？谁是这一课题在领域内值得关注的研究员？

以后在动态SLAM汇总处整理；

1.3 文章的贡献是什么？

①轻量的3D box推断方法，结合2D box和视点筛选方法viewpoint classification；
②结合语义和特征信息的BA方法；

1.4 文章解决方案的关键是什么

引入一个viewpoint classification方法直接从2D框得到粗糙初始位姿，然后在后端中优化完成动态追踪；

1.5 实验如何设计？实验结果足够论证其效果吗？

①展示了几个具体场景下的实验结果；
②和ORBSLAM2比较ego motion效果：误差选用RPE和ATE的RMSE;
静态场景和ORBSLAM2差不多，动态场景效果很好；
③和3DOP比较multi object tracking效果；
列出了算法在不同阈值设定下True Positive和平均定位误差两个维度上的比较；
列出了和3DOP在鸟瞰图以及3Dbox的Average precision比较；

1.6 数据集是什么？

KITTI和Cityscapes
1.7 还会存在什么问题
1.大型车辆的数据没有建模；
2.object的重识别（作者在conclution中也提到，提高目标的时间相关性质）
3.相机位姿首先是使用静态部分计算的并没有结合动态部分，会有问题

2 主要内容

上图：相机的3D轨迹和历史object轨迹，可以看出来
①object只在画面内时存在感知行为；
②车辆侧面的动态轨迹都很短，大概是因为与侧边车辆不断超越与被超越导致object一直被新建；
下图：从左到右为stereo特征匹配；画面边缘的车辆（忽然闯入或即将被超越）可以完成跟踪；通过object BA恢复的对象稀疏特征；动态运动跟踪；
可以看出：
①大客车没有被跟踪；
②最后一张图边缘的银色车辆也没有被跟踪，对边缘车辆感知的效果需要check

2.1 系统框架

三个模块：
①2D目标检测+viewpoint classification，其中viewpoint classification的功能是对原网络加了一层FC完成的根据2D框边界和3D box顶点粗略估计物体位姿；
②特征提取和匹配模块，将3D框投影至画面得到mask，进行ORB特征匹配和基于mask的object匹配；
③紧耦合优化方法融合语义信息和特征信息，结合运动学模型得到object运动估计；

2.2 Viewpoint Classification and 3D Box Inference

基于2D bounding box做3D bounding box及其位姿的初始计算（因为2D的检测相对直接3D会更准确）
网络会输出对车辆水平方向（8个，下图中左边）和垂直方向（2个，下图中右边）的viewpoints，两个方向共组成16种情况，表示相机对车辆的大致观测角度；

假定（个人推测）：
①检测的2D box是刚好包住3D bounding box在画面上的投影区域的，这时顶点会在2D box上;
②车辆的长宽高固定；
③车辆只有水平旋转；

此时会有四个等式表示bounding box上顶点的投影关系：
$$\{\begin{array}{r}{u}_{min}=\pi (p+R_{\theta }{C}_{1}d{)}_u,u_{max}=\pi (p+R_{\theta }{C}_{2}d{)}_u,\\ v_{min}=\pi (p+R_{\theta }{C}_{3}d{)}_v,v_{max}=\pi (p+R_{\theta }{C}_{4}d{)}_v,\end{array}$$由此可以解出用四自由度表示的位姿（空间位置3维+水平旋转1维，不考虑车另外两个方向的旋转，妙啊，可是如何解决路面变化导致的车辆另两个方向的旋转呢？）

2.3 Feature Extraction and Matching

区域生成：
①将3D bounding box到stereo图像中生成mask区域；
②存在重叠部分根据深度判断哪个是invisible；
③不够恢复3D bounding box（突然闯入画面中或即将被超越的车辆看不到所需的4个顶点），直接将左目中提取的投影到右目；

匹配：
①左右目提取ORB特征点；
②帧间object匹配使用2D框similarity score voting（中心距离+形状相似性，但是我实在理解不了怎么用2D框做动态下的object匹配的）；
③背景+匹配object间的特征匹配；

2.4 Ego-motion and Object Tracking

某一时刻下的观测关系如下

语义观测：$s_k^t=\{b_{kl}^t,b_{kr}^t,l_k,C_{k1:4}^t\}$（2Dbox的两个顶点+object标签+计算顶点的矩阵）；
$\mathcal{S}=\{s_k{\}}_{k=1:K_t},s_k=\{s_k^t{\}}_{t=0:T}$ 第k个物体在时刻t的观测；

特征点观测：${}^n{z}_k^t={\{}^n{z}_{kl}^t,{}^n{z}_{kr}^t\}$（第k个物体的第n个特征点在时刻t的左右目观测）；
$\mathcal{Z}=\{z_k{\}}_{k=0:K_t},z_k={\{}^n{z}_k{\}}_{n=0:N_k}{,}^n{z}_k={\{}^n{z}_k^t{\}}_{t=0:T}$ 第k个物体的第n个点在时刻t的观测；

相机位姿：${}^w{x}_c^t={\{}^w{p}_c^t,{}^w{R}_c^t\}$；
相机运动：${}^w{\mathcal{X}}_c={\{}^w{x}_c^t{\}}_{t=0:T}$; 相机在时刻t的位姿；

object位姿：${}^w{x}_{ok}^t={\{}^w{p}_{ok}^t,d_k{,}^w{\theta }_{ok}^t,v_{ok}^t,{\delta }_{ok}^t\}$（第k个物体在时刻t，位置+时不变维数d+水平角度θ+速度v+车轮转向角度δ）；不太理解这个d的意义；
object运动：${}^w{\mathcal{X}}_o={\{}^w{x}_{ok}{\}}_{k=1:K_t}{,}^w{x}_{ok}={\{}^w{x}_{ok}^t{\}}_{t=0:T}$; 第k个物体在时刻t的位姿；

动态点位置：$\mathcal{F}={\{}^{k}f{\}}_{k=0:Kt},{}^{k}f={\{}^k{f}_n{\}}_{n=0:N_k}$，表示第k个object的第n个动态点；

任务：给定语义观测和特征点观测，估计相机和objec运动；
a.Ego-motion Tracking
用静态部分计算相机位姿，此时是没有考虑动态的，会有问题；
b.Semantic Object Tracking
最后误差分为：特征投影误差+2D box对3D bounding box构成的约束误差+object运动误差+维度先验误差（三维尺度误差）；
①特征投影误差（匹配的object间相对于计算得到的相机位姿的误差），表示object上点投影到左右目的误差：
$$r_{\mathcal{Z}}{(}^n{z}_k^t,{}^w{x}_c^t,{}^w{x}_{ok}^t,{}^k{f}_n)=\left[\begin{array}{c}\pi (h^{-1}{(}^w{x}_c^t,h{(}^w{x}_{ok}^t,{}^k{f}_n)))-{}^n{z}_{kl}^t\\ \pi (h{(}^r{x}_l,h^{-1}{(}^w{x}_c^t,h{(}^w{x}_{ok}^t,{}^k{f}_n))))-{}^n{z}_{kr}^t)\end{array}\right]$$②2D box观测误差：假定仍然符合网络给出的viewpoint结果，并且3D bounding box的顶点投影仍然在2D box上；即3D bounding box的位姿应该尽量符合给出的2D box：
$$r_{\mathcal{S}}(s_k^t{,}^w{x}_c^t{,}^w{x}_{ok}^t,d_k)=\left[\begin{array}{c}\pi (h_{c_1}{)}_u-(b_{kl}^t{)}_u\\ \pi (h_{c_2}{)}_u-(b_{kr}^t{)}_u\\ \pi (h_{c_3}{)}_v-(b_{kl}^t{)}_v\\ \pi (h_{c_4}{)}_u-(b_{kr}^t{)}_v\end{array}\right]$$$$h_{c_i}=h^{-1}{(}^w{x}_c^t,h{(}^w{x}_{ok}^t,C_i{d}_k^l))$$③车辆运动学模型在匀速假设下的误差:
运动学模型如下：
$${}^w{\hat{x}}_{ok}^t=\left[\begin{array}{c}{}^w{p}_{ok}^t\\ {}^w{\theta }_{ok}^t\\ {\delta }_{ok}^t\\ v_{ok}^t\end{array}\right]=\left[\begin{array}{cccc}{I}_{3\times 3},& o,& o,& \Lambda \\ o,& 1,& 0,& \frac{tan(\delta )\Delta t}{L}\\ o,& 0,& 1,& 0\\ 0,& 0,& 0,& 1\end{array}\right]\left[\begin{array}{c}{}^w{p}_{ok}^{t-1}\\ {}^w{\theta }_{ok}^{t-1}\\ {\delta }_{ok}^{t-1}\\ v_{ok}^{t-1}\end{array}\right],\Lambda =\left[\begin{array}{c}cos(\theta )\Delta t\\ sin(\theta )\Delta t\\ 0\end{array}\right]$$根据运动学模型可以根据车辆上一帧的位置推断下一帧的位置，误差为：
$$r_{\mathcal{M}}{(}^w{x}_{ok}^t,{}^w{x}_{pk}^{t-1})={}^w{x}_{ok}^t-{}^w{\hat{x}}_{ok}^t$$④维度先验误差：好像是每个物体的三维尺寸误差
$$r_{\mathcal{P}}(d_k^l,d_k)$$经过上述误差后，可以得到符合最大后验估计的object位姿，此时将3Dbox和恢复的点云对齐，即使得3D点云到3Dbox整体距离最小：
$${}^w{x}_{ok}^t=\underset{{}^w{x}_{ok}^t}{\mathrm{argmin}}\sum _{n=0}^{N_k}d{(}^w{x}_{ok}^t,{}^k{f}_n)$$

3 实验

3.1 实验效果

上半部分划蓝色圈的车辆跟踪了超过200米，轨迹为右上角的紫色曲线；
下半部分为高动态场景，左边为轨迹和速度图，雷达数据只是为了可视化；

其他场景，可以看到是有针对行人的检测的，但是并没有追踪行人

3.2 ego-motion

和ORBSLAM比较，左边是RPEt和RPEr，右边是raw数据下的ATE

3.3 Object Localization Evaluation

将数据分为三个难度，比较不同IoU阈值下，平均position error和True positive两个指标纸币之间的关系，所有观测结果（TP=1）的平均误差大小分别为5.9%, 6.1% 和6.3%；

和3DOP比较，鸟瞰图和3D box的Average precision比较，做了消融实验，只选了两种阈值；
在3D+0.5阈值时略差于3DOP；

问题

1.运动学模型（kinematics model）是怎么用进来的？
  在优化阶段有一个根据运动学模型推导的匀速误差
2.viewpoint classification直接分成16类合理吗？
  合理，因为这里只想要从2D box快速得到3D box及其位姿，这里位姿是粗糙的会在后面优化；