《视觉SLAM14讲》学习小结——供小白快速入门ch6

Zealot[胜利][憨笑][疑问]

已于 2024-08-02 03:08:22 修改

阅读量452

点赞数 4

分类专栏：《视觉SLAM14讲》学习小结——供小白快速入门：理论部分文章标签：计算机视觉

于 2024-07-30 16:26:12 首次发布

本文链接：https://blog.csdn.net/weixin_44469383/article/details/140780328

版权

《视觉SLAM14讲》学习小结——供小白快速入门：理论部分专栏收录该内容

13 篇文章 5 订阅

订阅专栏

非线性优化

如前所述，视觉SLAM的本质是一个参数优化问题。如何对此类问题进行优化已有基于Linux系统的支持库。为了使用这些库，我们首先要了解参数优化的基本过程和一些基本术语。本章的内容仅仅讲述优化的方法，并不涉及SLAM模型中外参的具体优化。后者将在第9章具体讲述。

一、状态最优估计的概念

根据ch2中的分析，视觉SLAM的本质是根据观测模型 $\mathbf{z}_{k,j}=\mathbf{h}(\mathbf{y}_j,\mathbf{x}_k,\mathbf{v}_{k,j})$ 来确定相机的外参和路标点。其中下标k表示时间，所以每一个时刻都可以得到一个这样的观测方程。 $\mathbf{y}_j$ 表示路标的坐标列阵，下标j用于区别不同的路标；也就是说每一个特征点都有一个 $\mathbf{y}_j$ ，可建立一个这样的观测方程。 $\mathbf{x}_k$ 表示相机的外参，也就是其位姿 $\mathbf{R},\mathbf{t}$ 。 $\mathbf{z}_{k,j}$ 表示相机的观测结果，也就是像素坐标。 $\mathbf{v}_{k,j}$ 表示观测噪声，与观测时刻和路标有关。根据ch5的公式（5），观测模型的具体形式为

$\mathbf{z}_{k,j}= \frac{1}{Z}\mathbf{K}(\mathbf{R}_{k}\mathbf{y}_j+\mathbf{t}_k)+\mathbf{v}_{k,j}$ （1）

为了根据观测结果 $\mathbf{z}$ 反求外参 $\mathbf{R},\mathbf{t}$ 和路标 $\mathbf{y}$ ，我们先猜测这三个参数的初始值，然后计算

$\mathbf{e}_{j,k}=\mathbf{z}_{k,j}- \frac{1}{Z}(\mathbf{K}\mathbf{R}_{k}\mathbf{y}_j+\mathbf{t}_k)$ （2）

以及

$\mathbf{F}(\mathbf{R},\mathbf{t},\mathbf{y})=\sum\limits_k\sum\limits_j\mathbf{e}_{j,k}^\mathrm{T}\mathbf{Q}_{k,j}^{-1}\mathbf{e}_{j,k}$ （3）

其中 $\mathbf{Q}_{k,j}^{-1}$ 为高斯分布 $\mathbf{v}_{k,j}$ 协方差矩阵之逆，又称为信息矩阵。如果没有量测噪声且 $\mathbf{R},\mathbf{t}$ 和路标 $\mathbf{y}$ 的猜测值正确，则 $\mathbf{e}_{j,k}$ 和 $\mathbf{F}$ 应当精确等于零。事实上因为 $\mathbf{F}$ 一般不为零。为此我们调整 $\mathbf{R},\mathbf{t}$ 和路标 $\mathbf{y}$ 的值使得 $\mathbf{F}$ 的变小。当 $J$ 达到最小时就认为所得到的 $\mathbf{R},\mathbf{t}$ 和路标 $\mathbf{y}$ 为最优估计。

如何调整 $\mathbf{R},\mathbf{t}$ 和路标 $\mathbf{y}$ 就是参数优化或静态优化问题所要研究的内容。如果式（1）中 $\mathbf{z}_{k,j}$ 与待优化变量的关系是线性的，则这种优化的求解称为线性优化问题。如果我们之间取 $\mathbf{R}_k$ 中的元素为待优化变量，则如ch4所述会变成一个有约束优化问题。如果取相机的方位角为待优化参数，则因的存在 $\mathbf{R}_k$ ，式（1）和（3）表征的是一个非线性优化问题。因此下面主要讨论非线性优化方法。

二、非线性优化方法

1.优化的总体框架

将 $\mathbf{R},\mathbf{t}$ 和路标 $\mathbf{y}$ 组合起来视为一个新的变量 $\mathbf{x}$ 。再令 $\mathbf{f}_{k,j}(\mathbf{x})=\mathbf{R}_{k,j}\mathbf{e}_{j,k}$ ，其中 $\mathbf{R}_{k,j}^\mathrm{T}\mathbf{R}_{k,j}=\mathbf{Q}^{-1}_{k,j}$ ，则式（3）右端可改写为 $|\mathbf{f}(\mathbf{x})|_2^2=\mathbf{f}(\mathbf{x})^\mathbf{T}\mathbf{f}(\mathbf{x})$ ，其中 $\mathbf{f}(\mathbf{x})$ 是由所有 $\mathbf{f}_{k,j}(\Delta\mathbf{x})$ 所构成的列阵。根据这些符号，求式（3）的最小值可用下面更一般的形式来表示

$\min\limits_{\mathbf{x}}F=|\mathbf{f}(\mathbf{x})|_2^2$ （4）

如果 $\mathbf{f}$ 的形式很简单，则这种最小值问题可通过求驻点 $\mathrm{d}F/\mathrm{d}\mathbf{x}=0$ 来解决。实际应用中， $\mathbf{f}$ 的表达式往往非常复杂。此外，我们往往还需要给 $\mathbf{f}$ 乘上核函数（在程序调试部分会讲解），使得 $\mathbf{f}$ 进一步复杂化，这时可采用如下数值算法来解决。

（a）先猜测某个初始值 $\mathbf{x}_0$ 。

（b）在第 $k$ 步，寻找一个增量 $\Delta\mathbf{x}_k$ 使得 $|\mathbf{f}(\mathbf{x}_k+\Delta\mathbf{x}_k)|_2^2$ 达到最小值

（c）若 $\Delta\mathbf{x}_k$ 足够小，则停止。

（d）否则，令 $\mathbf{x}_{k+1}=\mathbf{x}_k+\Delta\mathbf{x}_k$ ，返回第（b）步。

这样就将非线性优化问题转换为求 $|\mathbf{f}(\mathbf{x}_k+\Delta\mathbf{x}_k)|_2^2$ 的最小值问题。由于可以对 $\mathbf{f}$ 线性化，增量的计算将会简单很多。

从上面优化步骤可以看出，其核心算法在于(b)步。针对(b)算法的不同，优化方法可分为如下几种。

2.一阶和二阶梯度法（牛顿法）

在第 $k$ 步，将 $F$ 在 $\mathbf{x}_k$ 附近进行泰勒级数展开可得

$F(\mathbf{x}_k+\Delta\mathbf{x}_k)\approx F(\mathbf{x}_k)+\mathbf{J}(\mathbf{x}_k)^\mathrm{T}\Delta\mathbf{x}_k+\frac{1}{2}\Delta\mathbf{x}_k^\mathrm{T}\mathbf{H}(\mathbf{x}_k)\Delta\mathbf{x}_k$ （5）

其中 $\mathbf{J}$ 是 $F(\mathbf{x})$ 关于 $\mathbf{x}$ 的一阶导数，称为雅可比矩阵； $\mathbf{H}$ 是 $F(\mathbf{x})$ 关于 $\mathbf{x}$ 的二阶导数，称为海塞矩阵。如果略去二阶小量，则从式（5）可明显看出在 $\Delta\mathbf{x}=-\mathbf{J}(\mathbf{x}_k)$ 时可保证 $F(\mathbf{x}_k+\Delta\mathbf{x}_k)$ 变小。这样我们只要合适地选择一个比例常数 $\lambda$ 就可能使得 $F[\mathbf{x}_k-\lambda\mathbf{J}(\mathbf{x}_k)]$ 取得最小值。这种确定 $\Delta\mathbf{x}$ 的方法称为最速下降法。

如果不略去二阶小量，直接根据式（4）也可确定使 $F(\mathbf{x}_k+\Delta\mathbf{x}_k)$ 最优的 $\Delta\mathbf{x}$ 。根据驻点法有

$\mathbf{J}+\mathbf{H}\Delta\mathbf{x}_k=\mathbf{0} \Rightarrow \mathbf{H}\Delta\mathbf{x}_k=-\mathbf{J}$ （6）

求解此线性方程组即可得到 $\Delta\mathbf{x}$ ，这种确定 $\Delta\mathbf{x}$ 的方法称为牛顿法。

这两种方法的原理都非常直观，但在实际应用时都存在一些问题，例如最速下降法要确定 $\lambda$ 的值，牛顿法要计算海塞矩阵 $\mathbf{H}$ 。下面介绍两种更为实用的方法。

3.高斯牛顿法

牛顿法将式（4）中 $F(\mathbf{x})$ 进行泰勒级数展开，高斯牛顿法则是将式（4）中的 $\mathbf{f}(\mathbf{x})$ 进行泰勒级数展开并取线性近似

$\mathbf{f}(\mathbf{x}_k+\Delta\mathbf{x}_k)\approx \mathbf{f}(\mathbf{x}_k)+\mathbf{J}(\mathbf{x}_k)^\mathrm{T}\Delta\mathbf{x}_k$ （7）

这里雅可比矩阵 $\mathbf{J}$ 是 $\mathbf{f}(\mathbf{x})$ 关于 $\mathbf{x}$ 的一阶导数。根据前面的非线性优化算法步骤（b），需要求 $\Delta\mathbf{x}_k$ 使 $\mathbf{f}(\mathbf{x}_k+\Delta\mathbf{x}_k)$ 达到最小。这本质上是一个线性最小二乘问题

$\Delta\mathbf{x}_k^*=\arg\min\limits_{\Delta\mathbf{x}}|\mathbf{f}(\mathbf{x}_k)+\mathbf{J}^\mathrm{T}\Delta\mathbf{x}_k|_2^2$ （8）

利用驻点法不难证明这个问题的求解可转化为求解如下关于 $\Delta\mathbf{x}_k$ 的线性方程组

$\mathbf{J}(\mathbf{x}_k)\mathbf{J}^\mathrm{T}(\mathbf{x}_k)\Delta\mathbf{x}_k=-\mathbf{J}(\mathbf{x}_k)\mathbf{f}(\mathbf{x}_k)$ （9）

和式（6）对比可以发现，高斯牛顿法只需要计算雅可比矩阵 $\mathbf{J}$ 而不需要海塞矩阵 $\mathbf{H}$ ，因而使用更方便。

4.列文伯格-马夸尔特法

可以证明高斯牛顿法中的 $\mathbf{J}(\mathbf{x}_k)\mathbf{J}^\mathrm{T}(\mathbf{x}_k)$ 是半正定的。当 $\mathbf{J}(\mathbf{x}_k)\mathbf{J}^\mathrm{T}(\mathbf{x}_k)$ 为奇异矩阵或病态矩阵（即特征根为零或很小）时，根据线性方程组（9）计算得到的 $\Delta\mathbf{x}_k$ 就会很大甚至趋于无穷，使优化不收敛。为了解决这个问题就产生了列文伯格-马夸尔特法。它比高斯牛顿法更健壮，但收敛速度可能更慢，又称阻尼牛顿法。其思想是给计算得到的 $\Delta\mathbf{x}_k$ 添加一个范围或信赖区域。若计算得到的 $\Delta\mathbf{x}_k$ 落在信赖区域内，则继续按高斯牛顿法进行，否则就需要用改进的方法计算 $\Delta\mathbf{x}_k$ 。具体说，列文伯格-马夸尔特法将高斯牛顿法中的式（8）改写成

$\Delta\mathbf{x}_k^*=\arg\min\limits_{\Delta\mathbf{x}}|\mathbf{f}(\mathbf{x}_k)+\mathbf{J}^\mathrm{T}\Delta\mathbf{x}_k|_2^2, ~~s.t. |\mathbf{D}\Delta\mathbf{x}_k|^2\leq \mu$ （10）

其中 $\mathbf{D}$ 取矩阵 $\mathbf{J}(\mathbf{x}_k)\mathbf{J}^\mathrm{T}(\mathbf{x}_k)$ 对角元素的平方根。那么如何确定 $\mu$ 的值呢？很显然，如果 $\mathbf{J}(\mathbf{x}_k)\mathbf{J}^\mathrm{T}(\mathbf{x}_k)$ 越正定，计算得到的 $\Delta\mathbf{x}_k$ 就会越容易使式（7）成立。因此式

$\rho =\frac{\mathbf{f}(\mathbf{x}_k+\Delta\mathbf{x}_k)-\mathbf{f}(\mathbf{x}_k)}{\mathbf{J}(\mathbf{x}_k)^\mathrm{T}\Delta\mathbf{x}_k}$ （11）

是一个衡量 $\mu$ 值是否合理的判据。 $\rho$ 越大说明计算得到的 $\Delta\mathbf{x}_k$ 越合理，这时候可以适当增大 $\mu$ ；否则 $\rho$ 越小说明计算得到的 $\Delta\mathbf{x}_k$ 越不合理，这时候就需要减小 $\mu$ 。

根据以上分析，第二、1节中的优化总体框架需要相应地修改为

（a）给定初始值 $\mathbf{x}_0$ 和初始优化半径 $\mu$ 。

（b）在第 $k$ 步，寻找一个增量 $\Delta\mathbf{x}_k$ 以满足式（10）。

（c）按式（11）计算 $\rho$ ，若 $\rho\geq 3/4$ 则令 $\mu=2\mu$ ；若 $\rho\leq 1/4$ 则令 $\mu=\mu/2$ ，并返回第（b）步；若 $1/4\leq \rho\leq 3/4$ ，记录增量 $\Delta\mathbf{x}_k$ 。

（d）判断 $\Delta\mathbf{x}_k$ 是否足够小，是则停止。

（d）否则，令 $\mathbf{x}_{k+1}=\mathbf{x}_k+\Delta\mathbf{x}_k$ ，返回第（b）步。

从上面步骤可以看出，其核心在于求解方程（10）。对于此有约束的线性优化问题，根据拉格朗日函数法不难证明其解为如下线性方程组的解

$[\mathbf{J}(\mathbf{x}_k)\mathbf{J}^\mathrm{T}(\mathbf{x}_k)+\lambda\mathbf{D}^\mathrm{T}\mathbf{D}]\Delta\mathbf{x}_k=-\mathbf{J}(\mathbf{x}_k)\mathbf{f}(\mathbf{x}_k)$ （11）