[论文笔记] Fusion++: VolumetricObject-LevelSLAM

1. introduction

本文结合了MASK RCNN，说是语义SLAM，其实基本没用到语义信息，而是结合了物体检测结果，对每个物体进行三维重建，建立物体级的地图。并完善了基于这个地图的初始化、删除、更新、跟踪、重定位、图优化的工作。

存在的不足还是挺多的，默认场景是静态的，并且不追踪动态物体。MASK RCNN的阈值很高，这样漏检应该是比较严重的。MASK RCNN并非实时，30帧运行一次。

基于Kinect fusion，使用MASK RCNN分割每张图像上的实例级物体，建立每个物体的TSDF，利用其对每个物体进行重建、赋予3D前景mask，并且每个重建的物体都含有其语义信息和所有其他预测。

重建的物体以6自由度的位姿图的形式存储，每一帧以物体为单位，对其进行优化，并用于跟踪、重定位和闭环检测。在检测到闭环后，物体的位姿会被调整，但每个物体内部的结构不变，物体的语义信息也每一帧优化更新。

主要贡献有：

创建了一个物体朝向的SLAM系统，构造含有不同分辨率的3D实例级物体的地图。
每一帧的2D物体检测都使用体素前景(voxel foreground)融合，并记录其被检测到的次数比例。
实现了全局一致、闭环的地图，包含高质量的物体重建。

2. Method

系统流程是：

输入RGBD图像，初始化背景TSDF，用于local tracking和occlusion handling。
若位姿改变过大或跟踪丢失，进行重定位和图优化，并重置TSDF。
独立的检测线程使用MASK RCNN检测物体，检测结果经滤波与已存的物体级地图匹配。
若地图中无匹配上的物体，新建TSDF物体实例并加入地图。
关联的前景检测与物体的3D前景mask融合，并关联其语义信息和存在概率图。

2.1. TSDF Object Instances

地图是以重建的物体为单位组成的，每个物体由TSDF进行重建，记为vov_ovo。每个vov_ovo有一个相应的位姿属性TwoT_{wo}Two，表示这个物体出现的一帧到世界坐标系所在参考帧的转换。每一个重建的物体实例都由一个正方体volume进行表示，相应的，这个volume包含一个中心点位置（坐标系为?），以三维点坐标表示，和一个size，以边长表示。

总结一下，这部分讲了地图的基本构成单位：instance。

它的表示方法：TSDF重建。
如何初始化和更新其位置和大小。
如何更新其形状和前景标记。
如何渲染。
如何记录更新其检测概率和语义概率。
如何判断其是否应该被新建或删除。

2.1.1. Initialisation and resizing

这步的目的是新建一个TSDF重建模型，初始化其参数。

当在当前帧检测到当前地图中不存在的物体时，要重建一个新的TSDF插入地图。重建一个新的TSDF，首先要确定它的size和position。

重建时，根据检测到物体的mask，将其投影到三维世界坐标系中。具体做法是对于每个mask中的坐标点，找到其对应深度图中的深度，将归一化坐标scale，再经内参矩阵和外参矩阵转换，得到世界坐标系下的三维坐标。这里，外参是当前相机位姿的估计。文中说，同时会将部分背景也重建，因此猜想，这里的mask是mask rcnn直接得到的bounding box，而非分割轮廓？

确定position

找到10%和90%的坐标点，计算其均值为中心坐标。

确定size

确定体积为so=m∣∣p90−p10∣∣∞s_o=m||p_{90}-p_{10}||_∞so=m∣∣p90−p10∣∣∞，根据固定的分辨率ro=64r_o=64ro=64，计算每一个体素的大小vo=sorov_o=\frac {s_o}{r_o}vo=roso。

因此，分辨率固定，体积越小，体素越小，建模越精细。

更新TSDF

同时，在运行过程中，可能会检测到一个物体的其他区域（之前没看到的），就需要把之前的建模补全，重新计算大小和中心点。其中心点是通过以体素大小为单位平移和改变，大小改变时，保持体素大小不变，增加其分辨率。同时，限制了最大的分辨率和size。

确定是否新建一个新的模型

若计算到的中心点在相机5m内，且与其他已存在于地图中的模型的3DIOU<0.5，则新建。

2.1.2. Integration

这步的目的是在每一帧根据深度图和检测结果，维护地图中的TSDF重建模型和其参数。

维护分为两个方面：模型的voxel和对应的语义信息。

surface integration

这个是用来维护三维重建模型的形状。根据每一帧的深度图，模型vov_ovo的每个体素vvv包含的属性有：其location，归一截断段距离（normalised truncated signed distance value）Sk−1oS_{k-1}^{o}Sk−1o，相应权重Wk−1oW_{k-1}^{o}Wk−1o。

对于每一帧，若这个TSDF volume可见，并且其百分之五十的像素被追踪，且ICP RMSE<0.03，则对于这个volume，判断它的每个体素是否更新。是否更新volume，与它是否在当前帧被检测到是无关的。

若这个体素vvv投影到当前帧像素位置的深度值，小于对应深度图中的测量深度值加阶段距离（不懂这俩个深度值的区别，一个是根据三维坐标计算的深度，一个是当前帧传感器测量的RGFD图中的深度？），则这个测量深度，根据权重www去更新这个volume。

instance mask intergration

这一步是维护三维模型各体素是否是前景的计数器。MASK RCNN的检测结果是一个二项值：是或不是前景。每个体素有一个对应的计数器：Fk−1o(v)F_{k-1}^{o}(v)Fk−1o(v)、Nk−1o(v)N_{k-1}^{o}(v)Nk−1o(v)，根据MASK在这个像素点的值MkiM_{k}^{i}Mki，进行更新。

Fko(v)=Fk−1o(v)+Mki(Kπ(cP(v)))F_{k}^{o}(v)=F_{k-1}^{o}(v)+M_{k}^{i}(K \pi (cP(v)))Fko(v)=Fk−1o(v)+Mki(Kπ(cP(v)))

Nko(v)=Nk−1o(v)+(1−Mki(Kπ(cP(v)))N_{k}^{o}(v)=N_{k-1}^{o}(v)+(1-M_{k}^{i}(K \pi (cP(v)))Nko(v)=Nk−1o(v)+(1−Mki(Kπ(cP(v)))

其中，pipipi是将相机坐标系下的三维坐标归一化的函数，经过K转换成像素坐标。那么，P(v)P(v)P(v)是体素在世界坐标系下的三维坐标？ccc难道是外参？

然后再计算前景统计的比例，根据这个比例判断这个体素是不是前景。

2.1.3. Raycasting

某种光线追踪方法，渲染了深度、法线、顶点、TGB、目标索引。主要是用来判断是不是前景，避免碰撞什么的。

2.1.4. Existence probability

这一步，维护了一个instance被检测到的次数计数器。对每一帧，若一个instance被清晰的观测到，则记录它是否被检测到。然后被检测到的次数所占的比例，如果过小，则从地图中删除这个instance。

2.1.5. Semantic Labels

这一步，维护了TSDF的语义类别概率。文章中认为semantic fusion的更新方式存在问题，而选择使用直接取平均。

2.2. Detection and Data Association

这一步是讲检测结果的数据关联，具体是在上面都说过。

使用MASK RCNN得到前景mask、bounding box和类别概率。MASK RCNN的检测结果经过滤波，只留下最合适的100个。

这100个检测结果和地图中的TSDF匹配，找mask相交最大的匹配，去更新它的mask和类别概率。

2.3. Layered Local Tracking

这部分讲了一下tracking的方法，和kinect fusion的差不多，没有细看。

有几点值得注意：

地图中同时建了一个背景的TSDF，用于地图中无instance或出现碰撞（occlusion）的情况。
利用上一帧的位姿，将单独instance结合背景TSDF，render到当前坐标系下，去得到一个外层的参考帧，用于之后的tracking。
参考帧和当前帧构造point-to-plane error，使用高斯牛顿法优化，优化目标是当前帧的位姿。
同时对每一个实例进行位姿图优化，使用ICP RMSE评测，用于instance integration和检查是否跟踪失败。

2.4. Relocalisation

如果跟踪丢失或者重置了TSDF，则进行重定位，把当前帧与地图中的instance对其。

文章认为，只对重建的volume使用ICP效果不好（应该是这些volume与有深度的特征点ICP？），因此，选择使用snapshots of sparse BRISK features对当前图像进行检测，再根据深度图投影到3D点。

文章使用了snapshots of sparse BRISK features的方法。我的意会是，对地图中的物体，每隔15°提一次snapshot，用于和当前帧中检测的结果的对应深度图来匹配。匹配时使用3D-3D RANSAC，对每一个instance进行匹配，如果有物体匹配上了，就对所有点使用3D-3D RANSAC进行匹配，来得到最终重定位的位姿。

这个snapshot不太懂，究竟是什么的snapshot和什么做RANSAC？？

2.5. Object level Pose Graph

和slam++的方法一样。

建图

节点：

都以SE(3)的形式表示位姿变换，具体是针对一个固定的世界参考帧www的旋转和平移。

Tw,oT_{w,o}Tw,o：世界到物体jjj的位姿转换
Tw,cT_{w,c}Tw,c：世界到时间戳为iii的相机的位姿转换

边：

也是以SE(3)的形式表示的位姿变换。

To,cT_{o,c}To,c：为第iii个时间戳的相机到物体jjj的位姿转换
Tck,ck−1T_{c_{k},c_{k-1}}Tck,ck−1：为相邻两帧之间相机位姿的转换。

优化

边之间的constraint

To,cT_{o,c}To,c：为对于物体每个像素的ICP误差
Tck,ck−1T_{c_{k},c_{k-1}}Tck,ck−1：为对于为止instance的背景的ICP误差

然后可以写出误差项

优化过程中，要求协方差矩阵，用李代数求导的方式，定义扰动项，求雅可比，最后写出信息矩阵H。

最后得到总的误差项，就是所有边的误差和，包括cam-obj和cam-cam。然后对其进行优化。

优化后得到新的instance的位姿，和相机的位姿。在新建其他新的instance之前，对这些优化的位姿变量更新。

此外还提及了，如果一个instance发生resize，其中心点坐标也会移动，这时，其拥有一个移动前后的变换TO,O′T_{O,O'}TO,O′，对于预期相连的点，cam-obj和world-obj，相应的位姿都要被这个TO,O′T_{O,O'}TO,O′更新。