为了在最小二乘问题中去除外值,对状态进行更准确的估计值,往往需要手动设置阈值并选择核函数。本文将选择核函数的问题转换为了对参数的估计,再优化过程中根据残差分布自适应选择核函数和阈值。
自适应鲁棒核函数
本文使用的自适应核函数如下:
ρ
(
r
,
α
,
c
)
=
α
−
2
α
(
(
(
r
/
c
)
2
∣
α
−
2
∣
+
1
)
α
/
2
−
1
)
\rho(r,\alpha,c)=\frac{\alpha-2}{\alpha}((\frac{(r/c)^2}{|\alpha-2|}+1)^{\alpha/2}-1)
ρ(r,α,c)=αα−2((∣α−2∣(r/c)2+1)α/2−1)
在
α
\alpha
α取
−
∞
,
−
2
,
0
,
1
,
2
-\infty,-2,0,1,2
−∞,−2,0,1,2时,核函数等价于
W
e
l
s
c
h
,
G
e
m
a
n
−
M
c
C
l
u
r
e
,
C
a
u
c
h
y
,
p
s
e
u
d
o
−
H
u
b
e
r
/
L
1
−
L
2
Welsch,Geman-McClure,Cauchy,pseudo-Huber/L1- L2
Welsch,Geman−McClure,Cauchy,pseudo−Huber/L1−L2和
L
2
L2
L2范数。由于
α
\alpha
α取不到,实际上采用的是逼近的极限。
为了避免选择
α
\alpha
α将所有点都变成外点从而取得极小值,需要给优化函数增加额外的约束项。
ρ
α
(
r
,
α
,
c
)
=
ρ
(
r
,
α
,
c
)
+
l
o
g
c
+
l
o
g
Z
(
α
)
\rho_\alpha(r,\alpha,c)=\rho(r,\alpha,c)+logc+logZ(\alpha)
ρα(r,α,c)=ρ(r,α,c)+logc+logZ(α)
其中
Z
(
α
)
=
∫
−
∞
∞
e
−
ρ
(
r
,
α
,
c
)
d
r
Z(\alpha)=\int_{-\infty}^{\infty} e^{-\rho(r,\alpha,c)} dr
Z(α)=∫−∞∞e−ρ(r,α,c)dr
截断核函数
由于 Z ( α ) Z(\alpha) Z(α)在 α < 0 \alpha<0 α<0时无定义,所以需要对积分范围进行限制,将积分上下限改为: [ − τ , τ ] [-\tau,\tau] [−τ,τ],实验中发现 τ \tau τ的取值为 10 c 10c 10c即可,更大的范围对结果没有明显提升。同时限定 α \alpha α的范围为 [ − 10.2 ] [-10.2] [−10.2]。
参数估计
参数估计分为两步,首先估计 α \alpha α,然后根据 α \alpha α的值来估计其他参数。 c c c作为对于残差标度的估计,在整个过程中保持不变。
实验
本文在ICP和BA两个问题上验证了方法的有效性,在 S L A M SLAM SLAM上,本文的方法不需要调参就可以达到和现有调参方法相近或更好的约束。在 B A BA BA问题中,本文的方法在获得较好的结果的同时,解收敛区域更大。
展望
- 对 α \alpha α和 c c c的联合优化
- 在一次优化中分块选择不同的 α \alpha α值