视觉SLAM14讲学习笔记-ch6

YJD-Climb

于 2024-09-09 16:13:37 发布

阅读量374

点赞数 3

文章标签：笔记概率论计算机视觉

本文链接：https://blog.csdn.net/shuaibidebei/article/details/142056989

版权

本博客主要记录学习高翔老师视觉SLAM14讲的总结笔记，若有总结不对的地方，欢迎大家与我讨论。

一、状态估计

由于总是存在噪声，通过z,u来推导x,y的过程将会成为一个状态估计的问题，解决该问题的方式有两种：滤波器和批量。
滤波器仅关心当前时刻的位姿 $x_{k}$
批量方法可以在更大范围达到优化
通过一系列概率学推导，可得到求解x,y的最优解为最大似然估计，即

$(x,y)^{*}_{MLE}=\arg\max P(z,u\mid x,y)$

上式可直观解释为当x,y达到什么状态时，产生的观测数据更接近真实观测数据。

二、最小二乘法

最小二乘法用概率学公式表示为：

$P(z_{j,k}\mid x_{k},y_{j})=N(h(y_{j},x_{k}),Q_{k,j})$

$\Rightarrow (x_{k},y_{j})^{*}=\arg\max N(h(y_{j},x_{k}),Q_{k,j})$

$\Rightarrow (x_{k},y_{j})^{*}=\arg\min (z_{k,j}-h(x_{k},y_{j}))^{T}Q_{k,j}^{-1}(z_{k,j}-h(x_{k},y_{j}))$

由此，可推导出有关状态估计的最小二乘函数：

$\left\{\begin{matrix} e_{u,k}=x_{k}-f(x_{k-1},u_{k})\\ e_{z,j,k}=z_{k,j}-h(x_{k},y_{j})\\ \min J(x,y)=\sum_{k}e_{u,k}^{T}R^{-1}e_{u,k}+\sum_{k}\sum_{j} e_{z,k,j}^{T}Q_{k,j}^{-1}e_{z,k,j} \end{matrix}\right.$

通过非线性优化方法，微调代价函数中待优化项，使得整体代价变小。

三、非线性最小二乘

对于下式：

$\min_{x}F(x)=\frac{1}{2} \left \| f(x) \right \|_{2}^{2}$

求解，一般 $\frac{\mathrm{d} F}{\mathrm{d} x} =0$ ，即可求得极小点，但对于很多 $F(x)$ 的形式过于复杂，直接求解的方法比较困难，所以可采用迭代的方法求解。

3.1一阶和二阶梯度法

将 $F(x)$ Taylar展开：

$F(x_{k}+\bigtriangleup x_{k})\approx F(x_{k})+J(x_{k})^{T}\bigtriangleup x_{k}+\frac{1}{2} \bigtriangleup x_{k}^{T}H(x_{k})\bigtriangleup x_{k}$

对于上式线性化后求极值，若保留一阶梯度，则为最速下降法：

$\bigtriangleup x^{*}=-J(x_{k})$

若保留到二阶梯度，则为牛顿法：

$H(x_{k})\bigtriangleup x^{*}=-J(x_{k})$

3.2高斯牛顿法

若将 $f(x)$ Taylar展开：

$f(x+\bigtriangleup x)\approx f(x)+J(x)^{T}\bigtriangleup x$

注意，这与一阶二阶梯度法不同，这里的雅克比矩阵 $J(x)$ 是对 $f(x)$ 的一阶导数。由此，求解梯度最优值可转化为：

$\bigtriangleup x^{*}=\arg\min\frac{1}{2} \left \| f(x)+J(x^{T})\bigtriangleup x \right \| ^{2}$

对上式右边展开，并对 $\bigtriangleup x$ 求导可得：

$J(x)J(x)^{T}\bigtriangleup x=-J(x)f(x)$

上式可称为增量方程、高斯牛顿方程或正规方程。缺点是 $J(x)J(x)^{T}$ 不容易求逆，会导致出现病态矩阵。

3.3列文伯格-马夸尔特方法

指标 $\rho$ （信赖区域）来刻画近似模型和实际模型之间的差异：

$\rho =\frac{f(x+\bigtriangleup x)-f(x)}{J(x)^{T}\bigtriangleup x}$

当 $\rho$ 越接近1，表示近似效果越好，小于1，表示近似效果较差，大于1，需要扩大近似范围。由此，可约束 $\bigtriangleup x$ 的求解范围，即：

$\min_{\bigtriangleup x}\frac{1}{2} \left \| f(x_{k})+J(x_{k})^{T}\bigtriangleup x \right \| ^{2} s.t\left \| D\bigtriangleup x \right \| ^{2}\le u$

若 $\rho>\frac{3}{4}$ ，u=2u，若 $\rho<\frac{3}{4}$ ，u=0.5u，将上述待约束的求解最优函数转换为无约束最优问题：

$L(\bigtriangleup x.\lambda )=\frac{1}{2} \left \| f(x_{k})+J(x_{k})^{T}\bigtriangleup x \right \| ^{2} +\frac{\lambda }{2} (\left \| D\bigtriangleup x \right \|^{2}-u )$