【视觉SLAM十四讲】非线性优化（附Ceres和g2o的安装）

本文为视觉 SLAM 学习总结。

在给定观测数据 $z_{k,j}$ 时，如何估计状态变量 $x_k,y_j$，即同时定位和建图

本讲内容概要

• 最小二乘法的含义和处理方式
• Gauss-Newton，Levenburg-Marquadt 等下降策略
• Ceres 库和 g2o 库的基本使用方法

状态估计问题

最大后验与最大似然

回顾 SLAM 方程：
$$\{\begin{array}{ll}{x}_k=f(x_{k-1},u_k)+w_k& \\ z_{k,j}=h(y_j,x_k)+v_{k,j}& \end{array}$$
其中

• 相机位姿：$x_k=T_k=exp({\xi }_k)$
• 像素：$s{z}_{k,j}=Kexp(\hat{\xi })y_j$
• 噪声：$w_k\sim (0,R_k),v_k\sim (0,Q_{k,j})$

最简单的情况是：$f,g$ 都为线性函数，噪声服从高斯分布，这种情况在 CH10 中会讲到，使用 KEF 可以求得无偏的最优解。如果是非线性系统，和非高斯噪声，情况会变得很复杂，要分情况讨论。

以前通常用滤波器求解状态估计，该方法假设该系统具有马尔可夫性——系统下一时刻状态仅依赖于上一个状态。于是我们可以只维护上一个状态，当有新的输入进入时就更新一次状态估计。

近年来非线性优化已成为主流方法。

状态变量：所有待求解的量，即所有时刻的位姿和路标
x = { x 1 , … , x N , y 1 , … , y M } x=\{x_1,…,x_N,y_1,…,y_M\} x={ x1​,…,xN​,y1​,…,yM​}
状态估计实际上是求解条件分布：$P(x|z,u)$，这里的 $z,u$ 是统称。考虑最简单的情况，当只有观测时，该问题类似于求解一个 SfM（Structure from Motion） 问题。

由贝叶斯法则有：
$$P(x|z)=\frac{P(z|x)P(x)}{P(z)}\propto P(z|x)P(x)$$
其中 $P(z|x)$ 为似然，$P(x)$ 为先验。

然而，$P(x|z)$ 条件分布很难求解，但是我们可以求解：

• 最大后验估计（MAP）。因为分母与 $x$ 无关直接省略。
  $$x_{MAP}^{\ast }=\arg \max P(x|z)=\arg \max P(z|x)P(x)$$

• 最大似然估计（MLE）。此时先验 $x$ 未知
  x M A P ∗