Robust Stereo Visual Odometry Using Iterative Closest Multiple Lines
Abstract
本文在视觉里程计中的线性特征匹配以及位姿信息恢复中做了一些必要的创新，本文一共有俩个贡献：
1）首先，一种类似于ICP方法的ICMP线性特征重构方案的提出，可以有效的避免陷入局部做小值；
2）一种"假设-验证"的方法，当基于梯度的匹配算法失败后我们的一种后背的基于“假设-验证”的算法进行补充。
Introduction
由于特征点简单、高效的特性，基于特征信息的视觉里程计使用较为广泛，然而在一些场景中无法提供足够数量的特征点信息，以边
(
e
d
g
e
)
(edge)
(edge)为特征的视觉里程计可以实现更加稳定的位姿信息估算。
由于检测有效的边是一项相对困难的任务，于是我们将特征边匹配的问题转换为求解优化最优的位姿变换信息使得二者之间能够匹配到最多数量的边。
在求解6自由度的维系信息时存在局部最小值的问题，对于特征点信息可以通过ICP的方法进行位姿信息估算，但如果使用点序列来表示特征边，同时使用传统求解特征点的方法求解位姿信息是一种不明智的做法，这样只会增加计算量且不会带来任何有用的额外信息。
本文中我们提出了一种类似ICP方法的ICML线性特征信息匹配原则，它使用
o
n
e
−
t
o
−
m
a
n
y
one-to -many
one−to−many的思想实现最有的线性特征匹配，为了提升系统的鲁棒性，文章中海介绍了一种后备的基于随机采样的方案，不同于
R
A
N
S
A
C
RANSAC
RANSAC，该方案更完善的使用了计算资源。
Related Work
介绍了一些之前基于特征边实现的视觉里程计，通常是基于外观进行的匹配操作，只在一部分的子集中进行匹配，容易在稀疏环境下发生问题。
Stereo Reconstruction
实现线性特征的匹配操作如下：
- 使用 C a n n y − d e t e c t Canny-detect Canny−detect提取边界信息；
- 在上述提取的边界信息中使用 D o u g l a s − P e u c k e r Douglas-Peucker Douglas−Peucker方法提取出直线信息；
- 将每幅图中获取的线性特征根据角度进行排序；
- 最后根据搜索区域和重叠部分进行特征线匹配。
其中，这一步中所有的水平线段由于不易匹配暂时不在这一步中，会在后续的匹配中和对角线和垂直线进行匹配操作。
文中通过特征线的端点代表特征线信息，已知俩帧匹配的像素投影坐标信息 ( u l , v l ) , ( u r , v r ) , v = v l = v r (u_l,v_l),(u_r,v_r),v=v_l=v_r (ul,vl),(ur,vr),v=vl=vr和相机的内参，计算对应的点的相机坐标系下的空间坐标信息为： c = ( b u l u l − u r , b v u l − u r , b f u l − u r ) T \bold c=(\frac{bu_l}{u_l-u_r},\frac{bv}{u_l-u_r},\frac{bf}{u_l-u_r})^T c=(ul−urbul,ul−urbv,ul−urbf)T其中， b , f b,f b,f分别表示相机参数基线和焦距。通过位姿变换信息可以获得世界坐标系下的空间点坐标： p = R − 1 c + t \bold p=\bold R^{-1}\bold c+\bold t p=R−1c+t
Motion Reconstruction Using Lines
针对双目视觉信息，通过俩条不平行的特征线进行位姿信息恢复。
A.Line Reprojection Error
对于特征点信息，可以直接通过定义投影点计算与匹配信息之间的距离作为重投影误差，对于直线特征信息我们可以通过定义重投影特征线对应的端点信息到匹配的特征线之间的距离作为俩个特征线之间的重投影信息。
定义特征线之间的距离信息
d
i
j
(
x
)
=
0.5
(
∣
d
p
1
(
I
j
(
x
)
,
I
^
i
)
∣
+
∣
d
p
2
(
I
j
(
x
)
,
I
^
i
)
∣
)
d_{ij}(x)=0.5(|d_{p_1}(I^j(x),\hat I^i)|+|d_{p_2}(I^j(x),\hat I^i)|)
dij(x)=0.5(∣dp1(Ij(x),I^i)∣+∣dp2(Ij(x),I^i)∣)
俩线之间的重叠部分为
l
(
I
j
(
x
)
,
I
^
i
)
l(I^j(x),\hat I^i)
l(Ij(x),I^i)其中，
I
j
(
x
)
I^j(x)
Ij(x)表示投影特征线，
I
^
i
\hat I^i
I^i表示匹配特征线，
d
p
1
(
I
j
(
x
)
,
I
^
i
)
d_{p_1}(I^j(x),\hat I^i)
dp1(Ij(x),I^i)表示一侧线性端点端点到另一侧线性距离。
定义俩种误差公式mean pixel error & mean squares error：
M
E
(
x
)
=
∑
i
,
j
d
i
j
(
x
)
l
i
j
∑
i
,
j
l
i
j
M
S
E
(
x
)
=
0.5
∑
i
,
j
(
d
p
1
(
I
j
(
x
)
,
I
^
i
)
2
+
d
p
2
(
I
j
(
x
)
,
I
^
i
)
2
)
l
i
j
∑
i
,
j
l
i
j
ME(x)=\frac{\sum_{i,j}d_{ij}(x)l_{ij}}{\sum_{i,j}l_{ij}}\\MSE(x)=0.5\frac{\sum_{i,j}(d_{p_1}(I^j(x),\hat I^i)^2+d_{p_2}(I^j(x),\hat I^i)^2)l_{ij}}{\sum_{i,j}l_{ij}}
ME(x)=∑i,jlij∑i,jdij(x)lijMSE(x)=0.5∑i,jlij∑i,j(dp1(Ij(x),I^i)2+dp2(Ij(x),I^i)2)lij
其中，
l
i
,
j
l_{i,j}
li,j表示投影线和匹配线之间的重叠部分。
一般情况下会将误差函数
M
E
ME
ME加入核函数的约束
h
(
z
)
=
{
0.5
z
2
k
∣
z
∣
−
0.5
k
2
h(z)=\begin{cases} 0.5z^2\\ k|z|-0.5k^2\end{cases}
h(z)={0.5z2k∣z∣−0.5k2其中，
k
=
1
p
x
k=1px
k=1px。
B.Iterative Closet Multiple Lines(ICML) Algorithm
将所有满足条件
d
i
j
<
d
m
a
x
,
l
i
j
>
0
,
α
i
j
<
α
m
a
x
（
12
度
）
d_{ij}<d_{max},l_{ij}>0,\alpha_{ij}<\alpha_{max}（12度）
dij<dmax,lij>0,αij<αmax（12度）的线性特征都进行试匹配。
最小误差
M
S
E
MSE
MSE函数：
f
(
x
)
=
1
2
e
(
x
)
T
W
e
(
x
)
e
(
x
)
=
(
⋯
,
d
p
1
(
I
i
(
x
)
,
I
^
i
)
,
d
p
2
(
I
i
(
x
)
,
I
^
i
)
,
⋯
)
f(x)=\frac1 2e(x)^TWe(x)\\e(x)=(\cdots,d_{p_1}(I^i(x),\hat I^i),d_{p_2}(I^i(x),\hat I^i),\cdots)
f(x)=21e(x)TWe(x)e(x)=(⋯,dp1(Ii(x),I^i),dp2(Ii(x),I^i),⋯)
最小误差
M
E
ME
ME函数：
f
(
x
)
=
1
2
W
e
h
(
x
)
e
(
x
)
=
h
(
e
(
x
)
)
f(x)=\frac1 2We_h(x)\\e(x)=h(e(x))
f(x)=21Weh(x)e(x)=h(e(x))
其中，
W
=
d
i
a
g
l
i
j
）
W=diagl_ij）
W=diaglij），利用
L
M
LM
LM求解最小值
x
=
(
t
x
,
t
y
,
t
z
,
ϕ
x
,
ϕ
y
ϕ
z
)
J
=
∂
e
(
x
)
∂
x
x=(t_x,t_y,t_z,\phi_x,\phi_y\phi_z)\\J=\frac{\partial e(x)}{\partial x}
x=(tx,ty,tz,ϕx,ϕyϕz)J=∂x∂e(x)
由于分母一样，对分母进行了省略。
通过求解的增量式的位姿信息，可以通过上一时刻的位姿信息获取下以时刻的位姿信息。
R
k
=
Δ
R
R
k
−
1
t
k
=
t
k
−
1
−
(
Δ
R
R
k
−
1
)
Δ
t
R_k=\Delta RR_{k-1}\\t_k=t_{k-1}-(\Delta RR_{k-1})\Delta t
Rk=ΔRRk−1tk=tk−1−(ΔRRk−1)Δt
上述信息必须通过误差距离
d
i
j
d_{ij}
dij之间的值进行终止判断，初始设置误差距离
d
i
n
i
t
d_{init}
dinit，终止条件为
d
f
i
n
a
l
d_{final}
dfinal，于是整个系统流程为
- 配置 d m a x = d i n i t d_{max}=d{init} dmax=dinit；
- 将所有满足条件 d i j < d m a x , l i j > 0 , α i j < α m a x （ 12 度 ） d_{ij}<d_{max},l_{ij}>0,\alpha_{ij}<\alpha_{max}（12度） dij<dmax,lij>0,αij<αmax（12度）的线进行候选；
- 利用 L M LM LM方法求解最优值；
- 如果 d i j < d f i n a l d_{ij}<d_{final} dij<dfinal跳出，否则 d m a x = d m a s / 2 d_{max}=d_{mas}/2 dmax=dmas/2并且返回第二步。
由于 o n e − t o − m a n y one-to-many one−to−many策略，这里有太多的试匹配线需要处理，为了从中获取最优价值的线段信息，我们依据 d i j d_{ij} dij进行排序，将距离误差太大的试匹配线踢除直到线的重叠距离值等于3倍的待匹配特征线的长度，剩下的试匹配线中可以获得最优匹配特征（我觉得作者在胡扯，很有可能把正确的匹配信息误删。），一般情况下剩余的特征线大于3。
C.Evaluation of Common Configurations
针对ICL和ICML俩种方法，当在neighbor范围内是否存在正确的匹配信息，我们分别进行讨论。
1）Matching with Successful Line Detection
ICL和ICMLL都可以获取最优解，但ICL中会有一段梯度等于0的段。
2）Matching with Partial Line Detection
这种情况下误差函数
M
E
ME
ME无法获取最优解，会有俩个局部最优解存在，而函数
M
S
E
MSE
MSE依旧可以获取最优解。
D.Registration Failure Detection
为了检测获得结果是否为误匹配，文中通过
M
k
=
∑
i
,
j
l
i
j
m
i
n
(
∑
i
∣
I
^
i
∣
,
∑
j
∣
I
j
(
x
)
∣
)
>
M
m
i
n
M_k=\frac{\sum_{i,j}l_{ij}}{min(\sum_i|\hat I^i|,\sum_j|I^j(x)|)}>M_{min}
Mk=min(∑i∣I^i∣,∑j∣Ij(x)∣)∑i,jlij>Mmin确保有足够的特征线重叠存在。
通过判断
M
E
(
x
)
<
G
m
a
x
ME(x)<G_{max}
ME(x)<Gmax确保误差信息足够小。
但上述判别式成立的条件时在图中的特征线中有足够的多样性，于是通过公式$
L
k
=
l
1
+
l
2
+
l
3
>
L
m
i
n
L_k=l_1+l_2+l_3>L_{min }
Lk=l1+l2+l3>Lmin确保水平线和对角线足够多。
E.Robust Sample Consensus Matching
该部分采用不同与
R
A
N
S
A
C
RANSAC
RANSAC方法的采样算法通过采用差距较大的匹配特征线来实现鲁邦性的位姿信息恢复，用于作为梯度方法失败的后备方案。基本步骤包括将水平和垂直方向的特征线进行聚类并按照特征线之间的重叠部分进行排序，每个序列选择前
N
S
A
C
N_{SAC}
NSAC个特征线用于参与后续的特征匹配。需要保持俩条线之间至少相差45度，在上述假设的前提下我们进行下面的操作：
1）在假特征线组中选取俩条不平行的特征线进行基于梯度信息的位姿信息恢复；
2）如果
M
E
(
x
)
>
G
S
A
C
ME(x)>G_{SAC}
ME(x)>GSAC，返回上一步中；
3）选取所有
d
m
a
x
=
d
S
A
C
d_{max}=d_{SAC}
dmax=dSAC的匹配直线，将重叠长度进行累加；
其中比较多的重叠部分为最优解。
527
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
