CubeSLAM:单目3D目标SLAM算法,已开源!

作者:原野寻踪 https://zhuanlan.zhihu.com/p/69683253 本文已授权,未经允许,不得二次转载

前言

上一篇文章调研了RGBD相机的动态物体追踪的工作 MaskFusion:

https://zhuanlan.zhihu.com/p/62059382

文章的尺度主要还是限制在桌面上,拿起一些物体测试跟踪效果。离我所理解的物体级的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中,作为很重要的语义元素的物体,在地图中的几何表达到底是立方体还是双曲面呢,或许解是二者之一,也可能二者都不是最好的解。只能让时间给我们答案了。

接下来有时间我会再写一下双曲面的解析。

原文发布于微信公众号 - CVer(CVerNews)

原文发表时间:2019-06-23

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

发表于

我来说两句

0 条评论
登录 后参与评论

扫码关注云+社区

领取腾讯云代金券