CubeSLAM: Monocular 3-D Object SLAM

CMU,Facebook, Shichao Yang, 单双目，静/动态建图，SUN RGBD，KITTI，TUM，
注释版代码

总结

提出了完整的一套观测误差函数，跳出了传统SLAM只使用特征点的方式，整合进ORB-SLAM2的BA中进行优化，以及根据2D检测框预测3D目标框的方法，非常厉害。

论文摘要

本文贡献：

• 提出了基于单目摄像头的三维目标检测方法
• 多视图的、适用于静态和动态环境的、不需要任何先验知识的SLAM
• 提出具有新测量（novel measurement）功能的多视图 BA 约束，利用单视图的检测结果来联合优化相机位姿、目标和特征点；
• 证实了以上两种方法可以互相促进
• 公开数据集中不但得到了较好的位姿估计精度，还提高了 3D 目标的检测精度

论文简介

当前基于单目摄像头的工作受限于需要先验知识来确定尺度，因此将检测任务和SLAM任务分开来做，限制了其使用。本文着重于在静态和动态环境中同时解决两项任务。

本文提出了能够结合2D、3D目标检测和SLAM 位姿估计，在动静态环境都能运行的系统：

• 基于2D物体检测结果，在物体投影后与 2D 边界框紧密贴合的假设下，通过消失点（VP）采样来生成多个有效的立方体检测框；
• 利用多视图 BA 中的点和相机进一步优化所选的立方体的姿态；
• 目标在两个地方被用到：为难以三角化的点提供初始深度，以及在 BA 中提供几何约束；
• 同时， SLAM 中的相机位姿可以用于改善单视图的目标检测。

相关工作

论文同时涉及了单目、双目、检测、SLAM、静态、动态，所以related works都要说一遍。。。。

单目3D目标检测

A. 3-D Box Proposal Generation （区域提议生成）

论文根据二维目标检测的结果来生成三维目标框，这里有一个问题，就是三维检测框有9个维度----（x,y,z,Rx,Ry,Rz,Dx,Dy,Dz）,但是根据三维目标框的投影角应紧靠二维包围盒的假设，对应于二维检测框的四个边只有四个约束，因此不可能完全约束全部九个参数。

为了不使用先验知识，论文采用VP（vanishing point，是投影到透视图上后的平行线的交点）来减少未知参数。三维长方体有三个正交轴，投影后可以形成三个VP

3D 立方体有三个正交轴，根据旋转 R 和相机参数 K，通过透视投影生成三个VP

如果知道三个VP(VP1,VP2,VP3),那么其他几个角点就可以计算出来

从2D检测框4个角点获取3D目标框的8个角点的方法：

• 假设三个vp和上角点1已知或估计，并且×表示两条线的交点，那么2=（VP1，1）×（B，C），3=（VP1，4）×（VP2，2），5=（VP3，3）×（C，D）

• 消失点采样：如图 2 所示, a 为观察到三个面的情况，观察到7个点，并计算出 3 个消失点，假如对点 1 进行采样，可以通过 VP 线与矩形的射线相交确定角 2 和角 3，然后再确定角 4 及其他四个底角；b,c 两种情况也一样，先采样一个点，通过消失点计算出其他 7 个点

总结就是：首先估计物体三*度的旋转R和上边缘的一个角点，然后计算所有其他 2D 点，并进行反投影获得 3D 位置和尺寸。所以现在的问题是：如何获得物体的旋转和顶角？

• 有一种方式是利用深度网络通过大量的数据集直接训练预测得到，但本文为了充分利用 SLAM 优化后的几何约束，选择先进行详尽的采样，再进行评分选择最优的结果
• 对于平放在地面的物体，用于计算 VP 的旋转可以用物体的偏航角（yaw）和相机的俯仰角（pitch）和滚转角（roll）来表示，并且相机的两个角参数可以通过 SLAM 或其他传感器来获得，以减少采样空间；比如在 KITTL 数据集中，相机几乎与地面平行，具有固定的俯仰和滚转角。

本文只考虑后者，也就是地面上的物体

B.Proposal Scoring （区域提议打分）

可以使用不同的代价函数来为区域提议打分，文章使用的是：
I 是输入图像，O ={R,t,d}是目标框提议

1. DistanceError φdist(O,I): 2D 长方体边缘应与实际图像的边缘匹配**。利用 **Canny 边缘检测方法构建距离图，然后再求长方体边缘倒角距离（Chamfer distance进行累加求和，再通过 2D 框的大小进行归一化）
2. Angle Alignment Error φangle(O,I): 基于距离的error对于false positive 较为敏感，比如表面纹理。因此还需要检测线段并测量它们是否与立方体生成期间计算的消失点对齐。
3. Shape Error φshape(O): 前面两项可以在 2D 图像空间进行评估，但相似的长方体角可能会产生完全不同的 3D 立方体，提出一个代价函数来惩罚具有较大歪斜比（s=长度/宽度）的立方体，定义形状误差的代价函数为：
   
   s = max(dx/dy,dy/dx)， σ 为阈值

Object slam

将单图像三维目标检测扩展到多视角目标SLAM，实现了目标姿态和摄像机姿态的联合优化。

SLAM系统基于ORB-Slam2，前端相机追踪，后端BA优化。本文主要的改进是把目标 、特征点、相机位姿结合进BA。

A. BA Formulation

如果 将摄像机姿态空间和三维目标框和特征点分别表示为C={Ci}、O={Oj}、P={Pk}，则BA可以表示为一个非线性最小二乘优化问题：

e(c,o),e(c,p), 和 e(o,p）表示相机-目标，相机-特征点，目标-特征点之间的测量误差。然后根据高斯牛顿法或Levenberg–Marquardt法求解。（G2o,iSAM）

B. Measurement Errors

注意所有的测量误差都是用Huber loss来提升鲁棒性：

huber loss:

1. Camera-Object Measurement: 相机-目标误差由两部分组成：

   a) 3D 物体检测准确时（例如RGBD相机数据）：

   先将物体真实的位姿旋转至相机坐标系下，再与测量量比较，得到测量误差：
   注意：在没有先前的对象模型的情况下，我们基于图像的立方体检测不能区分对象的正面和背面。例如，可以通过将对象坐标系旋转 90 度并交换长宽来表示同一个长方体。因此，需要将目标物体沿高度方向旋转 90 ， 0 和 180 度以得到angle error中的最小误差

   b) 2-D Measurements: 将长方体地标投影到图像平面上，得到如图4（b）中红色矩形所示的二维包围盒，然后将其与蓝色检测到的二维包围盒进行比较
   
   
   c表示检测框中心坐标，s表示检测框的长宽。

   由于许多不同的三维目标框可以投影到同一个二维检测框上，因此需要更多的观测来完全约束摄像机的姿态和长方体。

2. Object-Point Measurement: 先将特征点P转换到目标框（9维）中然后计算error

取max操作是想鼓励所有的点到目标框内部，而不是仅仅停留在目标框表面。

4. Camera-Point Measurement:

   使用标准的3D重投影误差（ＯＲＢ－ｓｌａｍ）

C. Data Association

由于双关，对象重叠区域中的点不与任何对象相关联,因此需要从 SLAM 优化结果中检测和剔除动态目标，但是标准目标跟踪方法不能分类它是否是静态的，除非使用特定的运动分割。

本文使用了基于特征点匹配的数据关联方法：如果在至少两帧的二维对象边界框中观察到特征点，并且特征点到检测框中心的三维距离小于1 m，则首先将特征点与其对应的对象相关联。

最后，如果两个目标在不同的帧中拥有最多的共享特征点，并且数目超过了一定的阈值，则在不同的帧中匹配这两个目标

通过实验，这种方法对于宽基线匹配、重复对象和遮挡效果良好。属于运动目标的动态特征点将被丢弃，因为它们无法满足对极约束。因此，具有很少相关特征点的对象被视为动态对象。

IMPLEMENTATIONS

A. Object Detection

对于 2D 目标检测，在室内场景使用概率阈值为 0.25 的 YOLO 检测器 ，在室外 KITTI 数据集中使用概率阈值为 0.5 的 MS-CNN ，均可在 GPU 上实时运行；

1. 由于数据集中已提供相机的准确位姿（若没有相机位姿，可以通过 SLAM 位姿估计来得到相机的滚转和俯仰角），只需要采样目标的偏航角 yaw 便可计算消失点 VP
2. 在立方体可以旋转的情况下，生成 15 个偏航角范围为 90 度的物体
3. 然后在 2D 边界框的顶部采样 10 个点，注意并非所有的样本都可以生成有效的立方体方案，因为有些立方体的顶角可能在 2D 边界框之外

本方法的一个优点是不需要大量的训练数据，只需要调整公式的两个权重

B. Object SLAM

• 本文建立在 ORB-SLAM2 上，没有更改相机跟踪与关键帧筛选模块，在新创建的关键帧的基础上检测立方体物体，并使之关联，然后使用相机和特征点位姿进行 BA 调整
• 当基线或视差小于阈值时**，**立方体还用于初始化难以三角测量的特征点的深度
• 由于目标的数量远少于特征点，在实时运行时，目标关联和 BA 优化非常有效

EXPERIMENTS—SINGLE VIEW DETECTION

数据集： SUN RGBD 和 KITTI

使用了区域提议打分机制后，达到相同的recall,需要的提议大大减少，证实了本文 3D 原始提案的质量很高，因为他们可以保证与 2D 检测到的边界框相匹配

最终性能评测：

EXPERIMENTS—OBJECT SLAM

目标 SLAM 的评估指标：相机位姿估计和 BA 优化之后的 3D 目标 IoU
接下来在多个数据集上进行了测试

CONCLUSION

本文提出一种没有先验目标模型的单目三维目标检测与 SLAM 方法，并首次证明，语义目标检测与几何 SLAM 在一个统一框架中可以相互关联

• 单视图的三维物体检测**，**基于消失点从二维边界框中有效地生成高质量的立方体提案，然后通过图像信息有效地对提案进行评分
• 提出一种基于目标的 SLAM，在相机-目标-特征点之间产生测量，并提出新的目标关联方法，有效处理遮挡和动态情形；
• 目标可以为点提供深度约束，为相机提供尺度约束，反过来，SLAM 也为 3D 目标检测提供初始化。

实验结果：本文在不同的室内和室外数据集上评估了以上两个功能，在 SUN RGBD 数据集上实现了最准确的目标检测，在 KITTI 里程计数据集上达到了最佳的位姿估计。

下一步工作：考虑动态目标和基于目标的稠密地图，将更完整的场景理解与 SLAM 优化相结合。

引用

https://blog.****.net/wuyanmin1995/article/details/85202762