《第6讲 非线性优化 》读书笔记

本文是《视觉SLAM十四讲》第6讲的个人读书笔记，为防止后期记忆遗忘写的。

本节知识脉络

1. 对状态估计问题，通过概率论中贝叶斯公式，求解后验概率等价于求解最大似然函数。求解最大似然函数等价于其最小化负对数的求解。通过公式推导，引出最小二乘。问题转换为：求解最大似然，需要求解目标函数最小二乘公式。
2. 最小二乘的求解需要求导，为避免求导数的巨大计算代价，采用下降迭代近似来求解问题。对于 ∆x的确定，进而引出了不同的求解方法： 一阶和二阶梯度法、Gauss-Newton、Levenberg-Marquadt等等……
3. 在代码中如何实现上述各种求解方法呢？需要用到ceres和g2o第三方库。

---

通过第3-5讲的学习，现在我们已经知道，方程中的位姿可以由变换矩阵来描述，然后用李代数进行优化。观测方程由相机成像模型给出。然而，由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的。所以，与其假设数据必须符合方程，不如来讨论，如何在有噪声的数据中进行准确的状态估计。这就是我们本章讨论的问题。

由于在 SLAM 问题中，同一个点往往会被一个相机在不同的时间内多次观测，同一个相机在每个时刻观测到的点也不止一个。这些因素交织在一起，使我们拥有了更多的约束，最终能够较好地从噪声数据中恢复出我们需要的东西。

6.1 状态估计问题

6.1.1 最大后验与最大似然

SLAM模型由两个方程构成

Xk 乃是相机的位姿。我们可以使用变换矩阵或李代数表示它（由 Tk 或 exp(ξ∧ k )表达）。

至于观测方程，即针孔相机模型。比如：假设在 xk 处对路标 yj 进行了一次观测，对应到 图像上的像素位置 zk,j，那么，观测方程可以表示成：

<------------->

这里 K 为相机内参，s 为像素点的距离。同时这里 的 zk,j 和 yj 都必须以齐次坐标来描述。

在运动和观测方程中，我们通常假设两个噪声项 wk,vk,j 满足零均值的高斯分布。我们希望通过带噪声的数据 z 和 u，推断位姿 x 和地图 y（以 及它们的概率分布），这构成了一个状态估计问题。

很长一段时间内，研究者们使用滤波器，尤其是扩展卡尔曼滤波器（EKF）求解它。但是卡尔曼滤波器关心当前时刻的状态估计 xk，而对之前的状态则不多考虑；相对的，近年来普遍使用的非线性优化方法，使用所有时刻采集到的数据进行状态估计，并被认为优于传统的滤波器 [13]，成为当前视觉 SLAM 的主流方法。

概率论上：对机器人状态的估计，就是求已知输入数据 u 和观测数据 z 的条件下，计算状态 x 的条件概率分布：当我们没有测量运动的传感器（u就是IMU的读数）， 只有一张张的图像时，即只考虑观测方程带来的数据时，相当于估计 P(x|z) 的条件概率分布。利用贝叶斯法则，有：

---

6.1.2 最小二乘的引出

 

• 如何求最大似然估计呢？

对于某一次观测：假设了噪声项为了计算使它最大化的 xk, yj，我们往往使用最小化负对数的方 式，来求一个高斯分布的最大似然。（这是在 求出似然函数概率 的最大概率值-->等价于求  后验函数概率 的最大概率值）

取它的负对数，则变为：。

对原分布求最大化相当于对负对数求最小化。第一项与 x 无 关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。代 入 SLAM 的观测模型，相当于我们在求：                                                                                                                    

该式等价于最小化噪声项（即误差）的平方（Σ 范数意义下）。

 

• 我们现在将运动方程和观测方程同时考虑，不再只单单考虑观测方程。

我们定义数据与估计值之间的误差：。并求该误差的平方之和：