这一篇论文的主要贡献有两个,一个是在orb-slam的基础上加入了线特征,在不影响效率的情况下,提升了系统在低纹理的情况下的表现;二是提出了一个只用到线匹配就估计出初始地图的方法(连续三帧)。这篇论文是基于单目的,还有一个同名的PL-SLAM是双目的SLAM系统,不要弄混了~
I. INTRODUCTION
首先指出了为什么要使用线特征:
对于ORB-SLAM,如果在低纹理场景,或者是图像有运动模糊时,系统就会失效,但是在这些场景中往往还有线条,所以就希望能够应用线特征。
同时,论文也说明了使用的线特征的难点在哪里:
1)要找到合适的线条检测子和参数化方式
2)从线匹配去计算位姿的可靠性是低于用点的,而且这些线特征对遮挡非常敏感
3)使用端点还可以对orb点特征的代码进行部分复用
最终,选择了用端点来参数化,原因是:这种方式对于遮挡和无检测的情况更加鲁棒。
III. SYSTEM OVERVIEW
PL-SLAM的pipeline如下图所示:
跟orb-slam一样,追踪线程用来估计相机的位姿和决定是否要加入新关键帧;局部建图增加新关键帧信息到地图并用BA优化;闭环线程持续的进行闭环检测并修正。
在PL-SLAM中,和线特征相关的处理包括了:
- 检测:使用LSD的方法,时间复杂度是 O ( n ) O(n) O(n),n为图片中像素的个数
- 三角化
- 匹配:使用了以Line Band Descriptors为基础的方法,通过一个关系图(relational graph)当前的线会和已经在地图中的线进行配对。在获得了初始的map-to-image的线特征对集合后,所有在局部地图中的线都被投影到图像上,进一步寻找匹配对,然后,如果这个图片有足够多的新环境信息,他就会被标记为关键帧,它对应的线会被三角化并添加进地图。
- 线的剔除:从少于三个视点或少于 25% 的帧中看到的线会被丢弃(剔除)。
- 优化:线在地图中的位置使用局部BA进行优化。
- 重定位: 注意,因为对整个地图进行线的匹配的计算量非常大,所以回环检测中只使用点特征。
IV. LINE-BASED SLAM
这一节重点介绍线的参数化方式以及误差函数,还会解释怎么把它用到特征匹配,BA和全局重定位中。
A.基于线的重投影误差
根据参考文献[30], P,Q是一条线段的两个端点,
p
d
,
q
d
∈
R
2
\textbf{p}_d, \textbf{q}_d \in \mathbb R^2
pd,qd∈R2是这两端点在图像平面的2D检测结果,
p
d
h
,
q
d
h
∈
R
3
\textbf{p}_d^h, \textbf{q}_d^h \in \mathbb R^3
pdh,qdh∈R3是对应的齐次坐标。从齐次坐标,可以得到一个归一化的线系数:
I
=
p
d
h
×
q
d
h
∣
p
d
h
×
q
d
h
∣
(
1
)
I= \frac{\textbf{p}_d^h \times \textbf{q}_d^h}{\vert \textbf{p}_d^h \times \textbf{q}_d^h \vert} \space \space \space (1)
I=∣pdh×qdh∣pdh×qdh (1)
有了这个系数,下面再来看线的重投影误差,它是点到线距离之和,这里点是指的两端点投影后的结果,线是指的在图像中检测到的线段。具体可以看下面这张图:对于左图:
P,Q是三维空间中某一条线段的端点(绿色点),而在图像中也有对应的两个绿点,这个就是由真实的P,Q投影过来的。而在图像中,本身就存在两个拍摄下来的端点
p
d
,
q
d
∈
R
2
p_d, q_d \in \mathbb R^2
pd,qd∈R2(对应于P,Q的观测值),观测值对应三维空间的端点
P
d
,
Q
d
∈
R
3
P_d, Q_d \in \mathbb R^3
Pd,Qd∈R3.
I
I
I是检测到的线系数,而
I
~
\widetilde{I}
I
则是指的投影的线系数。
对于右图:
d
1
,
d
2
d_1, d_2
d1,d2就是我们要的线重投影误差,而
d
1
′
,
d
2
′
d_1^{'}, d_2^{'}
d1′,d2′是检测到的线重投影误差(检测到的2D线段(蓝色)和对应的投影的3D线段(绿色)之间的误差)。
线重投影误差的公式就为:
E
l
i
n
e
(
P
,
Q
,
I
,
θ
,
K
)
=
E
p
l
2
(
P
,
I
,
θ
,
K
)
+
E
p
l
2
(
Q
,
I
,
θ
,
K
)
(
2
)
E_{line}(P,Q,I,θ,K)=E^2_{pl}(P,I,θ,K)+E^2_{pl}(Q,I,θ,K) \space \space \space(2)
Eline(P,Q,I,θ,K)=Epl2(P,I,θ,K)+Epl2(Q,I,θ,K) (2)
E
p
l
(
P
,
I
,
θ
,
K
)
=
I
T
π
(
P
,
θ
,
K
)
(
3
)
E_{pl}(P,I,θ,K)=I^T \pi(P,θ,K) \space \space \space(3)
Epl(P,I,θ,K)=ITπ(P,θ,K) (3)
I
I
I是检测到的线系数,
π
(
P
,
θ
,
K
)
\pi(P,θ,K)
π(P,θ,K)是端点P在图像上的投影,K是内参矩阵,相机参数
θ
=
{
R
,
t
}
\theta = \{R,t\}
θ={R,t}
注意!在实际中,由于线的遮挡和误检测,图像中检测到的线段端点
p
d
,
q
d
p_d, q_d
pd,qd往往不能和
P
,
Q
P,Q
P,Q匹配,所以这里才又定义了个检测到的线投影误差。
检测到的线重投影误差的公式就为:
E
l
i
n
e
,
d
(
p
d
,
p
d
,
I
)
=
E
p
l
,
d
2
(
p
d
,
I
)
+
E
p
l
,
d
2
(
q
d
,
I
)
(
4
)
E_{line,d}(p_d, p_d,I)=E^2_{pl,d}(p_d,I)+E^2_{pl,d}(q_d, I) \space \space \space(4)
Eline,d(pd,pd,I)=Epl,d2(pd,I)+Epl,d2(qd,I) (4)
E
p
l
,
d
(
P
d
,
I
)
=
I
T
p
d
E_{pl,d}(P_d,I)=I^T p_d
Epl,d(Pd,I)=ITpd
对于“检测到的线重投影误差”会有递归操作,一边优化相机位姿,一边把
E
l
i
n
e
,
d
E_{line,d}
Eline,d近似到
E
l
i
n
e
E_{line}
Eline
B.使用点和线的BA
相机位姿参数θ={R,t}在每一帧都用BA进行优化,θ约束在SE(3)李群中。
下面就是对BA要优化的cost function的定义(包括点和线两种特征):
X
j
∈
R
3
X_j \in \mathbb R^3
Xj∈R3是地图中的第j个点,对于第i个关键帧,这个点可以被投影到图像平面上,有
x
~
i
,
j
=
π
(
X
j
,
θ
i
,
K
)
(
5
)
\widetilde{x}_{i,j} = \pi (X_j, \theta_i, K) \space \space \space(5)
x
i,j=π(Xj,θi,K) (5)
θ
i
\theta_i
θi就是第i个关键帧的位姿。又知道这个点的观测量
x
i
,
j
x_{i,j}
xi,j,那么3D误差就为:
e
i
,
j
=
x
i
,
j
−
x
~
i
,
j
(
6
)
e_{i,j} = x_{i,j} - \widetilde{x}_{i,j} \space \space \space(6)
ei,j=xi,j−x
i,j (6)
类似的,
P
j
,
Q
j
P_j, Q_j
Pj,Qj是地图上的第j条线段,对应的图像投影(第i个关键帧)可以被写成:
p
~
i
,
j
h
=
π
(
P
j
,
θ
i
,
K
)
(
7
)
q
~
i
,
j
h
=
π
(
Q
j
,
θ
i
,
K
)
(
8
)
\widetilde{p}^h_{i,j} = \pi (P_j, \theta_i, K) \space \space \space(7) \\ \widetilde{q}^h_{i,j} = \pi (Q_j, \theta_i, K) \space \space \space(8)
p
i,jh=π(Pj,θi,K) (7)q
i,jh=π(Qj,θi,K) (8)
已知在图像上的第j条线段两个端点的观测值
p
i
,
j
p_{i,j}
pi,j和
q
i
,
j
q_{i,j}
qi,j,可以使用公式(1)来估计观测到的线系数
I
~
i
,
j
\widetilde{I}_{i,j}
I
i,j(这里的h表示齐次坐标)。所以,定义线的误差向量为(点到线的误差):
e
i
,
j
′
=
(
I
~
i
,
j
)
T
(
K
−
1
p
i
,
j
h
)
(
9
)
e
i
,
j
′
′
=
(
I
~
i
,
j
)
T
(
K
−
1
q
i
,
j
h
)
(
10
)
e_{i,j}^{'} = (\widetilde{I}_{i,j})^T(K^{-1}p_{i,j}^h) \space \space \space(9)\\ e_{i,j}^{''} = (\widetilde{I}_{i,j})^T(K^{-1}q_{i,j}^h) \space \space \space(10)
ei,j′=(I
i,j)T(K−1pi,jh) (9)ei,j′′=(I
i,j)T(K−1qi,jh) (10)
按照文献[30]中解释的,这个误差值在端点沿着3D线发生偏移的时候也会发生变化,作为隐式正则化,允许我们在 BA 中使用这种非最小线参数化(?)。
然后,就把两种误差统一到一个cost function中:
C
=
∑
i
,
j
ρ
(
e
i
,
j
T
Ω
i
,
j
−
1
e
i
.
j
+
e
i
,
j
′
T
Ω
i
,
j
′
−
1
e
i
,
j
′
+
e
i
,
j
′
′
T
Ω
i
,
j
′
′
−
1
e
i
,
j
′
′
C = \sum_{i,j} \rho (e_{i,j}^T \Omega_{i,j}^{-1}e_{i.j} +{e_{i,j}^{'}}^{T}{\Omega_{i,j}^{'}}^{-1}e_{i,j}^{'} + {e_{i,j}^{''}}^{T}{\Omega_{i,j}^{''}}^{-1}e_{i,j}^{''}
C=i,j∑ρ(ei,jTΩi,j−1ei.j+ei,j′TΩi,j′−1ei,j′+ei,j′′TΩi,j′′−1ei,j′′
这里的 ρ \rho ρ是Huber robust cost function,三个 Ω \Omega Ω是分别与检测到关键点和线端点的尺度相关的协方差矩阵。
C.全局重定位
当相机的追踪失效时,就要进行重定位,一种典型的解决方式就是PnP算法,它可以利用匹配好的之前关键帧的3D地图点来估计当前帧(lost)的位姿。在PnP之上,还用RANSAC来去除外点匹配。
ORB-SLAM中使用的是EPnP,但它只能使用点来作为输入;所以为了解决线特征的重定位,使用了EPnPL,它可以最小化“检测到的线重投影误差”,即公式(4)。
EPnPL的优点: 对线遮挡和误检测的情况有鲁棒性
为什么EPnPL鲁棒呢,具体是怎么做的呢?
因为这个方法分为两个步骤
1)先最小化检测到的线重投影误差,并估计线的端点
p
d
,
q
d
p_d, q_d
pd,qd
2)再沿着线移动观测到的端点,以便于能匹配到三维空间端点P,Q的投影
p
~
d
,
q
~
d
\widetilde{p}_d, \widetilde{q}_d
p
d,q
d.只要这些匹配建立了,相机的位姿就可以被可靠的估计出来。
V. MAP INITIALIZATION WITH LINES
这一节介绍了怎么只用线匹配来估计初始地图(进行初始化)。
ORB-SLAM的初始化:使用至少两帧的点特征对应关系,用单应性矩阵或者本质矩阵估计算法来算出初始地图和位姿参数。
本论文提出的:
从图中看到,P,Q是三维空间中一条线段的两个端点,把这两端点投影到3帧相机视角下,
{
p
1
,
q
1
}
,
{
p
1
,
q
1
}
,
{
p
1
,
q
1
}
\{p_1, q_1\}, \{p_1, q_1\}, \{p_1, q_1\}
{p1,q1},{p1,q1},{p1,q1}是每一帧的端点投影坐标,
I
1
,
I
2
,
I
3
∈
R
3
I_1, I_2, I_3 \in \mathbb R^3
I1,I2,I3∈R3是对应的线系数(由投影的端点算出)。
算法提出的假设: 在连续相机位姿之间,只有小且连续的旋转,所以有第一到第二帧的旋转和第二到第三帧的旋转一样。
在这个假设下,三个相机旋转是
R
1
=
R
T
,
R
2
=
I
,
R
3
=
R
R_1 = R^T, R_2=I, R_3=R
R1=RT,R2=I,R3=R, I是3X3单位阵。
注意,线系数
I
i
,
i
=
{
1
,
2
,
3
}
I_i,i=\{1,2,3\}
Ii,i={1,2,3}也代表了向量的参数,该向量垂直于由投影中心
O
i
O_i
Oi和投影点
p
i
,
q
i
p_i,q_i
pi,qi形成的平面。这样的两个向量
I
i
I_i
Ii的叉乘将和直线P,Q平行,同时与第三个矢量正交,所有这些矢量经过适当的旋转并放入到公共参考系中。这个约束可以写为:
I
2
T
(
(
R
T
I
1
)
×
(
R
I
3
)
)
=
0
(
11
)
I^T_2((R^TI_1)×(RI_3))=0 \space \space \space(11)
I2T((RTI1)×(RI3))=0 (11)
对于小旋转,R可以近似为:
R
=
(
1
−
r
3
r
2
r
3
1
−
r
1
−
r
2
r
1
1
)
(
12
)
R= \left( \begin{array}{ccc} 1& -r_3 & r_2 \\ r_3 & 1& -r_1 \\ -r_2 & r_1 & 1\\ \end{array}\right) \space \space \space(12)
R=⎝⎛1r3−r2−r31r1r2−r11⎠⎞ (12)
对于这个参数化,有三条匹配线,我们将会有三个含有三个未知数 r 1 , r 2 , r 3 r_1,r_2,r_3 r1,r2,r3的二次方程,如方程11。我们调整文献[15]的多项式求解器,最多可以得到8个解。对每个可能的旋转矩阵,我们可以通过三焦张量方程[11]来获得 t1,t3。假设 t2=0。我们评估8个可能解,然后保留使得方程11最小的那个解。
为了在使用三焦点张量方程求解平移分量时,获得足够的独立约束,我们需要两个额外的线对应关系,因此,我们算法所需的线特征匹配数量是5。
论文翻译: https://blog.csdn.net/u013341645/article/details/78668316