前言
高翔博士的这本视觉SLAM十四讲这本书，作为要入门的我来说，起到了很好的引导作用，但是在第六章节非线性优化这块，即使我看了他的视频，书也看了两遍，我仍然是懵懵懂懂，跟据书上介绍的非线性优化理论，即无法深入理解又没法把这个优化理论应用与实际程序应用场景，最终决定先放下这本书，在百度百科中对本章的非线性优化理论进行重新回炉，对推到的每一步都在草稿纸上进行推导，才对非线性优化理论的精髓得以了解，然后在实践应用中对有噪声的曲线进行拟合，得出的结论也验证了非线性优化的可行性。在这也建议小伙伴们在学习视觉SLAM这本书时，尤其关于数学方面的知识时，多百度一下此方面的一些介绍,理解后在看真这本书，わかりました。
状态估计问题
批量状态估计与最大后验估计
假设在
x
k
x_k
xk处对路标进行一次观测，对应到图像上的像素位置
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)
K为相机内参，s为像素点的距离。假设在相机运动过程中受到噪声
w
k
,
v
k
,
j
w_k,v_{k,j}
wk,vk,j影响，在这种情况下我们接受到的是带有噪声的运动传感器输入值（
u
k
u_k
uk)和像素位置(
z
k
,
j
z_{k,j}
zk,j)
{
x
k
=
f
(
x
k
−
1
,
u
k
)
+
w
k
z
k
,
j
=
h
(
y
j
,
x
k
)
+
v
k
,
j
\begin{cases} x_k = f(x_{k-1},u_k)+w_k\\z_{k,j}=h(y_j,x_k)+v_{k,j} \end{cases}
{xk=f(xk−1,uk)+wkzk,j=h(yj,xk)+vk,j
通过带噪声的z和来推断位姿x和地图y(以及他们的概率分布）
在已知传感器数据u和观测数据u的条件下，利用贝叶斯法则对下(x,y)进行条件概率分布
P
(
x
,
y
∣
z
,
u
)
=
P
(
z
,
u
∣
x
,
y
)
P
(
x
,
y
)
P
(
z
,
u
)
P(x,y|z,u)=\frac{P(z,u|x,y)P(x,y)}{P(z,u)}
P(x,y∣z,u)=P(z,u)P(z,u∣x,y)P(x,y)
直接求得其后验概率比较困难，我们可以求一个状态的最有估计，使得在该状态下后验概率的大化：
（
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
)
（x,y)^*_{MAP}=arg max P(x,y|z,u)=arg max {P(z,u|x,y)P(x,y)}
（x,y)MAP∗=argmaxP(x,y∣z,u)=argmaxP(z,u∣x,y)P(x,y)
当我们去掉先验概率时，就变成求解最大似然估计
（
x
,
y
)
M
L
E
∗
=
a
r
g
m
a
x
P
(
z
,
u
∣
x
,
y
)
（x,y)^*_{MLE}=arg max {P(z,u|x,y)}
（x,y)MLE∗=argmaxP(z,u∣x,y)
通俗的将就是在什么样的状态下，最有可能产生现在观测到的数据。
最小二乘的引出
对上式最大似然估计的求解，可以通过有效简单的形式解得。
对于某一次的观测：
z
k
,
j
=
h
(
y
i
,
x
k
)
+
v
k
,
j
z_{k,j}= h(y_i,x_k)+v_{k,j}
zk,j=h(yi,xk)+vk,j,其中噪声项
v
k
v_k
vk~
N
(
0
,
Q
k
,
j
)
N(0,Q_{k,j})
N(0,Qk,j)
所以观测数据的条件概率
P
(
z
k
,
j
∣
x
k
,
y
i
)
=
N
(
h
(
y
i
,
x
k
)
,
Q
k
,
j
)
P(z_{k,j}|x_k,y_i)=N(h(y_i,x_k),Q_{k,j})
P(zk,j∣xk,yi)=N(h(yi,xk),Qk,j)
仍然是一个零均值为
h
(
y
i
,
x
k
)
h(y_i,x_k)
h(yi,xk)的高斯分布
通过最小化负对数来对这个高斯分布的观测模型的最大似然进行求解
(
x
k
,
y
j
)
∗
=
a
r
g
m
a
x
P
(
x
,
y
∣
z
)
=
a
r
g
m
a
x
P
(
z
∣
x
,
y
)
=
a
r
g
m
a
x
P
(
z
j
,
k
∣
x
k
,
y
j
)
(x_k,y_j)^*=argmaxP(x,y|z)=argmaxP(z|x,y)=argmaxP(z_{j,k}|x_k,y_j)
(xk,yj)∗=argmaxP(x,y∣z)=argmaxP(z∣x,y)=argmaxP(zj,k∣xk,yj)
=
a
r
g
m
a
x
N
(
h
(
y
i
,
x
k
)
,
Q
k
,
j
)
=
a
r
g
m
i
n
(
(
z
−
h
(
x
,
y
)
)
T
Q
−
1
(
z
−
h
(
x
,
y
)
)
)
=argmaxN(h(y_i,x_k),Q_{k,j})=argmin( (z-h(x,y))^T Q^{-1} (z-h(x,y)))
=argmaxN(h(yi,xk),Qk,j)=argmin((z−h(x,y))TQ−1(z−h(x,y)))
现在得出观测模型的最大似然估计，下面我们考虑批量信息时刻的数据，假设各时刻的输入和观测是相互独立的，于时我们可以对联合分布进行因式分解
P
(
z
,
u
∣
x
,
y
)
=
∏
k
P
(
u
k
∣
x
k
−
1
,
x
k
)
∏
k
,
j
P
(
z
k
,
j
∣
x
k
,
y
j
)
P(z,u|x,y)=\prod_kP(u_k|x_{k-1},x_k)\prod_{k,j}P(z_{k,j}|x_k,y_j)
P(z,u∣x,y)=k∏P(uk∣xk−1,xk)k,j∏P(zk,j∣xk,yj)
定义各次输入和观测数据与模型之间的误差
e
u
,
k
=
x
k
−
f
(
x
k
−
1
,
u
k
)
e_{u,k}=x_k-f(x_{k-1},u_k)
eu,k=xk−f(xk−1,uk)
e z , j , k = z k , j − h ( x k , y i ) e_{z,j,k}= z_{k,j}-h(x_k,y_i) ez,j,k=zk,j−h(xk,yi)
所有时刻估计值与真实读数之间的马氏距离，等价于求最大似然估计
m
i
n
J
(
x
,
y
)
=
∑
k
e
u
,
k
T
R
k
−
1
e
u
,
k
+
∑
k
∑
j
e
z
,
k
,
j
T
Q
k
,
j
−
1
e
z
,
k
,
j
二
次
型
minJ(x,y) =\sum_ke^T_{u,k}R^{-1}_{k} e_{u,k}+\sum_k\sum_je^T_{z,k,j}Q^{-1}_{k,j}e_{z,k,j} \text 二次型
minJ(x,y)=k∑eu,kTRk−1eu,k+k∑j∑ez,k,jTQk,j−1ez,k,j二次型
由于噪声的存在，当我们把估计的轨迹与与地图带入SLAM的运动，观察测方程中，他们不会完美的成立，此时我们需要进行对状态的估计值进行微调，通过不断的迭代，使得误差下降到允许范围内。
非线性最小二乘
求 m i n F ( x ) = ∣ ∣ ∣ f ( x ) ∣ ∣ 2 2 {min}F(x)=|||f(x)||^2_2 minF(x)=∣∣∣f(x)∣∣22
- 给定某个参数的初始值 x 0 x_0 x0
- 对于第k次迭代，寻找一个增量 Δ x k \Delta x_k Δxk,使得 m i n F ( x + Δ x k ) = ∣ ∣ ∣ f ( x + Δ x k ) ∣ ∣ 2 2 {min}F(x+\Delta x_k)=|||f(x+\Delta x_k)||^2_2 minF(x+Δxk)=∣∣∣f(x+Δxk)∣∣22达到极小值
- 若 Δ x k 足 够 小 ， 则 停 止 \Delta x_k足够小，则停止 Δxk足够小，则停止
- 否侧，令 x k + 1 = x k + Δ x k , 返 回 第 2 步 x_{k+1}=x_k+\Delta x_k,返回第2步 xk+1=xk+Δxk,返回第2步
一阶、二阶梯度法
当执行到第k次迭代时，对目标函数在第
x
k
x_k
xk处附近进行泰勒级数展开：
F
(
x
k
+
Δ
x
k
)
≈
F
(
x
k
)
+
J
(
x
k
)
T
Δ
x
k
F(x_k+\Delta x_k)\approx F(x_k)+J(x_k)^T \Delta x_k
F(xk+Δxk)≈F(xk)+J(xk)TΔxk
对于求
m
i
n
F
(
x
)
minF(x)
minF(x)我们可以取
Δ
x
k
=
−
J
(
x
k
)
\Delta x_k = -J(x_k)
Δxk=−J(xk),这么我们的函数值下降的最快，也称最速下降法
当然我们可以对目标函数在
x
k
x_k
xk处Taylor展开至二阶
F
(
x
k
+
Δ
x
k
)
≈
F
(
x
k
)
+
J
(
x
k
)
T
Δ
x
k
+
1
2
Δ
x
T
H
Δ
x
F(x_k+\Delta x_k)\approx F(x_k)+J(x_k)^T \Delta x_k+\frac {1}{2}\Delta x^TH\Delta x
F(xk+Δxk)≈F(xk)+J(xk)TΔxk+21ΔxTHΔx
我们对右侧关于
Δ
x
\Delta x
Δx求导，并令其导数为令，得到其极值
J
+
H
Δ
x
=
0
⟹
H
Δ
x
=
−
J
J+H\Delta x = 0 \implies H \Delta x = -J
J+HΔx=0⟹HΔx=−J
求解这个方程组就得到了增量
Δ
x
\Delta x
Δx的值，该方法也称为牛顿法
下面的话如果增量足够下则停止，否则进行再次迭代。
高斯牛顿法
高斯牛顿法呢，时从母式进行改动
a
r
g
m
i
n
∣
∣
f
(
x
)
+
J
(
x
)
T
Δ
x
∣
∣
2
argmin||f(x)+J(x)^T \Delta x||^2
argmin∣∣f(x)+J(x)TΔx∣∣2把这个式子展开后，然后关于这个
Δ
x
\Delta x
Δx求导，并令其为0得到这个式子
J
(
x
)
f
(
x
)
+
J
(
x
)
J
(
x
)
T
Δ
x
=
0
J(x)f(x)+J(x)J(x)^T\Delta x = 0
J(x)f(x)+J(x)J(x)TΔx=0
我们称它为高斯牛顿方程
⟹
H
Δ
x
=
g
\implies H\Delta x= g
⟹HΔx=g
然后求解出增量
Δ
x
\Delta x
Δx,若其增量足够小
…
\ldots
…
下图是我将得到的数据在Matlab成的图
892
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
