摘要
大多数现有的SLAM方法都假设环境是静态的或基本上是静态的。动态区域的特征常被当作异常值来处理,而不用于相机姿态估计,但这一假设在许多实际环境中可能不成立。在这项工作中,在动态情况下不再将运动的物体视为异常点,而是基于动态点观测和运动模型约束,共同优化相机和运动物体的轨迹----->将2D、3D物体检测和SLAM姿态估计融合在一起以适用于静态和动态环境。
本文提出了一种用于静态和动态环境下的单目图像3D长方体物体检测和多视点对象SLAM,并证明了这两部分可以相互改进。具体步骤为:
- 首先,对于单目图像物体检测,我们从2D边界框和消失点采样中生成高质量的长方体。
- 然后,基于与图像边缘的对齐,进一步对这个生成的3D长方体进行评分和选择。
- 其次,提出了一种多视点BA,以实现对相机、目标和点的姿态优化。对象可以提供远程几何(long-range geometric)和尺度约束,以提高相机姿态估计和减少单目漂移。
没有将动态区域视为异常值,而是利用目标表示和运动模型约束来改进相机姿态估计。在SUN RGBD和KITTI上进行的三维检测实验表明,与现有方法相比,该方法具有更好的准确性和鲁棒性。在公共空间、KITTI和我们自己收集的数据集上,我们的SLAM方法实现了最先进的单目相机姿态估计,同时提高了三维物体检测的精度。
贡献如下:
- 提出了一种高效、准确、鲁棒的单目图像3D长方体检测方法。
- object SLAM方法具有相机、目标和点之间的测量,在许多数据集上实现了更好的姿态估计。
- 结果表明,物体检测与SLAM是相适应的。
- 提出了一种利用运动物体改进动态场景姿态估计的方法。
Ⅰ. 单图像3D目标检测
A. 3D box proposal generation
1) Principles:
本文利用2D边界框有效地生成3D长方体proposals,而不是在三维空间中随机采样目标proposals。
一般的3D长方体可以用9个自由度参数表示:3自由度的位置 t = [ t x , t y , t z ] t = [t_x, t_y, t_z] t=[tx,ty,tz]、3自由度的旋转 R R R、以及三自由度的长宽高dimensions: d = [ d x , d y , d z ] d = [d_x, d_y, d_z] d=[dx,dy,dz]。长方体坐标系建在长方体中心,与主轴对齐。相机内参矩阵K也是已知的。由于一个前端矩形检测的四个端点只能提供4个约束,因而需要其他的信息。如:提供或预测的物体大小,以及朝向。(在许多车辆检测算法中用到)
不依赖于预测的dimensions,我们利用消失点VP来改变和减少回归参数,以适应一般对象。
VP是平行线在透视图像上投影后的交点。3D长方体有三个正交的轴线,根据物体相对于相机坐标系下的旋转R和标定内参矩阵K,进行投影后可以形成三个VP:
其中, R c o l ( i ) R_{col(i)} Rcol(i)是第 i i i个R的列。这部分的解析可以看我的另一篇博客——利用消失点进行相机标定
2) Get 2D corners from the VP
如何基于VP得到8个2D长方体顶点?由于最多可以同时观察到三个长方体面,我们可以根据图2所示的可观察面的数量将长方体结构分为三种常见的类别。每个结构都可以是左右对称的。
图2:2D目标框生成长方体proposals。如果估计了三个消失点和一个顶点,其他七个顶点也可以通过解析计算得到。如(a)中给定顶点1,则顶点2、顶点4可以通过直线交点确定,其他顶点也一样。
假设已知或估计了三个VP和顶点 p 1 p_1 p1, x表示两条直线的交点,则 p 2 = ( V P 1 , p 1 ) ‾ × ( B , C ) ‾ p_2 = \overline{(VP_1, p_1)} × \overline{(B, C)} p2=(VP1,p1)×(B,C)…以此类推…
3) Get 3D box pose from 2D corners:
得到了2D平面内的长方体顶点后,我们要估计其三维位姿。我们将物体分为两种情况。
- 任意位姿物体:由于单目尺度的不确定性,使用PnP来求解一般长方体的3D位置和尺寸。
在数学上,长方体在物体坐标系下的的八个3D顶点为 [ ± d x , ± d y , ± d z ] / 2 [±d_x,±d_y,±d_z] /2 [±dx,±dy,±dz]/2,转换到相机坐标系为: R [ ± d x , ± d y , ± d z ] / 2 + t R[±d_x,±d_y,±d_z] /2+t R[±dx,±dy,±dz]/2+t。如图2(a)所示,我们可以选择四个相邻的顶点,例如1、2、4、7,然后从上述3D顶点投影到2D中去,如顶点1有:
其中, π π π是相机映射函数, p i p_i pi是八个2D物体顶点之一。每个顶点提供了两个约束,这样四个顶点可以完全约束对象的姿态(9自由度),除了尺度。找到迭代或非迭代PnP求解器进行求解。
- 地面物体:在地平面上建立世界坐标系,然后物体的滚/俯仰角度为0。与上一节类似,我们可以从VP中得到8个2D角。然后,不利用公式(2)中复杂的PnP求解器,直接将地面的顶点像素反投影到三维地平面,然后再计算其他垂直角,形成3D长方体。例如,上述在三维地平面上的顶点5可以表示成[n, m](相机帧里的法线和距离), 相对应的三维顶点
P
5
P_5
P5是反向投影的射线
K
−
1
p
5
K^{-1}p_5
K−1p5与地平面的交点:
该公式应该是空间直线与平面交点的表示公式。比例是由投影过程中的相机高度决定的。
4) Sample VP and Summary
从前两节的分析来看,边界估计的问题变成了如何得到三个消失点VP和一个2D顶点。因为在得到VP之后,我们可以用Ⅰ- A2来计算2D顶点,然后用Ⅰ- A3来计算3D边界框。由公式(1)可知,VP由物体旋转矩阵R确定,为了便于推广,我们选择手动对它们进行采样,然后对它们进行评分(排序)。
在本文的实验中,作者仅仅考虑了那些位于地面的物体
B. Proposal scoring
通过上述获得很多长方体候选后,设置损失函数对其评分。
图3.对长方体方案评分。左图先提取了一些直线,用于给方案打分。右图分别是根据不同的直线生成的立体矩形假设(2D青色边框)。左上方是最好的,右下方是最差的。每个不同假设可以得到不同的代价函数值。
把图像表示为
I
I
I,依照
I
−
A
(
1
)
I-A (1)
I−A(1)长方体方案表示为
O
=
{
R
,
t
,
d
}
O = \{R, t, d\}
O={R,t,d},则损失函数可以定义为:
其中,
φ
d
i
s
t
,
φ
a
n
g
l
e
,
φ
s
h
a
p
e
φ_{dist}, φ_{angle}, φ_{shape}
φdist,φangle,φshape是三种不同类型的损失函数;
w
1
,
w
2
w_1,w_2
w1,w2作为损失函数的权重,在小样本数据集上手动搜索后分别设置为0.8,1.5。
1)距离误差 φ d i s t ( O , I ) φ_{dist}(O, I) φdist(O,I)
2D长方体边应与实际图像边匹配。首先检测Canny边缘并基于其建立距离变换映射。然后,对于每条可见的长方体边(图2(a)中的实心蓝线),我们均匀地在其上采样10个点,并综合计算所有与对应的Canny边缘的距离值,然后除以2d矩形的对角线长度。
2)角度对准误差 φ a n g l e ( O , I ) φ_{angle}(O, I) φangle(O,I)
检测长线段(用LSD检测)(如图3中的绿线所示),测量其角度是否与消失点对齐。这些线首先与基于点-线支持关系的三个VPs之一关联。然后对于每个
V
P
i
VP_i
VPi,我们可以找到斜率最小和最大的两条最外侧线段,分别表示为
<
l
i
_
m
s
,
l
i
_
m
t
>
<l_{i\_ms},l_{i\_mt}>
<li_ms,li_mt>和
<
l
i
_
n
s
,
l
i
_
n
t
>
<l_{i\_ns},l_{i\_nt}>
<li_ns,li_nt>。
<
a
,
b
>
<a,b>
<a,b>表示端点为(a, b)的直线的倾斜角。最后角度对准误差为:
3) 形状误差 φ s h a p e ( O ) φ_{shape}(O) φshape(O)
相似的2D长方体顶点会产生不同的3D长方体。我们增加了一个成本来惩罚长方体与一个大的倾斜(长/宽)比率。
其中, s = m a x ( d x / d y , d y / d x ) s = max(d_x/d_y, d_y/d_x) s=max(dx/dy,dy/dx)是物体的倾斜比率; σ σ σ是阈值且在实验中设置为1。如果s < σ σ σ,则不惩罚。
Ⅱ. object SLAM
我们将单目图像的三维物体检测扩展到多视图物体级别的SLAM,共同优化目标姿态和相机姿态。系统构建在基于特征点的ORB SLAM2,其中包括相机跟踪前端和BA后端。这篇论文完善了BA来联合物体、点和相机位姿。本节讨论静态物体,下一节讨论动态物体。
A. Bundle Adjustment Formulation
将一组相机姿态,3D长方体和点分别表示为
C
=
{
C
i
}
,
O
=
{
O
j
}
,
P
=
{
P
k
}
C = \{C_i\}, O = \{O_j\}, P = \{P_k\}
C={Ci},O={Oj},P={Pk},BA可以表述为一个非线性最小二乘优化问题:
其中
e
(
c
,
o
)
、
e
(
c
,
p
)
、
e
(
o
,
p
)
e(c, o)、e(c, p)、e(o, p)
e(c,o)、e(c,p)、e(o,p)分别表示相机-物体、相机-点、物体-点的测量误差。
Σ
Σ
Σ是不同的误差测量的协方差矩阵。然后用g2o等库中的Gauss-newton或LM算法求解优化问题。
Notations:
- T c ∈ S E ( 3 ) T_c ∈ SE(3) Tc∈SE(3):相机位姿
- P ∈ R 3 P ∈ R^3 P∈R3: 点
- O = { T o , d } O = \{T_o, d\} O={To,d}:长方体对象建模为9自由度的参数。其中, T o = [ R ∣ t ] ∈ S E ( 3 ) T_o = [R|t] ∈ SE(3) To=[R∣t]∈SE(3)是6自由度的相机位姿, d ∈ R 3 d∈R^3 d∈R3是长方体维数。
- 下标m表示测量值。坐标系如图4(b)所示。
图4:(a)我们的object SLAM的流程。单视图目标检测为SLAM提供了长方体路标和深度初始化,SLAM则可以估计相机姿态,从而实现更精确的目标检测。(b)显示BA期间相机、物体和点之间的坐标关系和测量误差。
B. Measurement Errors
1)相机-物体
测量误差有两种。
(a)3D测量
第一种是在3D物体检测是准确的时候使用的3D测量,比如使用RGBD相机时。Section I-A中单幅图像检测到的物体位姿
O
m
=
(
T
o
m
,
d
m
)
O_m = (T_{om}, d_m)
Om=(Tom,dm),作为来自相机帧的物体测量。为了计算其测量误差,将路标物体变换到相机坐标系中,然后与测量结果进行比较:
将Huber鲁棒成本函数应用于所有测量误差,以提高鲁棒性。
需要注意的是,在没有先验时有歧义,无法区分物体的正面或背面。例如,我们可以通过旋转物体坐标系90◦和交换长度和宽度值来表示同一个长方体。因此,我们需要沿着高度方向旋转0,±90,180,在Eq. 8中找到最小的误差。
(b)2D测量
在二维测量中,我们将长方体路标点投影到图像平面上,得到如图4(b)中红色矩形所示的二维边界框,然后与检测到的蓝色的二维边界框进行比较。更详细地说,我们将8个顶点投影到图像上,找到投影像素的最大和最小x、 y坐标,形成一个矩形:
其中,
[
u
,
v
]
m
i
n
,
m
a
x
[u, v]_{min,max}
[u,v]min,max为八个投影顶点的最小/最大xy坐标,即投影矩形的左上角和右下角。c和s是2D方框的中心和大小。它们都是二维向量,因此
[
c
,
s
]
∈
R
4
[c, s]∈R^4
[c,s]∈R4。直接让4D参数相减,将4D矩形误差定义为:
与公式(8)中的3D误差相比,这种测量误差的不确定性要小得多,因为2D物体检测通常比3D检测更精确。这类似于将地图点投影到图像上以表示重投影误差。但是由于许多不同的三维长方体可以投射到同一个二维矩形上,因此需要更多的观测来充分约束相机的姿态和长方体,因此在投影后会丢失信息。
2)物体-点
如果点P属于图4(b)中所示的物体,它应该位于3D长方体的内部。首先将点变换到长方体坐标系,然后与长方体维度(长宽高)进行比较,得到三维误差:
使用max是鼓励点在立方体内部而非正好在表面。
3) 相机-点
在基于特征的SLAM中,我们使用了标准的三维点重投影误差:
其中, z m z_m zm为3D点 P P P观测到的像素坐标。
C. Data association
提出了一种基于特征点匹配的目标关联方法。对于许多基于点的SLAM方法,通过描述子匹配和对极几何搜索,可以有效地匹配不同视图中的特征点。
- 如果在至少两帧的2D物体边界框中观察到的点,且其与长方体中心的3D距离小于1m,则首先将特征点与对应的物体关联起来。例如在图5中,特征点与其关联物体的颜色相同。注意,在计算公式(11)中BA期间的物体-点测量误差时,也使用了这种物体-点关联。
- 匹配不同帧中的两个物体,如果它们之间共享特征点的数量最多,并且数量超过了一定的阈值(在我们的实现中是10个像素)。
哪些匹配点最多,则关联谁。
同时,运动物体的动态特征点由于不能满足外极约束而被丢弃。对于匹配上的点过少的框( 使用对极几何搜索),它们可以认为是动态物体而去除掉!!比如图5中的前青色汽车。
图5:KITTI 07动态和遮挡场景下的物体关联。绿色点为非物体点,其他颜色的点与相同颜色的物体相关联。前青色移动的汽车不作为SLAM路标点,因为没有特征点是与它相关。物体重叠区域内的点由于模糊性不与任何物体相关联。
Ⅲ. DYNAMIC SLAM
上一节处理静态物体SLAM。在本节中,我们提出一种联合估计相机姿态和动态物体轨迹的方法。
首先对物体做一些假设,以减少未知的数量,使问题可解:
- 物体是刚性的,并且遵循某种物理上可行的运动模型。刚体假设表明,点在其相关物体上的位置不随时间变化。这允许我们利用标准的3D地图点重投影误差来优化它的位置。
- 运动假设: 匀速恒运动模型; 对于车辆还有非完整轮式模型(nonholonomic wheel model),即限制侧面滑动.
A. Notations
在上一节A中,除了静态SLAM之外,还定义了一些新的地图元素。对于动态物体 O i O^i Oi,我们需要估计其在每一观察帧 j j j中的姿态 j O i ^jO^i jOi。我们使用动态点来表示与运动物体相关的特征点。对于运动物体 O i O_i Oi上的动态点 P k P_k Pk,将其固定在物体上的位置表示为 i P k ^iP^k iPk(根据刚性假设其是固定的)。它的世界位姿将随时间而改变,不适合SLAM优化。
图6:动态对象SLAM。蓝色节点表示静态SLAM部分,红色节点表示新的动态变量。绿色方块是动态地图的新要素,包括运动模型约束和物体与点的观测。
B. SLAM optimization
动态物体估计的因素图如图6所示。蓝色节点代表静态SLAM部分,红色节点代表动态物体、点和运动速度。绿色方块是测量因子,包括Eq. 10中的相机-物体因子,Eq. 14中的物体-速度因子,以及Eq. 15中的点-相机-物体因子。有了这些因素,相机的姿态也可以受到动态元素的约束。
1) Object motion model:
采用非完整车轮模型(non-holonomic wheel model),汽车运动用线性速度
v
v
v和转向角
φ
φ
φ表示。假设飞行器在局部平面上近似运动,则物体的roll/pitch=0,z轴平移量
t
z
=
0
t_z =0
tz=0。只需要
t
x
,
t
y
,
θ
t_x, t_y,θ
tx,ty,θ(heading yaw)来表示其完整状态
T
o
=
[
R
(
θ
)
∣
[
t
x
,
t
y
,
0
]
′
]
T_o = [R(θ)| [t_x, t_y, 0]']
To=[R(θ)∣[tx,ty,0]′]。由速度预测的物体运动状态为:
其中
L
L
L为前后轮中心之间的距离。注意,这个模型要求
x
,
y
,
θ
x, y, θ
x,y,θ 在后轮中心被定义,而物体坐标系在车辆中心被定义。两个坐标系有需要补偿的
L
/
2
L/2
L/2偏移量。最终运动模型误差为:
2) Dynamic point observation:
如前所述,动态点被固定在与它相关联的物体上,因此它首先被转换为世界坐标系,然后投影到相机上。假设第
k
k
k个点在第
i
i
i个物体上的局部位置为
i
P
k
^iP^k
iPk,第
j
j
j个图像中物体的位姿为
j
T
o
i
^jT^i_o
jToi,则该点的重投影误差为:
其中 T c j T^j_c Tcj为第 j j j个相机位姿, z k j z_{kj} zkj为该点观测像素。
C. Dynamic data association
点和物体关联方法:特征点由二维KLT稀疏光流算法直接跟踪,不需要三维点位置。像素跟踪后,考虑到物体的运动,将动态特征点的3D位置三角化。数学上,假设两个坐标系的投影矩阵为
M
1
,
M
2
M_1, M_2
M1,M2,在两帧的3D点位置分别为
P
1
、
P
2
P_1、P_2
P1、P2,对应的像素观测值分别为
z
1
、
z
2
z_1、z_2
z1、z2。两帧之间的物体运动变换矩阵为
∆
T
∆T
∆T,则可以推断
P
2
=
∆
T
P
1
P_2 = ∆T P_1
P2=∆TP1。根据投影规则,可以得到:
如果我们把
M
2
∆
T
M_2∆T
M2∆T看作一个补偿了物体运动的修正相机位姿,上面的方程就是标准的两视图三角化问题,可以用奇异值分解SVD来求解。
当像素位移较大时,KLT跟踪仍然可能失败。因此,对于动态物体跟踪,我们没有使用第Ⅱ-C节中的共享特征点匹配方法。相反,我们直接使用视觉目标跟踪算法,上一帧的检测框将被跟踪,并在这一帧预测位置。