前言
之前了RGBD相机的动态物体追踪的工作 MaskFusion：
文章的尺度主要还是限制在桌面上，拿起一些物体测试跟踪效果。离我所理解的物体级的SLAM还有一些差距。近期开始调研物体级的SLAM，这篇文章还不错: CubeSLAM: Monocular 3D Object SLAM， 它发表在 2019年 最近一期的 IEEE Transactions on Robotics上（2018年挂在arXiv上）。CubeSLAM用单目相机实现了物体级的建图、定位、和动态物体跟踪。
而这位华人作者Shichao Yang在CMU Robotics Institute实验室的导师指导下做工作。列一下他的其他工作：
-
Pop-up SLAM：一个很优秀的室内面slam work， IROS2016
-
Monocular Object and Plane SLAM in Structured Environments: 将CubeSLAM和 Pop-up-SLAM结合起来出的一篇 ，有时间再做它的分析。
我觉得他的探索思路是非常合理的：传统基于特征点的SLAM基本已没啥可做的了（有做的赶紧告诉我），点之上的高维表达包括线、面、物体，这些先验暂时无法做到通用，但在室内的结构环境下有不少可以尝试的点。必然需要解决这些物体的观测模型、地图中的表达、在BA中的融合数学模型等等。
对于语义SLAM来说，我们的前端有检测或分割图像的输入，而地图中物体的表达方式是什么呢？ICRA2019一篇：Structure Aware SLAM using Quadrics and Planes 探讨了用双曲面表达物体的方式。室内先验下 面、物体 如何在地图中表达，并以此形成一套观测、融合的体系，是至少目前还在研究的东西。
我们先回过头看CubeSLAM，CubeSLAM里则提出用立方体来建模物体，这是我们最直观的物体理解方式。它提出了整套观测误差函数，并放入ORBSLAM2的BA过程中统一优化。这里面提出几点很有价值的想法是:
-
如何从单目利用2D bounding box， 辅助Vanish Points(消影点) 来恢复三维立方体结构
-
如何在传统SLAM的静态假设上，将动态物体（车辆等）也追踪起来。实现相机位姿估计和运动物体位姿估计。
论文在室内SUN RGBD数据集和室外KITTI数据集做了完善的实验。
我将论文核心理论部分整理如下，欢迎一起讨论，也欢迎私信我指出错误。
代码开源情况
目前在github开放源码：https://github.com/shichaoy/cube_slam
亲测可用。然而开放的是纯物体级的SLAM代码，并没有如论文中整合ORB features的版本。作者给了室内数据集一个纯立方体的demo，跑起来很带感.
理论核心部分
III. Single Image 3D Object Detection
A. 3D box proposal generation
1) Principles
3Dbox用9个自由度描述，除了6维刚体变换外，还增加了3个dimension: 即长宽高.
由于一个前端矩形检测的四个端点只能提供4个约束，因而需要其他的信息。如：提供或预测的物体大小，以及朝向。(在许多车辆检测算法中用到)
我们使用VP来改变和减少回归参数，而不去预测依赖的维度。VP是平行线的相交点。一个3D cuboid有三个正交轴，因此可以形成3个VPs，而且只与物体旋转有关!
2) Get 2D corners from the VP
由于最多能看到3个立体面，我们考虑三种可能性。
图中只要已知VP和其中一个端点，其他端点都能被算出来。图：比如当已知VP，图a)中给定1点，则2、4点可以通过线相交确定。
3) Get 3D box pose from 2D corners
得到了2D平面内的立方体顶点后，我们要估计其三维位姿。将物体分成两类。
任意位姿物体: 使用PnP solver来求解通常的立体3D位姿和大小，由于单目尺度不确定性，我们需要确定一个尺度因子。数学上来说，立体的八个3D顶点在物体坐标系下是可描述的： ， 变换到相机参考系是：
。我们可以选择4个相连的顶点如1,2,4,7，然后从上述3D交点投影到2D中去，如顶点1有:
因此每个点可以贡献两个约束，4个点即可估计9DOF的pose，除了scale.
地面物体: 地面物体的roll/pitch都是0. 可以大大简化上述计算。我们不再需要用复杂的PnP Solver去求解，而是可以直接反向投影地面的顶点到三维地平面上，然后计算其他垂直的顶点。比如上述在三维地平面上的顶点5，可以表达为 [n,m] ( 相机帧里的向量和距离 ), 相对应的三维顶点 是反向投影的射线
与地平面的交点:
该公式应该是空间直线与平面交点的表示公式，在 MVG 书里或许有。
更详细地投影过程在论文27中描述。尺度由相机高度决定。
4) Sample VP and Summary
由于VP通过物体旋转矩阵R决定。通过深度网络，可以直接预测VP。我们手动sample它们，并且根据评分排序它们。( 这部分到底是如何生成的？意思是否是通过神经网络预测物体的旋转矩阵R来预测VP？）
这里有一点提醒：在本文的实验中，作者仅仅考虑了那些位于地面的物体.! 包括KITTI中的车辆也满足这个假设.
其实仔细一想，正常情况哪个物体不在地面上呢?
B. Proposal scoring
示意获取最佳的三维立方体假设。
在图中，左图先提取了一些直线，右图分别是根据不同的直线生成的立体矩形假设，左上角是最佳的假设。每个不同假设可以得到不同的代价函数值。由下式计算：
其中 ， R是旋转，t是平移，d是大小。下面分别介绍三种误差函数：
1) Distance error :
2D立体边界应该与真实图像的边界匹配。通过Canny边缘检测，并生成 DT变换，对于每个可见的立体边，均匀sample 10个点，并综合计算所有距离值，然后除以2d矩形的对角线长度。
2) Angle alignment error :
距离误差与false positive的边很敏感，比如物体表面的纹理。因此，我们也检测长线条段（用LSD检测），然后测量它们的角度是否跟VP点匹配。这些直线首先与3个VP点之一关联，基于点-线支撑关系。对于每个VP，可以找到最远的两条线段，它们有最小和最大的倾角。
这里的含义应该是这两条线的交点应该是消影点VP. 那么关键在于为何点-线支撑关系能找到这样的两条最远线段呢. 参考文献给的[11]
3) Shape error
前两个的误差可以在2D图像平面内计算。但是，相似的2D立体端点可能生成完全不一样的3D立方体。我们添加一个误差来惩罚有巨大倾斜( 长宽比 ) 的比率。也可以添加更严格的先验，比如某些特殊种类物体的固定长宽值或比例。
IV. Object SLAM
我们延伸单图像三维物体检测到多物体SLAM，来联合优化物体位姿和相机位姿。系统基于ORBSLAM2搭建，包含前端相机追踪和后端BA。
我们主要的挑战是完善BA来联合物体、点和相机位姿，本部分将详细介绍。其他SLAM应用细节可以见VI-B， 静态物体在本部分介绍，动态物体将在下一节强调。
A. Bundle Adjustment Formulation
先放出完整的BA公式：
分别代表相机-物体，相机-点，物体-点的约束。
B. Measurement Errors
1) 相机-物体测量
物体和相机之间的测量误差有两种:
a ) 3D测量: 第一种是在3D物体检测是准确的时候使用的3D测量，比如使用RGBD相机时。检测的物体位姿，作为物体在当前相机帧的测量。为了计算测量误差，我们转换路标物体到相机帧，然后比较测量误差：
其中 log 将 SE3的误差映射到 6DOF的切向量空间，因而 。为了改善鲁棒性，我们使用 Huber 鲁棒核函数。
注意这里在没有先验时有歧义，无法分清前向和后向。因而在优化时要沿着高度方向旋转0 +-90 180度来获得最小误差。
b) 2D测量: 对于2d测量，我们将立方体路标投影到图像平面来获得2D的bbox，然后与检测的bbox之间比较。简单来说，就是将8个顶点投影到平面内，寻找最小和最大的投影像素x,y坐标来建立一个矩形:
c和s分别是矩形框的中心和边长。定义两个矩形框之间的误差为:
即直接让4D参数相减。这个测量比起上述的3D测量不确定性更小，因为一般2D检测更加准确。但不同的三维实体也可能有相同的2D投影。
2）物体-点测量
若点P属于物体，则应该在3D物体内部。我们首先将点转换到立体坐标系，然后与立方体的大小比较来获得三维误差：
使用max是鼓励点在立方体内部而非正好在表面。
3） 相机-点测量
我们在基于特征的SLAM中使用标准3D点重投影误差。不多赘述。
C. Data association
使用帧间的点跟踪来跟踪检测框。考虑哪些检测框内离中心在10像素以内的点。哪些匹配点最多，则关联谁。同时，对于匹配上的点过少的框( 使用对极几何搜索)，它们可以认为是动态物体而去除掉！！
V. DYNAMIC SLAM
下面将同时估计相机位姿和动态物体的轨迹。基于几个基本假设：
-
刚体假设: 即物体上的点不会运动。
-
运动假设: 如速度模型; 对于车辆还有非完整轮式模型(nonholonomic wheel model)，即限制侧面滑动.
动态物体跟踪是一个大的话题。在这里简单提到，其核心仍然是追踪动态的点，只是将点聚集一组，彼此之间增加速度约束。相机位姿也可以被动态元素约束。
1) 物体运动模型
使用 nonholonomic wheel model.
2) Dynamic point observation
动态点与关联的物体绑定，因而首先被变换到世界系，然后投影到图像平面。
C. Dynamic data association
由于匹配动态物体很麻烦，所以静态物体的关联方式不适用。典型方法是预测其投影位置，然后在附近搜索。但是对于单目动态场景，准确预测物体运动很难，对极几何也不准。特征点直接用2D KLT 稀疏光流算法来跟踪，不需要3D点位置。像素跟踪后，动态特征的3D位置通过考虑物体运动完成三角化。
这是标准的两视图三角化问题.
KLT跟踪在像素位移太大时容易失败。因此我们直接使用视觉物体跟踪算法[40]，上一帧的检测框将被跟踪，并在这一帧预测位置。
VI. IMPLEMENTATIONS
A. Object detetion
使用 YOLO detector在室内，概率阈值0.25；使用MS-CNN在室外KITTI数据集，概率阈值0.5.它们在GPU上实时。
关于误差函数中各部分权重的调节，使用手动调节。因为物体检测的协方差不好测定。这里也是一个futrue work.
详细的实验不赘述，大家可以自行看论文。
一些有价值参考文献
知乎编辑器解析不了Markdown表格，我截图在此.
最后
目前来看，物体级的SLAM两个方向比较明显，一个是CubeSLAM为代表的用立方体描述；另一条线是 QuadricSLAM等代表的用椭球体(特殊双曲面)描述。目前来看后者的阵容更强大，还有Towards semantic SLAM等文章在做，而且基于多视图几何那本书里的理论，有扎实的几何基础。
立方体描述在理论上并没有自成一派，研究者目前似乎仅有CMU RI的这位华人作者，当然他本身也很强，前面还有pop-up slam做面的支撑。
两条线有主要区别在于，立方体的slam是基于单目的，而椭球体基于RGBD相机，且作者在最后表示我们的future work是探索一下只用单目(哈哈哈)。毕竟单目就实现物体级slam谁不爱呢。
总之，语义slam中，作为很重要的语义元素的物体，在地图中的几何表达到底是立方体还是双曲面呢，或许解是二者之一，也可能二者都不是最好的解。只能让时间给我们答案了。
往期文章一览
2、【OpenCV教程】core 模块-Mat-基本图像容器
4、CVPR 2019 程序主席发文泼冷水：计算机视觉的黄金时代靠的都是记忆
