[论文阅读]DynaSLAM II: Tightly-Coupled Multi-Object Tracking and SLAM

1 速读

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

作者想解决在更具体场景下（自动驾驶、多机器人协调、AR），动态SLAM系统对环境中的动态物体的追踪问题。相当于结合了SLAM和多目标跟踪，是比较新的问题

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

文章涉及到SLAM+多目标追踪，文章的系统框架是基于上一代DynaSLAM做的改进，而DynaSLAM是基于ORBSLAM做的改进；

1.3 文章的贡献是什么？

提出了一种将场景结构，相机位姿，动态目标轨迹在一个局部窗口内tightly BA优化的方法，其中动态目标的Bounding Box也可以通过优化其位姿实现更新；
以此实现了一个基于Stereo/RGBD的动态SLAM系统；

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

将SLAM和多目标追踪结合，用实时多目标追踪解决SLAM中动态污染的问题，同时用SLAM的定位效果完成多目标追踪。

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

实验分两部分：动态物体的纳入对视觉里程计的影响+多目标跟踪效果；
Visual Odometry：评价指标选用ATE，RPEt，RPER
比较了基础模型ORBSLAM和上一代框架DynaSLAM；
比较了同样将动态物体和SLAM框架结合的最新系统：ClusterSLAM，ClusterVO，VDO-SLAM：
①效果比ClusterSLAM好；
②除了旋转方向上，其他结果比VDO-SLAM；
③和ClusterVO效果相当；
Multi-Object Tracking：动态物体的轨迹和3D bounding box来自雷达数据和手工标注，评价指标选用多目标轨迹误差metric MOTP+相机轨迹误差metric TP
对于目标检测object detections：
比较了四个基于目标检测的算法；
对于目标轨迹object trajectories：
选取了具有较长动态物体停留的11段轨迹，计算误差；
Timing Analysis：实时性分析
在动态物体较少的情况下（两个动态物体）达到12fps，动态物体较多的情况下（20个）能够保持~10fps。（没有包括CNN处理的时间，那感觉没什么比较性了）

1.6 数据集是什么？

视觉里程计实验中用到：KITTI中的tracking和raw数据；（是否太少了？）

1.7 还会存在什么问题

1.没有在自测的数据集实验，同时实验数据较少；
2.和上一代类似，其实有很多可以精简优化以加速的地方（匹配方式，object构建的方法）；
3.说是object的track，其实只是point的聚类，两个object之间是没有任何关系的，如：一辆车辆被遮挡后重新出现，会被认定为一个新object而不是对老object的回环；

2 主要内容

与ORBSLAM相同的蓝色关键帧轨迹+考虑优化动态部分的青色关键帧轨迹

2.1 文章框架

作者没有给出文章框架…但是从方法内容上看是在上一代DynaSLAM的基础上添加了红线框的部分：Data Association特征-目标的关联、Dynamic Features目标表示、BA优化

2.2 Objects Data Association

①对于一张图片，分别计算特征点和语义特征；
语义目标的创建：moveable语义标签+包含大量特征点；（初始位姿为描述其的3D点质心+单位旋转）
②用imoveable区域初始化位姿；
动态特征点关联/object匹配：
 a.object速度已知，假设匀速并做投影匹配；
 b.object速度位置，或a没有足够匹配，在连续帧进行暴力匹配找到具有最多match的mask区域；
④用IOU检测匹配

2.3 Object-Centric Representation

以点云描述object太expensive，作者对每个object新建一个对应的obj坐标系，obj上的点在这个坐标系上描述，因为点相对这个坐标系是长久的、相对静止的；后续只优化这个坐标系的位姿
加入object后的几何投影图如下：

2.4 Bundle Adjustment with Objects

关键帧的新建：
①track很弱（和ORBSLAM2中相同）；
②某个object的track很弱（一个具有大量特征点的obj在当前只track到一小部分）；

因为①插入关键帧：和ORBSLAM2相同；
因为②插入关键帧：在接下来两秒内将该object的位姿、速度、包含的特征点，同相机位姿一起进行local BA优化；
因为①+②插入关键帧：相机位姿、地图、目标位姿、目标速度、目标特征点一同构建BA优化；

①速度连续性误差（速度不能突变）：
$$e_{vcte}^{i,k}=\left(\begin{array}{r}{v}_{i+1}^k-v_i^k\\ w_{i+1}^k-w_i^k\end{array}\right)$$②匀速模型误差：
$$\begin{array}{rl}& e_{vcte,XYZ}^{i,j,k}=(T_{WO}^{k,i+1}-T_{WO}^{k,i}△T_{O_k}^{i,i+1}){\overline{x}}_O^{j,k}\\ & △T_{O_k}^{i,i+1}=\left(\begin{array}{rr}Exp(w_i^k△t_{i,i+1})& v_i^k△t_{i,i+1}\\ O_{1\times 3}& 1\end{array}\right)\end{array}$$即object上的特征点通过求解的位姿$T_{WO}^{k,i+1}$得到的世界坐标位置和通过恒速模型得到的$△T_{O_k}^{i,i+1}$计算的位姿$T_{WO}^{k,i}△T_{O_k}^{i,i+1}$得到的世界坐标位姿之间的误差；
静态点的重投影误差
$$e_{repr}^{i,l}=u_i^l-{\pi }_i(T_{CW}^i{\overline{x}}_W^l)$$④动态点的重投影误差
$$e_{repr}^{i,j,k}=u_i^j-{\pi }_i(T_{CW}^i{T}_{WO}^{k,i}{\overline{x}}_O^{i,k})$$表示object k上的点j转换到世界坐标系再转换到相机坐标系上和观测u的像素误差；

最终的误差，从左到右表示静态重投影误差(棕色)+①速度连续性误差（红色）+④动态重投影误差（青色）+②匀速模型误差（粉红色）：
$$\begin{gathered} \underset{\theta }{m}in\sum _{i\in \mathcal{C}}(\sum _{l\in {\mathcal{M}\mathcal{P}}_i}\rho (||e_{repr}^{i,l}|{|}_{{\sum }_i^l}^2)+\sum _{k\in {\mathcal{O}}_i}(\rho (||e_{vcte}^{i,k}|{|}_{{\sum }_{△}^2}^2) \\ +\sum _{j\in {\mathcal{O}\mathcal{P}}_k}(\rho (||e_{repr}^{i,j,k}|{|}_{{\sum }_i^j}^2)+\rho (||e_{vcte,XYZ}^{i,j,k}|{|}_{{\sum }_{{△}_t}}^2)))) \end{gathered}$$待优化参数列表，从左到右表示相机位姿+object位姿+静态3d点坐标+动态object点坐标+速度+角速度：
$$\theta =\{T_{CW}^i,T_{WO}^{k,i},X_W^j,X_O^{j,k},v_i^k,w_i^k\}$$
BA图和海森矩阵：

上图表示：5个关键帧+1个object+10个object point+10个静态point的海森矩阵，需要想一想的是：
①Objects-KFs：虽然只有一个object，但是优化是在一个local windows中进行的，在这个local windows中每个时刻都会对应一个object_t，被关键帧观测的object_t被标记为粉红色，所以呈现这样的分布；
②Objects-Objects：相邻时刻的object之间会有相关性，所以呈现这种斜对角线的柱状分布；

2.5 Bounding Boxes

用一个长方体描述object，从作者文字描述看不懂是怎么得到这个box，应该跟别的工作类似，以后再看

问题

1、关键帧的新建规则会不会导致一直创建关键帧？