本文概述了视觉SLAM中状态估计的基本原理，通过最大似然估计和最小二乘方法解决相机位姿与路标点的推断问题。讲解了经典SLAM模型、噪声处理、线性和非线性系统的区别，以及滤波器与优化方法的运用，重点介绍了高斯牛顿法和列文伯格-马夸尔特法的求解过程。
<视觉SLAM十四讲>笔记
状态估计问题
1.最大似然以及最小二乘
经典的SLAM模型由一个运动方程和一个观测方程组成：
{
x
k
=
f
(
x
k
−
1
,
u
k
)
+
w
k
z
k
,
j
=
h
(
y
j
,
x
k
)
+
v
k
,
j
(1)
\left\{\tag{1} \begin{aligned} x_k&=f(x_{k-1},u_k)+w_k \\ z_{k,j}&=h(y_j,x_k)+v_{k,j} \end{aligned} \right.
{xkzk,j=f(xk−1,uk)+wk=h(yj,xk)+vk,j(1)
其中， x k x_k xk为相机的位姿(六自由度)，通常用变换矩阵或者李代数的指数描述(即: x k = T k = e x p ( ξ k ⋀ ) x_k=T_k=exp(\xi^{\bigwedge}_k) xk=Tk=exp(ξk⋀))
u k u_k uk为输入量； z k , j z_{k,j} zk,j为观测方程， y j y_j yj为路标点位置， w k w_k wk和 v k , j v_{k,j} vk,j为运动噪声和观测噪声.
在视觉SLAM中,观测方程 z k , j z_{k,j} zk,j就是图像里的像素.假设在 x k x_k xk处对路标 y j y_j yj进行了一次观测,对应到图像上的像素位置为 z k , j z_{k,j} zk,j,那么,观测方程就可以表示为: s z k , j = K ( R k y j + t k ) sz_{k,j}=K(R_ky_j+t_k) szk,j=K(Rkyj+tk).(这里是书上十四讲里的写法,其实将s除到右边更容易理解: z k , j = K ( R k y j + t k ) s z_{k,j}=\frac{K\left(R_ky_j+t_k\right)}{s} zk,j=sK(Rkyj+tk),毕竟在从三维点到二维像素的过程中丢失了像素点的距离信息,这个s就是像素点的距离)
关于噪声,通常假设噪声项服从高斯分布: w k w_k wk~ N ( 0 , R k ) N(0,R_k) N(0,Rk), v k v_k vk~ N ( 0 , Q k , j ) N(0,Q_{k,j}) N(0,Qk,j), R k R_k Rk, Q k , j − 1 Q^{-1}_{k,j} Qk,j−1为信息矩阵,即高斯分布协方差矩阵的逆
我们要求解的就是,如何通过带噪声的输入量 u k u_k uk以及带噪声的观测量 z k , j z_{k,j} zk,j来推断相机位姿 x k x_k xk以及路标点位置 y j y_j yj,这就构成了状态估计的问题.大概可以分为两种情况:
- 最简单的情况:线性系统,高斯噪声
- 复杂情况:非线性系统,非高斯噪声
历史上很长一段时间都使用滤波器求解状态估计. 滤波器的做法就是假设系统具有马尔可夫性,即系统在k时刻的状态只与k-1时刻的状态有关,与再之前的状态无关. 这样的话我们就只需要维护当前时刻系统的状态信息,当有新的数据输入到系统当中,就更新当前状态的估计 x k x_k xk.
但是近年来非线性优化已经成为主流.基于优化的方法可以在更大的范围内达到最优化,例如在SLAM中的滑动窗口估计法,我们可以固定一些历史轨迹近对当前时刻附近的轨迹进行优化.
考虑从1到N的所有时刻,并假设有M个路标点,定义所有时刻机器人位姿和路标坐标为:
x
=
{
x
1
,
.
.
.
,
x
N
}
x=\{x_1,...,x_N\}
x={x1,...,xN},
y
=
{
y
1
,
.
.
.
y
N
}
y=\{y_1,...y_N\}
y={y1,...yN}
假设u代表所有时刻的输入,z表示所有时刻的观测,求状态x,y的条件概率分布:
P
(
x
,
y
∣
z
,
u
)
P(x,y|z,u)
P(x,y∣z,u).
特别地,当我们不知道控制输入,只有一张张图片时,即只考虑观测方程带来的数据时,相当于估计
P
(
x
,
y
∣
z
)
P(x,y|z)
P(x,y∣z)的条件概率分布,此问题也称为SFM(Structure form Motion),即从许多图像中重建三维结构.
为了估计状态变量的条件分布(即在输入和观测的条件下位姿和路标位置的分布情况),利用贝叶斯法则,有:
P
(
x
,
y
∣
z
,
u
)
=
P
(
z
,
u
∣
x
,
y
)
P
(
x
)
P
(
z
,
u
)
∝
P
(
z
,
u
∣
x
,
y
)
⏟
似然
P
(
x
,
y
)
⏟
先验
(2)
P(x,y|z,u)=\frac{P(z,u|x,y)P(x)}{P(z,u)}\propto\underbrace{P(z,u|x,y)}_{\text{似然}}\underbrace{P(x,y)}_{\text{先验}}\tag{2}
P(x,y∣z,u)=P(z,u)P(z,u∣x,y)P(x)∝似然
P(z,u∣x,y)先验
P(x,y)(2)
其中,符号 ∝ \propto ∝表示成正比,其左侧称为后验概率(也就是我们要求解的条件概率),右侧的 P ( z ∣ x ) P(z|x) P(z∣x)为似然,另一部分称为先验.
直接求后验分布是困难的,但是求一个状态最优估计,使得在该状态下后验概率最大化是可行的:
(
x
,
y
)
M
A
P
∗
=
a
r
g
m
a
x
P
(
x
,
y
∣
z
,
u
)
=
a
r
g
m
a
x
P
(
z
,
u
∣
x
,
y
)
P
(
x
,
y
)
(3)
(x,y)^*_{MAP}=arg\ max\ P(x,y|z,u)=arg\ max\ P(z,u|x,y)P(x,y)\tag{3}
(x,y)MAP∗=arg max P(x,y∣z,u)=arg max P(z,u∣x,y)P(x,y)(3)
a r g m a x f ( x ) arg\ max\ f(x) arg max f(x)表示 f ( x ) f(x) f(x)最大时 x x x的取值.
注意:分母部分与待估计的状态
x
,
y
x,y
x,y无关,可以忽略.所以最大化后验概率 等价于 最大化似然和先验的乘积. 但是实际上,我们也不知道机位姿或路标在什么地方,即:没有先验,那么也就变成了求解最大似然估计:
(
x
,
y
)
M
L
E
∗
=
a
r
g
m
a
x
P
(
z
,
u
∣
x
,
y
)
(4)
(x,y)^*_{MLE}=arg\ max\ P(z,u|x,y)\tag{4}
(x,y)MLE∗=arg max P(z,u∣x,y)(4)
直观地讲,似然是指:“在现在的位姿下,可能产生怎样的观测数据”.由于我们知道观测数据,所以最大似然估计可以理解成:“在什么样的状态下,最可能产生现在观测到的数据”.
所以对于SLAM,我们可以将其看作时一个求解最大似然估计的过程.下面讨论最大似然估计的求解:
例子:假设某次观测,我们在
x
k
x_k
xk位置对路标点
y
j
y_j
yj进行观测,得到了观测数据
z
k
,
j
z_{k,j}
zk,j,但是
z
k
,
j
z_{k,j}
zk,j是受到高斯分布影响的,如下式:
z
k
.
j
=
h
(
y
j
,
x
k
)
+
v
k
,
j
(5)
z_{k.j}=h(y_j,x_k)+v_{k,j}\tag{5}
zk.j=h(yj,xk)+vk,j(5)
假设噪声项
v
k
∼
N
(
0
,
Q
k
,
j
)
v_k\sim N(0,Q_{k,j})
vk∼N(0,Qk,j),所以观测数据的条件概率分布为:
P
(
z
j
.
k
∣
x
k
,
y
j
)
=
N
(
h
(
y
j
,
x
k
)
,
Q
k
,
j
)
(6)
P(z_{j.k}|x_k,y_j)=N(h(y_j,x_k),Q_{k,j})\tag{6}
P(zj.k∣xk,yj)=N(h(yj,xk),Qk,j)(6)
它依然是一个高斯分布.现在要求x,y的最大似然估计(即x,y如何取值才能使P(z|x,y)最大化)
首先我们来看一看一般的高斯分布:
P
(
x
)
=
1
(
2
π
)
N
d
e
t
(
Σ
)
exp
(
−
1
2
(
x
−
u
)
T
Σ
−
1
(
x
−
μ
)
)
(7)
P(x)=\frac{1}{\sqrt{(2\pi)^Ndet(\Sigma)}}\exp\left( -\frac{1}{2}(x-u)^T\Sigma^{-1}(x-\mu) \right)\tag{7}
P(x)=(2π)Ndet(Σ)1exp(−21(x−u)TΣ−1(x−μ))(7)
那么在x取何值时
P
(
x
)
P(x)
P(x)最大呢?
首先取它的负对数形式:
−
l
n
(
P
(
x
)
)
=
1
2
l
n
(
(
2
π
)
N
d
e
t
(
Σ
)
)
+
1
2
(
x
−
μ
)
T
Σ
−
1
(
x
−
μ
)
(8)
-ln(P(x))=\frac{1}{2}ln\left( (2\pi)^N det(\Sigma) \right)+\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)\tag{8}
−ln(P(x))=21ln((2π)Ndet(Σ))+21(x−μ)TΣ−1(x−μ)(8)
因为对数函数是单调递增的,所以原函数的最大化相当于将负对数求最小化.在最小化上式时,第一项
1
2
l
n
(
(
2
π
)
N
d
e
t
(
Σ
)
)
\frac{1}{2}ln\left( (2\pi)^N det(\Sigma) \right)
21ln((2π)Ndet(Σ))与
x
x
x无关,可以略去.
所以,原问题的最大似然:
P
(
z
j
.
k
∣
x
k
,
y
j
)
=
N
(
h
(
y
j
,
x
k
)
,
Q
k
,
j
)
P(z_{j.k}|x_k,y_j)=N(h(y_j,x_k),Q_{k,j})
P(zj.k∣xk,yj)=N(h(yj,xk),Qk,j)
相当于最小化:
x
∗
=
a
r
g
m
i
n
(
(
z
k
,
j
−
h
(
x
k
,
y
j
)
)
T
Q
k
,
j
−
1
(
z
k
,
j
−
h
(
x
k
,
y
j
)
)
)
(9)
x^*=arg\ min\ \left( (z_{k,j}-h(x_k,y_j))^TQ^{-1}_{k,j}(z_{k,j}-h(x_k,y_j)\ ) \right)\tag{9}
x∗=arg min ((zk,j−h(xk,yj))TQk,j−1(zk,j−h(xk,yj) ))(9)
这就是最小二乘.
1 2 ( x − μ ) T Σ − 1 ( x − μ ) \frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu) 21(x−μ)TΣ−1(x−μ)就是一个最小而乘,它是一个平方形式的式子(直观来讲,就是 ( x − μ ) (x-\mu) (x−μ)的转置 乘 Σ − 1 ( x − μ ) \Sigma^{-1}(x-\mu) Σ−1(x−μ),最简单的情况下:如果 Σ \Sigma Σ是一个单位阵,就等于 ( x − μ ) T ( x − μ ) (x-\mu)^T(x-\mu) (x−μ)T(x−μ),它就是一个平方形式的式子,我们也称之为二范数,一般情况下如果存在 Σ − 1 \Sigma^{-1} Σ−1,我们也称 1 2 ( x − μ ) T Σ − 1 ( x − μ ) \frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu) 21(x−μ)TΣ−1(x−μ)为 Σ \Sigma Σ范数,当然这些只是术语,说它是最小二乘也没有问题).
Σ − 1 \Sigma^{-1} Σ−1也叫信息矩阵.
所以式(9)实际上最小化的是一个平方项,如果将所有的观测都放进去,并且展开的话,就是一堆平方项的和,所以叫最小二乘
求最大似然估计本质上就是在求最小二乘,因为它们之间就隔了一个负对数的运算
2.最小二乘理解与解算方法
现在我们将最大似然估计变成了最小二乘问题.仔细观察上式会发现,我们最小化的东西其实是噪声项( z k , j − h ( x k , y j ) z_{k,j}-h(x_k,y_j) zk,j−h(xk,yj),即误差)的一个二次型(这个二次型也叫马氏距离).
对于原问题:
{
x
k
=
f
(
x
k
−
1
,
u
k
,
w
k
)
z
k
,
j
=
h
(
y
j
,
x
k
,
v
k
,
j
)
\left\{ \begin{aligned} x_k&=f(x_{k-1},u_k,w_k) \\ z_{k,j}&=h(y_j,x_k,v_{k,j}) \end{aligned} \right.
{xkzk,j=f(xk−1,uk,wk)=h(yj,xk,vk,j)
我们对当前的估计值求误差,定义误差如下:
e
v
,
k
=
x
k
−
f
(
x
k
−
1
,
u
k
)
e
y
,
j
,
k
=
z
k
,
j
−
h
(
x
k
,
y
j
)
e_{v,k}=x_k-f(x_{k-1},u_k)\\ e_{y,j,k}=z_{k,j}-h(x_k,y_j)
ev,k=xk−f(xk−1,uk)ey,j,k=zk,j−h(xk,yj)
e v , k = x k − f ( x k − 1 , u k ) e_{v,k}=x_k-f(x_{k-1},u_k) ev,k=xk−f(xk−1,uk),为运动误差，也就是我们的估计值( x k x_k xk)-上一时刻按说应该推出来的值( f ( x k − 1 , u k ) f(x_{k-1},u_k) f(xk−1,uk))
e y , j , k = z k , j − h ( x k , y j ) e_{y,j,k}=z_{k,j}-h(x_k,y_j) ey,j,k=zk,j−h(xk,yj)为观测误差，也就是我们真实拿到的观测值( z k , j z_{k,j} zk,j)与我在 x k x_k xk处观测 y j y_j yj应该拿到的观测值( h ( x k , y j ) 的 差 h(x_k,y_j)的差 h(xk,yj)的差
然后我们将误差方程放入最小二乘里面,最小化误差的二范数,如下:
m
i
n
J
(
x
)
=
∑
k
e
v
,
k
T
R
k
−
1
e
v
,
k
+
∑
k
∑
j
e
y
,
k
,
j
T
Q
k
.
j
−
1
e
y
,
k
,
j
min\ J(x)=\sum_ke^T_{v,k}R^{-1}_ke_{v,k}+\sum_k\sum_je^T_{y,k,j}Q^{-1}_{k.j}e_{y,k,j}
min J(x)=k∑ev,kTRk−1ev,k+k∑j∑ey,k,jTQk.j−1ey,k,j
上式所表述的,就是运动误差和观测误差的平方和,然后我们要求一个 ( x , y ) (x,y) (x,y)使得总误差的平方和最小.如果将这个方程解出来了,就相当于得到了地图的最大似然估计.
直观解释:
- 由于噪声的存在,当我们将估计的轨迹与地图带入SLAM的运动和观测方程中时,它们并不会完美的成立. 此时就调整状态的估计,使得误差最小化
该问题有何结构?
- 由许多个误差的平方和(或Sigma范数和)组成.
- 虽然总体维度高,因为x中包含了所有的位姿和观测点,但每个项很简单,只关联两个变量,例如:运动误差只与 x k − 1 , x k x_{k-1},x_k xk−1,xk有关,观测误差只与 x k , y j x_k,y_j xk,yj有关
- 如果用李代数表达位姿,那么时无约束优化问题.但如果用旋转矩阵/变换矩阵描述位姿,则会引入旋转矩阵自身的约束,即需要在问题中加入s.t. R T R = I R^TR=I RTR=I且 d e t ( R ) = 1 det(R)=1 det(R)=1这样令人头大的问题
下面介绍通用的非线性最小二乘求解方法:
先考虑简单的问题,如何求这个式子:
m
i
n
x
F
(
x
)
=
1
2
∣
∣
f
(
x
)
∣
∣
2
2
\underset{x}{min}\ F(x)=\frac{1}{2}\left | \right |f(x)\left | \right |^2_2
xmin F(x)=21∣∣f(x)∣∣22,
这里,
x
∈
R
n
x\in\mathbb{R}^n
x∈Rn,f为任意函数,等号右边是一个2-范数的平方.2-范数简单理解
-
当 f f f很简单时:
解 d F d x = 0 \frac{dF}{dx}=0 dxdF=0,将得到极值点或鞍点,比较这些解即可
-
当f很复杂时:
d f / d x df/dx df/dx难求,或 d f / d x df/dx df/dx很难解时,可以使用迭代的方式求解
迭代方式:
- 1 给定某个初始值 x 0 x_0 x0.
- 2 对于第k次迭代,寻找一个增量 Δ x k \Delta x_k Δxk,使得 m i n x ∣ ∣ f ( x k + Δ x k ) ∣ ∣ 2 2 \underset{x}{min}\left | \right |f(x_k+\Delta x_k)\left | \right |^2_2 xmin∣∣f(xk+Δxk)∣∣22达到极小值
- 3 若 Δ x k \Delta x_k Δxk足够小,则停止
- 4 否则,令 x k + 1 = x k + Δ x k x_{k+1}=x_k+\Delta x_k xk+1=xk+Δxk,返回第二步.
这让求解倒数为0的问题变成了一个不断寻找下降增量 Δ x k \Delta x_k Δxk的问题,当函数下降,直到增量非常小的时候,就认为算法收敛,目标函数达到了一个极小值
问题:如何确定这个增量 Δ x k \Delta x_k Δxk?
确定增量的方法(即梯度下降策略):一阶和二阶梯度法
考虑第k次迭代,假设在
x
k
x_k
xk处,想要寻找到
Δ
x
k
\Delta x_k
Δxk.那么最直接的方法是将**目标函数
F
(
x
)
∗
∗
在
F(x)**在
F(x)∗∗在x_k$附近进行泰勒展开:
F
(
x
k
+
Δ
x
k
)
≈
F
(
x
k
)
+
J
(
x
k
)
T
Δ
x
k
+
1
2
Δ
x
k
T
H
(
x
k
)
Δ
x
k
F(x_k+\Delta x_k) \approx F(x_k) + J(x_k)^T \Delta x_k +\frac{1}{2}\Delta x^T_k H(x_k) \Delta x_k
F(xk+Δxk)≈F(xk)+J(xk)TΔxk+21ΔxkTH(xk)Δxk
其中
J
(
x
)
J(x)
J(x)是关于
x
x
x的一阶导数[也叫梯度矩阵/雅可比(Jacobian)矩阵(就是一阶项的系数)],一般会写成列向量,这样就可以和
Δ
x
\Delta x
Δx进行内积,得到一个标量],
H
H
H则是二阶导数[海塞(Hessian)矩阵],它们都在
x
k
x_k
xk处取值.我们可以选择保留泰勒展开的一阶或二阶项,那么对应的求解方法则称为一阶梯度或二阶梯度法,如果保留一阶梯度,那么取增量为反向的梯度,即可保证函数下降:
Δ
x
∗
=
−
J
(
x
k
)
\Delta x^*=-J(x_k)
Δx∗=−J(xk)
当然这只是个方向,通常还会再指定一个步长
λ
\lambda
λ,步长可以根据一定的条件计算出来.这种方法称为最速下降法.它的直观意义非常简单,只要我们沿着反向梯度方向前进,在一阶(线性)近似下,目标函数必定会下降.
(以上讨论都是在第
k
k
k次迭代时进行的,并不设计其他的迭代信息)
另外,我们亦可以选择保留二阶梯度信息,此时增量方程为(此处省略下标
k
k
k):
Δ
x
∗
=
a
r
g
m
i
n
F
(
x
)
+
J
(
x
)
T
Δ
x
+
1
2
Δ
x
T
H
(
x
)
Δ
x
\Delta x^*=arg\ min\ F(x) + J(x)^T \Delta x +\frac{1}{2}\Delta x^T H(x) \Delta x
Δx∗=arg min F(x)+J(x)TΔx+21ΔxTH(x)Δx
右侧只含有
Δ
x
\Delta x
Δx的零次,一次和二次项.求右侧等式关于
Δ
x
\Delta x
Δx的导数并令它为0,得到:
J
+
H
Δ
x
=
0
⟹
H
Δ
x
=
−
J
J+H\Delta x=0 \ \ \implies \ \ H\Delta x =-J
J+HΔx=0 ⟹ HΔx=−J
求解这个线性方程,就得到了增量.该方法又称为牛顿法.
可以看到,一阶和二阶梯度法都十分直观,只要把函数放在迭代点附近进行展开,并针对更新量做最小化即可.事实上,我们用一个一次或二次的函数近似了原来的函数,然后用近似函数的最小值来猜测原函数的最小值,只要原目标函数局部看起来像是一次或二次函数,这类算法就是成立的.
不过,这两种方法也存在它们自身的问题.最速下降法由于过于贪心,容易走出锯齿路线,反而增加了迭代次数,
而牛顿法则需要计算出目标函数的 H H H矩阵,这在问题规模较大时非常困难,通常倾向于避免 H H H的计算.为了能够保留二阶梯度,同时还能简化掉 H H H矩阵或者甚至避免掉它的计算,对于一般的问题,一些拟牛顿法可以得到较好的效果,而对于最小二乘问题,还有几类更实用的方法:高斯牛顿法(Gauss-Newton)和列文伯格-马夸尔特法(Levenberg-Marquadt).
3.高斯牛顿法
高斯牛顿法是优化算法中最简单的方法之一,它的思想是将
f
(
x
)
f(x)
f(x)进行一阶的泰勒展开.注意这里不是目标函数
F
(
x
)
F(x)
F(x)而是
f
(
x
)
f(x)
f(x)(否则就变成牛顿法了)
f
(
x
+
Δ
x
)
≈
f
(
x
)
+
J
(
x
)
T
Δ
x
f(x+\Delta x)\approx f(x)+J(x)^T \Delta x
f(x+Δx)≈f(x)+J(x)TΔx
其中,
J
(
x
)
T
J(x)^T
J(x)T是关于x的导数,为
n
×
1
n \times 1
n×1的向量.根据前面的框架,当前的目标是寻找增量
Δ
x
\Delta x
Δx,使得
∣
∣
f
(
x
+
Δ
x
)
∣
∣
2
\left | \right | f(x+\Delta x) \left | \right |^2
∣∣f(x+Δx)∣∣2达到最小.为了
Δ
x
\Delta x
Δx,我们需要解一个线性的最小二乘问题:
Δ
x
∗
=
a
r
g
m
i
n
x
1
2
∣
∣
f
(
x
)
+
J
(
x
)
T
Δ
x
∣
∣
2
\Delta x^* =arg \ \underset{x}{min} \frac{1}{2} \left | \right | f(x)+J(x)^T \Delta x \left | \right |^2
Δx∗=arg xmin21∣∣f(x)+J(x)TΔx∣∣2
将上述目标函数对
Δ
x
\Delta x
Δx求导并令导数为0.先展开平方项:
1
2
∣
∣
f
(
x
)
+
J
(
x
)
T
Δ
x
∣
∣
2
=
1
2
(
f
(
x
)
+
J
(
x
)
T
Δ
x
)
T
(
f
(
x
)
+
J
(
x
)
T
Δ
x
)
=
1
2
(
∣
∣
f
(
x
)
∣
∣
2
2
+
2
f
(
x
)
J
(
x
)
T
Δ
x
+
Δ
x
T
J
(
x
)
J
(
x
)
T
Δ
x
)
\begin{aligned} \frac{1}{2} \left | \right | f(x)+J(x)^T \Delta x \left | \right |^2 &=\frac{1}{2} \left(f(x)+J(x)^T \Delta x \right)^T \left(f(x)+J(x)^T \Delta x \right)\\ &=\frac{1}{2}\left( \left | \right |f(x)\left | \right |^2_2 + 2f(x)J(x)^T\Delta x + \Delta x^T J(x) J(x)^T \Delta x \right) \end{aligned}
21∣∣f(x)+J(x)TΔx∣∣2=21(f(x)+J(x)TΔx)T(f(x)+J(x)TΔx)=21(∣∣f(x)∣∣22+2f(x)J(x)TΔx+ΔxTJ(x)J(x)TΔx)
求上式关于
Δ
x
\Delta x
Δx的导数,并令其为0:
J
(
x
)
f
(
x
)
+
J
(
x
)
J
T
(
x
)
Δ
x
=
0
J(x)f(x)+J(x)J^T(x)\Delta x =0
J(x)f(x)+J(x)JT(x)Δx=0
可以得到以下方程组:
J
(
x
)
J
T
(
x
)
⏟
H
(
x
)
Δ
x
=
−
J
(
x
)
f
(
x
)
⏟
g
(
x
)
\underbrace{J(x)J^T(x)}_{H(x)}\Delta x=\underbrace{-J(x)f(x)}_{g(x)}
H(x)
J(x)JT(x)Δx=g(x)
−J(x)f(x)
这个方程是关于
Δ
x
\Delta x
Δx的线性方程组,我们称它为增量方程,也可以称为高斯牛顿方程或者正规方程.我们将左边的系数定义为H,右边定义为g,那么上式变为:
H
Δ
x
=
g
H\Delta x=g
HΔx=g
对比牛顿法可见,高斯牛顿法用更
J
J
T
JJ^T
JJT作为牛顿你发中二阶海塞矩阵的近似,从而省略了计算H的过程.求解增量方程是整个优化问题的核心所在,如果我们能顺利解出该方程,那么高斯牛顿的算法步骤可以写成:
1 给定初始值 x 0 x_0 x0;
2 对于第 k k k次迭代,求出当前的雅可比矩阵 J ( x K ) J(x_K) J(xK)和误差 f ( x k ) f(x_k) f(xk);
3 求解增量方程: H Δ x k = g ⟵ Δ x k = H − 1 g H\Delta x_k=g\ \longleftarrow \ \Delta x_k =H^{-1}g HΔxk=g ⟵ Δxk=H−1g;
4 若 Δ x \Delta x Δx足够小,则停止;否则,令 x k + 1 = x k + Δ x k x_{k+1}=x_k+\Delta x_k xk+1=xk+Δxk,返回第二步.
只要能求解增量方程,就能保证目标函数能够正确的下降.
为了求解增量方程,就需要求解 H − 1 H^{-1} H−1,这就需要 H H H矩阵可逆.但是实际数据中的 J J − 1 JJ^{-1} JJ−1只有半正定性,即可能出现 J J − 1 JJ^{-1} JJ−1为奇异矩阵(行列式为0)的情况.此时增量的稳定性较差.,导致算法不收敛. 直观地说,原函数在这个点地局部近似不像是一个二次函数.更严重的是,就算我们假设 H H H非奇异,如果我们求出来的步长 Δ x \Delta x Δx太大,也会导致局部近似不够准确,这样甚至无法保证它的迭代收敛,哪怕是让目标函数更大都是有可能的.
列文伯格-马夸尔特方法在一定程度上修正了高斯牛顿法的这些问题.
4.列文伯格-马夸尔特法
高斯牛顿属于先搜索方法:先找到方向,再确定长度
列-马 属于信赖区域方法,认为近似只再区域内可靠
高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近由较好的近似效果,但是这个"附近"应该怎么确定呢?,可以很自然地想到给 Δ x \Delta x Δx添加一个范围,称为信赖区域(Trust Region).**这个范围定义了在什么情况下二阶近似是有效的.而这个范围应该怎么确定呢?一个比较好的方法是根据近似模型和实际函数之间的差异来确定,如果差异小,说明近似效果好,可以扩大近似的范围;如果差异大,就缩小近似的范围.**我们定义一个指标来刻画近似的好坏程度:
ρ
=
f
(
x
+
Δ
x
)
−
f
(
x
)
J
(
x
)
−
1
Δ
x
\rho=\frac{f(x+\Delta x)-f(x)}{J(x)^{-1}\Delta x}
ρ=J(x)−1Δxf(x+Δx)−f(x)
ρ
\rho
ρ的分子式函数实际下降的数值,分母是近似模型下降的数值.如果
ρ
\rho
ρ接近1,则近似是好的;如果
ρ
\rho
ρ太小,说明实际减小的数值远小于近似减小的数值,说明近似比较差,需要缩小近似的范围;反之,如果
ρ
\rho
ρ比较大,说明实际下降的比预计的更大,可以放大近似范围.
于是可以构建一个改良版的非线性优化框架,该框架会比高斯牛顿法有更好的效果.
1 给定初始值 x 0 x_0 x0,以及初始优化半径 μ \mu μ;
2 对于第k次迭代,在高斯牛顿法的基础上加上信赖区域,求解:
m i n Δ x k 1 2 ∣ ∣ f ( x k ) + J ( x k ) T Δ x k ∣ ∣ 2 , s . t . ∣ ∣ D Δ x k ∣ ∣ 2 ≤ μ \underset{\Delta x_k}{min} \frac{1}{2} \left | \right | f(x_k)+J(x_k)^T \Delta x_k \left | \right |^2,\ \ s.t. \ \ \left | \right | D \Delta x_k \left | \right |^2 \leq \mu Δxkmin21∣∣f(xk)+J(xk)TΔxk∣∣2, s.t. ∣∣DΔxk∣∣2≤μ
其中, μ \mu μ是信赖区域的半径, D D D为系数矩阵
3 按照上上面的式子计算 ρ \rho ρ
4 若 ρ \rho ρ> 3 4 \frac{3}{4} 43,则设置 μ = 2 μ \mu=2\mu μ=2μ
5 若 ρ \rho ρ< 3 4 \frac{3}{4} 43,则设置 μ = 0.5 μ \mu=0.5\mu μ=0.5μ
6 如果 μ = 2 μ \mu=2\mu μ=2μ大于某阈值,则认为近似可行,令 x k + 1 = x k + Δ x k x_{k+1}=x_k+\Delta x_k xk+1=xk+Δxk
7 判断算法是否收敛. 如果不收敛则返回第二步,否则结束.
这里范围扩大的倍数和阈值都是经验值,可以替换称别的值.在上式中,我们将增量限定在一个半径为 μ \mu μ的球中,认为只有在这个球中才是有效的.带上 D D D之后，这个球可以看成一个椭球.在列文伯格提出的优化方法中,将 D D D取成单位阵,相当于直接将 Δ x \Delta x Δx约束在一个球中. 随后马夸尔特提出将 D D D取成非负数对角阵–实际中通常用 J T J J^TJ JTJ的对角元素平方根,使得在梯度最小的维度上约束范围更大一些.
对信赖区域的优化:可以利用拉格朗日乘子转化为无约束:
m
i
n
Δ
x
k
1
2
∣
∣
f
(
x
k
)
+
J
(
x
k
)
T
Δ
x
k
∣
∣
2
+
λ
2
∣
∣
D
Δ
x
∣
∣
2
\underset{\Delta x_k}{min} \frac{1}{2} \left | \right | f(x_k)+J(x_k)^T \Delta x_k \left | \right |^2 +\frac{\lambda}{2} \left | \right | D \Delta x \left | \right |^2
Δxkmin21∣∣f(xk)+J(xk)TΔxk∣∣2+2λ∣∣DΔx∣∣2
仍按照
G
−
N
G-N
G−N展开,增量方程为:
(
H
+
λ
D
T
D
)
Δ
x
=
g
(H+\lambda D^TD)\Delta x=g
(H+λDTD)Δx=g
在
L
e
v
e
n
b
e
r
g
Levenberg
Levenberg中,取
D
=
I
D=I
D=I,则:
(
H
+
λ
I
)
Δ
x
=
g
(H+\lambda I)\Delta x=g
(H+λI)Δx=g
可以看到,当参数
λ
\lambda
λ参数比较小时,
H
H
H占主要地位,说明二次近似模型在该范围内时比较好的,
L
e
v
e
n
b
e
r
g
−
M
a
r
q
u
a
d
t
Levenberg-Marquadt
Levenberg−Marquadt方法更接近
G
a
u
s
s
−
N
e
w
t
o
n
Gauss-Newton
Gauss−Newton方法,另一方面,当
λ
\lambda
λ比较大时,
λ
I
\lambda I
λI占据主要地位,
L
e
v
e
n
b
e
r
g
−
M
a
r
q
u
a
d
t
Levenberg-Marquadt
Levenberg−Marquadt更接近于一阶梯度下降法,这说明附近的二次近似不够好.
L
e
v
e
n
b
e
r
g
−
M
a
r
q
u
a
d
t
Levenberg-Marquadt
Levenberg−Marquadt的求解方式,可在一定程度上避免矩阵的非奇异和病态问题,提供更稳定,更准确的增量
Δ
x
\Delta x
Δx.
非线性优化小结:
- 主要方法:最速下降法,牛顿,G-N,L-M,Dogleg等
- 与线性优化不同,非线性需要针对具体问题具体分析
- 问题非凸时,对初值敏感,会陷入局部最优
- 目前没有非凸问题的通用最优值的寻找办法
- 问题凸时,二阶方法通常一两步就能收敛.
5. 实践:手写高斯牛顿法 g2o,Ceres库的使用
下面用一个简单的例子说明如何求解最小二乘问题.
考虑一条满足以下方程的曲线:
y
=
e
x
p
(
a
x
2
+
b
x
+
c
)
+
w
y=exp(ax^2+bx+c)+w
y=exp(ax2+bx+c)+w
其中,
a
,
b
,
c
a,b,c
a,b,c为曲线的参数,
w
w
w为高斯噪声,满足
w
∼
(
0
,
σ
2
)
w\sim(0,\sigma^2)
w∼(0,σ2).
假设我们有
N
N
N个关于
x
,
y
x,y
x,y的观测数据点,想根据这些数据点求出曲线的参数.那么可以求解最小二乘问题以估计曲线参数:
m
i
n
a
,
b
,
c
1
2
∑
i
=
1
n
∣
∣
y
i
−
e
x
p
(
a
x
i
2
+
b
x
i
+
c
)
∣
∣
2
\underset{a,b,c}{min} \frac{1}{2} \sum_{i=1}^{n} \left | \right | y_i-exp(ax_i^2+bx_i+c)\left | \right | ^2
a,b,cmin21i=1∑n∣∣yi−exp(axi2+bxi+c)∣∣2
在这个问题中,待估计的变量是
a
,
b
,
c
a,b,c
a,b,c,而不是
x
x
x. 我们的程序**先根据模型生成
x
,
y
x,y
x,y的真值,然后在真值中添加高斯分布的噪声.随后使用高斯牛顿法从带噪声的数据拟合参数模型.定义误差为:
e
i
=
y
i
−
e
x
p
(
a
x
i
2
+
b
x
i
+
c
)
,
e_i=y_i-exp(ax_i^2+bx_i+c),
ei=yi−exp(axi2+bxi+c),
那么,可以求出每个误差项对于状态变量的导数:
∂
e
i
∂
a
=
−
x
i
2
e
x
p
(
a
x
i
2
+
b
x
i
+
c
)
∂
e
i
∂
b
=
−
x
i
e
x
p
(
a
x
i
2
+
b
x
i
+
c
)
∂
e
i
∂
c
=
−
e
x
p
(
a
x
i
2
+
b
x
i
+
c
)
\begin{aligned} \frac{\partial e_i}{\partial a} &= -x_i^2exp(ax_i^2+bx_i+c) \\ \frac{\partial e_i}{\partial b} &= -x_iexp(ax_i^2+bx_i+c) \\ \frac{\partial e_i}{\partial c} &= -exp(ax_i^2+bx_i+c) \end{aligned}
∂a∂ei∂b∂ei∂c∂ei=−xi2exp(axi2+bxi+c)=−xiexp(axi2+bxi+c)=−exp(axi2+bxi+c)
于是:
J
i
=
[
∂
e
i
∂
a
,
∂
e
i
∂
b
,
∂
e
i
∂
c
]
T
J_i= \left [ \frac{\partial e_i}{\partial a},\frac{\partial e_i}{\partial b},\frac{\partial e_i}{\partial c} \right ]^T
Ji=[∂a∂ei,∂b∂ei,∂c∂ei]T
高斯牛顿法的增量方程为:
(
∑
i
=
1
100
J
i
(
σ
2
)
−
1
J
i
T
)
Δ
x
k
=
∑
i
=
1
100
−
J
i
(
σ
2
)
−
1
e
i
\left ( \sum_{i=1}^{100} J_i (\sigma^2)^{-1} J_i^T \right ) \Delta x_k = \sum_{i=1}^{100} -J_i (\sigma^2)^{-1} e_i
(i=1∑100Ji(σ2)−1JiT)Δxk=i=1∑100−Ji(σ2)−1ei
代码实践:
首先,我们先制造一些观测值:
取
a
=
1
,
b
=
2
,
c
=
1
a=1,b=2,c=1
a=1,b=2,c=1,即
y
=
e
x
p
(
x
2
+
2
x
+
1
)
y=exp(x^2+2x+1)
y=exp(x2+2x+1),这个是真实的数据,然后取
x
=
{
0.1
,
0.2
,
.
.
.
0.99
,
1
}
x=\{0.1,0.2,...0.99,1\}
x={0.1,0.2,...0.99,1}共100个值,然后取相应的y的值,不过这里的y的值需要加上高斯噪声以用来拟合:
int main(int argc, char **argv) {
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma;
cv::RNG rng; // OpenCV随机数产生器
vector<double> x_data, y_data; // 数据
for (int i = 0; i < N; i++) {
double x = i / 100.0;
x_data.push_back(x);
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
}
...
...
}
观测值都被放在了x_data以及y_data中
接下来进行高斯牛顿法的迭代:
给一个初值,这里给的初值是
a
=
2
,
b
=
−
1
,
c
=
5
a=2,b=-1,c=5
a=2,b=−1,c=5
首先求出雅可比矩阵,即
J
i
=
[
∂
e
i
∂
a
,
∂
e
i
∂
b
,
∂
e
i
∂
c
]
T
J_i= \left [ \frac{\partial e_i}{\partial a},\frac{\partial e_i}{\partial b},\frac{\partial e_i}{\partial c} \right ]^T
Ji=[∂a∂ei,∂b∂ei,∂c∂ei]T,然后根据
J
i
J_i
Ji求解H以及g(代码中的b)
int main(int argc, char **argv) {
......
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值
......
// 开始Gauss-Newton迭代
int iterations = 100; // 迭代次数
double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的cost
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
for (int iter = 0; iter < iterations; iter++) {
Matrix3d H = Matrix3d::Zero(); // Hessian = J^T W^{-1} J in Gauss-Newton
Vector3d b = Vector3d::Zero(); // bias
cost = 0;
for (int i = 0; i < N; i++) {
double xi = x_data[i], yi = y_data[i]; // 第i个数据点
double error = yi - exp(ae * xi * xi + be * xi + ce);
Vector3d J; // 雅可比矩阵
J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce); // de/da
J[1] = -xi * exp(ae * xi * xi + be * xi + ce); // de/db
J[2] = -exp(ae * xi * xi + be * xi + ce); // de/dc
H += inv_sigma * inv_sigma * J * J.transpose();
b += -inv_sigma * inv_sigma * error * J;
cost += error * error;
}
.......
}
求解增量方程 H Δ x = g H\Delta x=g HΔx=g:
int main(int argc, char **argv) {
......
// 求解线性方程 Hx=b
Vector3d dx = H.ldlt().solve(b);
if (isnan(dx[0])) {
cout << "result is nan!" << endl;
break;
}
if (iter > 0 && cost >= lastCost) {
cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
break;
}
ae += dx[0];
be += dx[1];
ce += dx[2];
......
}
完整代码如下:
#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
int main(int argc, char **argv) {
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma;
cv::RNG rng; // OpenCV随机数产生器
vector<double> x_data, y_data; // 数据
for (int i = 0; i < N; i++) {
double x = i / 100.0;
x_data.push_back(x);
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
}
// 开始Gauss-Newton迭代
int iterations = 100; // 迭代次数
double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的cost
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
for (int iter = 0; iter < iterations; iter++) {
Matrix3d H = Matrix3d::Zero(); // Hessian = J^T W^{-1} J in Gauss-Newton
Vector3d b = Vector3d::Zero(); // bias
cost = 0;
for (int i = 0; i < N; i++) {
double xi = x_data[i], yi = y_data[i]; // 第i个数据点
double error = yi - exp(ae * xi * xi + be * xi + ce);
Vector3d J; // 雅可比矩阵
J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce); // de/da
J[1] = -xi * exp(ae * xi * xi + be * xi + ce); // de/db
J[2] = -exp(ae * xi * xi + be * xi + ce); // de/dc
H += inv_sigma * inv_sigma * J * J.transpose();
b += -inv_sigma * inv_sigma * error * J;
cost += error * error;
}
// 求解线性方程 Hx=b
Vector3d dx = H.ldlt().solve(b);
if (isnan(dx[0])) {
cout << "result is nan!" << endl;
break;
}
if (iter > 0 && cost >= lastCost) {
cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
break;
}
ae += dx[0];
be += dx[1];
ce += dx[2];
lastCost = cost;
cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
"\t\testimated params: " << ae << "," << be << "," << ce << endl;
}
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
return 0;
}
运行结果:
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
