# 视觉SLAM笔记(三)-非线性优化

SLAM问题中，机器人自身的状态估计主要有两种方式: 基于滤波器的方法(Kalman filter, Particle filter等)和基于非线性优化的方法，目前主流的SLAM方案均使用了非线性优化方法

## 状态估计

${x}_{k}=f\left({x}_{k-1}, {u}_{k}\right)+{w}_{k}\\ {z}_{k, j}=h\left({y}_{j}, {x}_{k}\right)+{v}_{k, j}$

$P(x|z)=\frac{P(z|x)P(x)}{P(z)}$

${x}^{*}_{M A P}=\arg \max P({x} | {z})=\arg \max P({z} | {x}) P({x})$

$x^*_{MLE}=\arg \max P(z|x)$

$P\left(z_{j, k} | {x}_{k}, {y}_{j}\right)=N\left(h\left({y}_{j},{x}_{k}\right), {Q}_{k, j}\right)$

$P({x})=\frac{1}{\sqrt{(2 \pi)^{N} \operatorname{det}({\Sigma})}} \exp \left(-\frac{1}{2}({x}-{\mu})^{T} {\Sigma}^{-1}({x}-{\mu})\right)$

$-\ln (P({x}))=\frac{1}{2} \ln \left((2 \pi)^{N} \operatorname{det}({\Sigma})\right)+\frac{1}{2}({x}-{\mu})^{T} {\Sigma}^{-1}({x}-{\mu})$

${x}^{*}=\arg \min \left(\left({z}_{k, j}-h\left({x}_{k}, {y}_{j}\right)\right)^{T} {Q}_{k, j}^{-1}\left({z}_{k, j}-h\left({x}_{k},{y}_{j}\right)\right)\right)$

$e_{v, k}=x_{k}-f\left(x_{k-1}, u_{k}\right)\\ e_{y, j, k}=z_{k, j}-h\left(x_{k}, y_{j}\right)$

$J({x})=\sum_{k}{e}_{v, k}^{T}{R}_{k}^{-1}{e}_{v, k}+\sum_{k} \sum_{j} {e}_{y, k, j}^{T} {Q}_{k, j}^{-1} {e}_{y, k, j}$

## 非线性优化

1. 给定初值$x_0$
2. 对于第$k$次迭代，寻找一个增量$\Delta x_k$，使得$\left\|f\left({x}_{k}+\Delta {x}_{k}\right)\right\|_{2}^{2}$达到极小值
3. $\Delta x_k$足够小则停止
4. 否则$x_{k+1}=x_{k}+\Delta x_{k}$，返回2
步骤内的主要问题在于如何寻找$\Delta x$

### 一阶和二阶梯度法

$\|f({x}+\Delta {x})\|_{2}^{2} \approx\|f({x})\|_{2}^{2}+{J}({x}) \Delta {x}+\frac{1}{2} \Delta {x}^{T} {H} \Delta {x}$

#### 一阶梯度法

$\|f({x}+\Delta {x})\|_{2}^{2} -\|f({x})\|_{2}^{2}={J}({x}) \Delta {x}$

#### 二阶梯度法

$H \Delta x = -J^T(x)$

### Gauss-Newton

$f(x+\Delta x) \approx f(x)+J(x) \Delta x$

\begin{aligned} \|f({x})+{J}({x}) \Delta {x}\|^{2} &=(f({x})+{J}({x}) \Delta {x})^{T}(f({x})+{J}({x}) \Delta {x}) \\ &=\left(\|f({x})\|_{2}^{2}+2 f({x})^{T}{J}({x}) \Delta {x}+\Delta {x}^{T} {J}({x})^{T} {J}({x}) \Delta {x}\right) \end{aligned}

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

### Levenberg-Marquardt Method

Levenberg-Marquardt方法主要是解决高斯牛顿法的缺点，其增量计算方程如下:
$(J(x)^{T} J(x)+\lambda D^TD) \Delta x=-J(x)^{T} f(x)$
Levenberg将$D$取为单位阵$I$，将$\Delta x$约束在一个球形范围内，而Marquardt将$D$取为非负数对角阵(通常用$J^TJ$的对角元素平方根表示)，使得在梯度小的维度上约束范围大
$\lambda$较小时，该方法更类似于高斯牛顿法，而$\lambda$较大时则更类似于最速下降法。

## 参考文献

[1] 高翔, 张涛, 颜沁睿, 刘毅, 视觉SLAM十四讲：从理论到实践, 电子工业出版社, 2017
[2] https://vision.in.tum.de/teaching/ss2019/mvg2019

©️2019 CSDN 皮肤主题: 编程工作室 设计师: CSDN官方博客