《视觉SLAM-非线性优化与g2o-高翔》笔记

《视觉SLAM-非线性优化与g2o-高翔》笔记

本文是观看视频《视觉SLAM-非线性优化与g2o-高翔》整理的笔记。

1. 从滤波到优化

1.1 模型

状态变量$x$： $x=[x_1,x_2,...,x_N,y_1,y_2,...,y_M]$
运动模型$f$： $x_k=f(x_{k-1},u_k)+v_k$
观测模型$g$： $z_{k,j}=g(x_k,y_j)+n_{k,j}$
其中，$x_i$为位姿，$y_i$为路标坐标，都是未知量。$u$为运动输入量，$z$为观测值，都是测量值。

1.2 传统EKF方法

在工作点附近计算雅可比矩阵（一阶偏导）$F$ 和 $G$。
$$\begin{gathered} F=:\frac{\delta f}{\delta x_k}|{\hat{x}}_{k-1}{v}_k\sim N(0,Q_k) \\ G=:\frac{\delta g}{\delta x_k}|{\hat{x}}_k{n}_{k,j}\sim N(0,R_k) \end{gathered}$$

step1. 预测：估计先验的均值和协方差矩阵
$${\breve{x}}_k=f({\hat{x}}_{k-1},u_k){\breve{P}}_{k+1}=F_k{\hat{P}}_k{F}_k^T+Q_k$$

step2. 更新：计算卡尔曼增益，并用观测模型纠正先验模型，得到后验估计
$$\begin{gathered} K_k={\breve{P}}_k{G}_k^T(G_k{\breve{P}}_k{G}_k^T+R_k{)}^{-1} \\ {\hat{x}}_k={\breve{x}}_k+K_k(z-\hat{z}){\hat{P}}_k=(i-K{G}_k){\breve{P}}_k \end{gathered}$$

EKF的问题：
线性化误差，无迭代，噪声非高斯，需要维护一个大的协方差矩阵（平方级别复杂度）。
解决思路=>使用非线性优化

1.3 非线性优化思路

思路	问题
1. 设定目标函数，选定初值 状态向量：$x=[x_1,x_2,...,x_N,y_1,y_2,...,y_M]$	目标函数是什么？
2. 寻找梯度，使目标函数下降	梯度如何计算？
3. 梯度下降，迭代直到收敛	梯度只是局部下降，如何保证目标函数下降？

1.4 目标函数

在视觉和激光SLAM中，运动模型差别较小，观测模型则有所差异。这里我们以观测模型为例。
观测模型： $z_{k,j}=g(x_k,y_j)+n_{k,j}{n}_{k,j}\sim N(0,\Sigma )$
误差分布： $z_{k,j}\sim N(g(x_k,y_j),\Sigma )$

以Bayers方式理解，优化的目标是：在已知观测值$z$的条件下，求能使$P(x,y|z)$概率大最的$xy$。即求：

$$(x,y{)}^{\ast }=\underset{x,y}{\mathrm{argmax}}P(x,y|z)$$

根据贝叶斯方程，后验与似然×先验成正比。$P(x,y|z)\propto P(z|x,y)P(x,y)$
则有两种方式求解 $xy$：

• 最大似然（MLE）：$(x,y{)}^{\ast }=\underset{x,y}{\mathrm{argmax}}P(z|x,y)$
• 最大后验（MAP）：$(x,y{)}^{\ast }=\underset{x,y}{\mathrm{argmax}}P(z|x,y)P(x,y)$

一般不知道路标的先验位置，所以用MLE多一些。

怎么求MLE呢？
我们会使用负对数简化目标函数。

这么做是因为，高斯分布函数有如下性质：取负对数后，可以分为两部分，前一部分与$z$无关，求极值时可以不考虑；而后一部分的结构比变换之前简单了很多。

P ( z ) = − l n {    1 ( 2 π ) N d e t Σ exp ⁡ [ 1 2 ( x − μ ) T Σ − 1 ( x − μ ) ]    } = 1 2 l n [ ( 2 π ) N d e t Σ ] + 1 2 [ ( x − μ ) T Σ − 1 ( x − μ ) ] P(z) = -ln \{ \; \frac{1}{ {(2\pi)}^N det\Sigma} \exp[\frac{1}{2}(x-\mu)^T \Sigma^{-1} (x-\mu) ] \; \} \\ =\frac{1}{2} ln[{(2\pi)}^N det\Sigma] + \frac{1}{2} [(x-\mu)^T \Sigma^{-1} (x-\mu)] P(z)=−ln{ (2π)NdetΣ1​exp[21​(x−μ)TΣ−1(x−μ)]}=21​ln[(2π)NdetΣ]+21​[(x−μ)TΣ−1(x−μ)]

对变量求极大值，相当于求负对数的极小值。则求下式即可达到最大似然：
$$\begin{array}{rl}\underset{x,y}{\mathrm{argmax}}P(z|x,y)& =\underset{x,y}{\mathrm{argmin}}-ln(P(z|x,y))\\ & =\underset{x,y}{\mathrm{argmin}}(z-\mu {)}^T{\Sigma }^{-1}(z-\mu )\end{array}$$

我们知道 $z_{k,j}\sim N(g(x_k,y_j$