视觉SLAM十四讲-第六讲笔记

状态估计问题

SLAM模型：

• 状态方程：$x_k=f(x_{k-1},u_k)+w_k$
• 运动方程：$z_{k,j}=h(y_j,x_k)+v_{k,j}$

其中:

• x：相机位姿。可以由旋转矩阵$T$或者李代数$exp(ϵ$^$)$表示。
• u：传感器读数
• z：观测值。二维像平面下的齐次坐标。
• y：实际物体。三维坐标下的齐次坐标。
• w、v：噪声

观测方程：
$s{z}_{k,j}=Kexp(ϵ$^$)y_j$
其中，$exp(ϵ$^$)$是相机位姿，即外参。$exp(ϵ$^$)y_j$把三维世界中的点转换到相机坐标系下，K是内参，把相机坐标系下的结果转换到像平面。s为像素点的距离。

优化问题：在存在噪声$w$和$v$的情况下，推断位姿$x$和地图$y$及其状态分布。 -> 状态估计问题。
使用非线性优化求解。

状态变量：$x=x_1,...,x_N,y_1,...,y_M$。所有待估计的变量。
已知：输入读数$u$，观测值$z$。（带噪声）
目标：$P(x|z,u)$，只考虑$z$时：$P(x|z)$

贝叶斯法则

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

• $p(x|z)$：后验。已知后面的条件
• $p(z|x)$：似然。已知前面的概率，求条件x下z的发生概率(相似性)
• $p(z)$：先验。已知的独立概率

最大后验概率（MAP）

求使在z条件下，得$p(x|z)$最大的x。
状态估计：在已有观测值的条件下，使得在此条件下位姿估计和场景估计概率最大化的估计。

最大似然概率（MLE）

$p(z)$是定值，和待估计量无关。最大化$p(x|z)$相当于最大化似然和先验的乘积。在$p(x)$未知，没有先验的情况下，即是求解似然$p(z|x)$的最大值。
状态估计：在什么样的位姿和场景下，最能够得到当前的观测值。

最小二乘求解MLE

在高斯分布假设下，对于某次观测：KaTeX parse error: Can't use function '$' in math mode at position 27: …_j,x_k)+v_{k,j}$̲，噪声$v_{k,j}$~ $…。
则:
$p(z|x)=N(h(y_j,x_k),Q_{k,j})$，似然概率也是一个高斯分布。
对其求负对数，最大化MLE相当于最小化其负对数。
$$x\ast =argmin((z_{k,j}-h(x_k,y_j{)}^T{Q}_{k,j}^{-1}(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)$

最小化负对数即最小化观测误差的平方。误差不是噪声！！
将观测误差和位姿估计误差的平方加和，得到：
$$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}$$

最小二乘问题！其最优解等于MLE估计，等于MAP的估计。由于噪声存在，对状态估计值微调，使整体误差下降，达到一个极小值。引入约束，是一个非线性优化问题。

• 目标函数：min位姿估计和观测估计误差的平方和。每个误差项仅与一两个状态变量有关，各自是要给小规模约束。
• 参数块：每个误差项对应的参数变量。
• 稀疏性：整体误差由小型误差构成。增量方程稀疏。
• 约束：若使用李代数，无约束。若使用变换矩阵，有变换矩阵自身约束。
• 误差：二范数。

求解最小二乘

思想：对于$f(x)$，寻找增量$▲x$，使得$f(x+▲x)$最小。这里默认$▲x$模长确定，只寻找它的下降方向。

梯度法

**目标函数$||f(x+▲x)|{|}_2^2$**在x附近泰勒展开。

$$||f(x+▲x)|{|}_2^2\approx ||f(x)|{|}_2^2+J(x)▲x+1/2▲x^{T}H▲x$$

• $J(x)$：雅可比矩阵。$||f(x)|{|}^2$关于$x$的一阶导。
• $H(x)$：海塞矩阵。$||f(x)|{|}^2$关于$x$的二阶导。

一阶梯度法

仅保留一阶梯度。即负梯度，最速下降法
增量方向：$▲x\ast =-J(x{)}^T$
有时步长过大，会出现震荡

二阶梯度法

保留至二阶梯度。**又称牛顿法 **
增量方向：$$▲x\ast =argmin||f(x)|{|}_2^2+J(x)▲x+1/2▲x^{T}H▲x$$
另右侧为0，解得：
$$H▲x=-J^T$$
需要求解H。求解H计算开销过大

高斯牛顿法

思想：将 $f(x)$ 进行泰勒展开。
$$f(x+▲x)\approx f(x)+J(x)▲x$$
增量方向：：$$▲x\ast =argmin||f(x+▲x)|{|}_2^2\approx argmin||f(x+J(x)▲x)|{|}^2$$
解得：
$$J(x{)}^{T}J(x)▲x=-J(x{)}^{T}f(x)$$
$J(x{)}^{T}J(x)$是$H$的近似。计算开销小。但只有半正定，不保证收敛
**线搜索法：**高斯牛顿法中步长为1，在确定方向后寻找步长，为线搜索。

阻尼牛顿法

即信赖域法。
信赖域：展开点x附近的区域，用于保证泰勒展开是否有效。在信赖域内的近似有效，超出无效。
确定范围：根据近似和实际之间的差异确定。差异小可扩大，差异大就缩小。
定义差异：
$$p=\frac{f(x+▲x)-f(x)}{J(x)▲x}$$
分子：实际差异
分母：泰勒展开的近似差异

• p接近1：好的近似
• p过小：近似差，缩小信赖域
• p过大：近似不完全，放大信赖域

求解：
$$mi{n}_{▲x}1/2||f(x)+J(x)▲x)|{|}^2,s.t.||D▲x|{|}^2<=u$$
D是限定增量的椭球(信赖域)
使用拉格朗日乘子，将有约束问题转化为无约束问题。

增量方程：
$$(H+\lambda D^{T}D)▲x=g$$
$g$是目标误差$f(x)$
$\lambda$较小时，近似好，接近高斯牛顿法。$\lambda$较大，近似不好，接近随俗下降法。