目标：
- 最小二乘法含义、处理方式
- Gauss-Newton，Levenberg-Marquadt下降策略
- Ceres库和g2o库
1 | 引入 |
如何在有噪声的数据中进行准确的状态估计
状态估计问题
最大后验与最大似然
1 | 经典SLAM模型下的表示 |
状态方程+观测方程
$$
\boldsymbol{x}{k}=f\left(\boldsymbol{x}{k-1}, \boldsymbol{u}{k}, \boldsymbol{w}{k}\right) \boldsymbol{z}{k, j}=h\left(\boldsymbol{y}{i}, \boldsymbol{x}{k}, \boldsymbol{v}{k, j}\right)
$$
观测方程重表示
$$
s \boldsymbol{z}{k, j}=\boldsymbol{K} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{y}{j}
$$
$K$ : 相机内参
$s$ : 像素点的距离
$z_{k, j}$ 和 $y_{j}$ 必须以齐次坐标来描述, 且中间有一次齐次到非齐次的转换
考虑噪声项
噪声项满足条件：
$\boldsymbol{w}{k}, \boldsymbol{v}{k, j}$ 满足零均值的高斯分布:
$$
\boldsymbol{w}{k} \sim N\left(0, \boldsymbol{R}{k}\right), \boldsymbol{v}{k} \sim N\left(0, \boldsymbol{Q}{k, j}\right)
$$
转化为状态估计问题:
$$
\boldsymbol{z}, \boldsymbol{u} \rightarrow \boldsymbol{x}, \boldsymbol{y}
$$
通过带噪声的数据 $z$ 和 $u$, 推断位姿 $x$ 和地图 $y$ (以及它们的概率分布)
解决办法：
| 时期 | 方法 | 特点 |
|---|---|---|
| 历史上 | 滤波器，尤其是扩展卡尔曼滤波器(EKF) | 只关心当前时刻状态估计 |
| 近年来 | 非线性优化 | 使用所有时刻采集到的数据进行 |
问题重描述
末知：状态变量(待估计变量)
$$
\boldsymbol{x}=\left{\boldsymbol{x}{1}, \ldots, \boldsymbol{x}{N}, \boldsymbol{y}{1}, \ldots, \boldsymbol{y}{M}\right}
$$
已知：输入数据 $u$ 和观测数据 $z$
描述：
$$
P(\boldsymbol{x} \mid \boldsymbol{z}, \boldsymbol{u})
$$
SfM(Structure from Motion)问题
注：简化描述(无测量运动的传感器, 仅有一张张的图像), 即如何从许多图像中重建三维空间结构, 此种 情况下, SLAM可以看作是图像具有时间先后顺序
$$
P(\boldsymbol{x} \mid \boldsymbol{z})
$$
- 贝叶斯法则应用
$$
P(\boldsymbol{x} \mid \boldsymbol{z})=\frac{P(\boldsymbol{z} \mid \boldsymbol{x}) P(\boldsymbol{x})}{P(\boldsymbol{z})} \propto P(\boldsymbol{z} \mid \boldsymbol{x}) P(\boldsymbol{x}) \text { 后验 }=\frac{\text { 似然 } \cdot \text { 先验 }}{\text { 与待估计状态 } \boldsymbol{x} \text { 无关的量 }} \propto \text { 似然 } \cdot \text { 先验 }
$$
直接求解后验分布困难, 但是求一个状态最优估计，使得该状态下，后验概率最大化（Maximize a Posterior, MAP) 则可行
不知道机器人位置大概在什么地方, 没有先验, 可求解 $x$ 的最大似然估计（Maximize Likelihood Estimation, MLE)问题重描述
末知：状态变量(待估计变量)
$$
\boldsymbol{x}=\left{\boldsymbol{x}{1}, \ldots, \boldsymbol{x}{N}, \boldsymbol{y}{1}, \ldots, \boldsymbol{y}{M}\right}
$$
已知：输入数据 $u$ 和观测数据 $z$
描述：
$$
P(\boldsymbol{x} \mid \boldsymbol{z}, \boldsymbol{u})
$$SfM(Structure from Motion)问题
注：简化描述(无测量运动的传感器, 仅有一张张的图像), 即如何从许多图像中重建三维空间结构, 此种 情况下, SLAM可以看作是图像具有时间先后顺序
$$
P(\boldsymbol{x} \mid \boldsymbol{z})
$$
- 贝叶斯法则应用
$$
P(\boldsymbol{x} \mid \boldsymbol{z})=\frac{P(\boldsymbol{z} \mid \boldsymbol{x}) P(\boldsymbol{x})}{P(\boldsymbol{z})} \propto P(\boldsymbol{z} \mid \boldsymbol{x}) P(\boldsymbol{x}) \text { 后验 }=\frac{\text { 似然 } \cdot \text { 先验 }}{\text { 与待估计状态 } \boldsymbol{x} \text { 无关的量 }} \propto \text { 似然 } \cdot \text { 先验 }
$$直接求解后验分布困难, 但是求一个状态最优估计，使得该状态下，后验概率最大化（Maximize a Posterior, MAP) 则可行
不知道机器人位置大概在什么地方, 没有先验, 可求解 $x$ 的最大似然估计（Maximize Likelihood Estimation, MLE)
$\boldsymbol{x}{\mathrm{MAP}}^{}=\arg \max P(\boldsymbol{x} \mid \boldsymbol{z})=\arg \max P(\boldsymbol{z} \mid \boldsymbol{x}) P(\boldsymbol{x})$
$\boldsymbol{x}^{}{ }{M L E}=\arg \max P(\boldsymbol{z} \mid \boldsymbol{x})$
MAP与MLE对比
$$
\begin{aligned}
&\boldsymbol{x}{\mathrm{MAP}}^{}=\arg \max P(\boldsymbol{z} \mid \boldsymbol{x}) P(\boldsymbol{x}) \
&\boldsymbol{x}^{}{ }{M L E}=\arg \max P(\boldsymbol{z} \mid \boldsymbol{x})
\end{aligned}
$$
| 概念 | 文字描述 |
|---|---|
| MAP | 存在先验 |
| MLE | 在什么样状态下，最可能产生现在观测到的数据 |
最小二泰的引出
观测模型构建
对于某次观测
$$
z_{k, j}=h\left(y_{j}, x_{k}\right)+v_{k, j}
$$
由于噪声项 $v_{k} \sim N\left(0, Q_{k, j}\right)$, 故观测数据的条件概率为:
$$
P\left(z_{j, k} \mid x_{k}, y_{j}\right)=N\left(h\left(y_{j}, x_{k}\right), Q_{k, j}\right)
$$
最小化负对数求高斯分布的最大似然
任意高维高斯分布 $x \sim N(\mu, \Sigma)$ 概率密度展开形式:
$$
P(x)=\frac{1}{\sqrt{(2 \pi)^{N} \operatorname{det}(\Sigma)}} \exp \left(-\frac{1}{2}(x-\mu)^{T} \Sigma^{-1}(x-\mu)\right)
$$
取负对数:
$$
-\ln (P(x))=\frac{1}{2} \ln \left((2 \pi)^{N} \operatorname{det}(\Sigma)\right)+\frac{1}{2}(x-\mu)^{T} \Sigma^{-1}(x-\mu)
$$
抛除常数的右边第一项, 并用已知条件替换符号:
$$
\boldsymbol{x}^{*}=\arg \min \left(\left(z_{k, j}-h\left(\boldsymbol{x}{k}, \boldsymbol{y}{j}\right)\right)^{T} \boldsymbol{Q}{k, j}^{-1}\left(z{k, j}-h\left(\boldsymbol{x}{k}, \boldsymbol{y}{j}\right)\right)\right)
$$
等价于最小化噪声项的平方（噪声项即误差，平方为在 $\Sigma$ 范数意义下），故定义数据与估计值间的误差：
$$
e_{v, k}=x_{k}-f\left(x_{k-1}, u_{k}\right) e_{y, j, k}=z_{k, j}-h\left(\boldsymbol{x}{k}, \boldsymbol{y}{j}\right)
$$
求得误差的平方之和为:
$$
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}
$$
至此，问题转换为了总体意义下的最小二乘问题 (Least Square Problem)。
SLAM中最小二乘问题所具有的特定结构
- 问题目标函数由许多个误差的(加权)平方和组成
- 状态变量维数高但误差项简单
- 每个误差项是一个小规模约束, 对应优化变量称为参数块(Parameter Block)
- 增量方程求解具有稀疏性 (整体误差由很多小型误差项之和组成的问题)
- 不同表示
- 李代数表示 $\rightarrow$ 无约束最小二乘问题
- 旋转矩阵→引入约束 (正交阵且行列式为1）→优化困难
- 误差的度量, 平方形式直观但存在问题
非线性最小二乘
问题的描述
$$
\min {x} \frac{1}{2}|f(\boldsymbol{x})|{2}^{2}
$$
其中, $x \in \mathbb{R}^{n}, f$ 是任意一个非线性函数 $f(x) \in \mathbb{R}^{m}$
解析解:
$$
\frac{\mathrm{d} f}{\mathrm{~d} x}=0
$$
数值解（Gradient Descent）：问题的描述
$$
\min {x} \frac{1}{2}|f(\boldsymbol{x})|{2}^{2}
$$
其中, $x \in \mathbb{R}^{n}, f$ 是任意一个非线性函数 $f(x) \in \mathbb{R}^{m}$
解析解:
$$
\frac{\mathrm{d} f}{\mathrm{~d} x}=0
$$
数值解（Gradient Descent）：
一阶和二阶梯度法
一阶梯度法
目标函数在 $x$ 附近进行泰勒展开
$$
|f(\boldsymbol{x}+\Delta \boldsymbol{x})|{2}^{2} \approx|f(\boldsymbol{x})|{2}^{2}+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}+\frac{1}{2} \Delta \boldsymbol{x}^{T} \boldsymbol{H} \Delta \boldsymbol{x}
$$
- $\boldsymbol{J}$ 是 $|f(\boldsymbol{x})|^{2}$ 关于 $x$ 的导数 (雅可比矩阵)
- $H$ 则是二阶导数 (海塞 (Hessian)矩阵)
保留一阶梯度
$$
\Delta \boldsymbol{x}^{}=-\boldsymbol{J}^{T}(\boldsymbol{x})
$$
直观意义：只要我们沿着反向梯度方向前进即可; 还需要该方向上取一个步长 $\lambda$, 求得最快的下降方式
二阶梯度法
保留二阶梯度后增量方程为
$$
\Delta \boldsymbol{x}^{}=\arg \min |f(\boldsymbol{x})|_{2}^{2}+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}+\frac{1}{2} \Delta \boldsymbol{x}^{T} \boldsymbol{H} \Delta \boldsymbol{x} .
$$
求右侧等式关于 $\Delta x$ 的导数并令它为零, 就得到了增量的解:
$$
\boldsymbol{H} \Delta \boldsymbol{x}=-\boldsymbol{J}^{T} .
$$
该方法称又为牛顿法。
一阶和二阶梯度法
一阶梯度法
目标函数在 $x$ 附近进行泰勒展开
$$
|f(\boldsymbol{x}+\Delta \boldsymbol{x})|{2}^{2} \approx|f(\boldsymbol{x})|{2}^{2}+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}+\frac{1}{2} \Delta \boldsymbol{x}^{T} \boldsymbol{H} \Delta \boldsymbol{x}
$$
- $\boldsymbol{J}$ 是 $|f(\boldsymbol{x})|^{2}$ 关于 $x$ 的导数 (雅可比矩阵)
- $H$ 则是二阶导数 (海塞 (Hessian)矩阵)
保留一阶梯度
$$
\Delta \boldsymbol{x}^{}=-\boldsymbol{J}^{T}(\boldsymbol{x})
$$
直观意义：只要我们沿着反向梯度方向前进即可; 还需要该方向上取一个步长 $\lambda$, 求得最快的下降方式
二阶梯度法
保留二阶梯度后增量方程为
$$
\Delta \boldsymbol{x}^{}=\arg \min |f(\boldsymbol{x})|_{2}^{2}+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}+\frac{1}{2} \Delta \boldsymbol{x}^{T} \boldsymbol{H} \Delta \boldsymbol{x} .
$$
求右侧等式关于 $\Delta x$ 的导数并令它为零, 就得到了增量的解:
$$
\boldsymbol{H} \Delta \boldsymbol{x}=-\boldsymbol{J}^{T} .
$$
该方法称又为牛顿法。
1 | 缺点 |
- 最速下降法：过于贪心，容易走出锯齿路线，增加迭代次数
- 牛顿法：Hessian矩阵求解困难
Gauss-Newton
思想
将 $f(\boldsymbol{x})$ 进行一阶的 泰勒展开 (请注意不是目标函数 $f(x)^{2}$ )
$$
f(x+\Delta \boldsymbol{x}) \approx f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}
$$
$\boldsymbol{J}(\boldsymbol{x})$ 为 $f(\boldsymbol{x})$ 关于 $\boldsymbol{x}$ 的导数, 实际上是一个 $m \times n$ 的矩阵, 也是一个雅可比矩 阵
步骤
目标：寻找下降矢量 $\Delta x$, 使得 $|f(x+\Delta x)|^{2}$ 达到最小
问题转化: 解一个线性的最小二乘问题
$$
\Delta \boldsymbol{x}^{*}=\arg \min {\Delta \boldsymbol{x}} \frac{1}{2}|f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}|^{2}
$$
展开:
$$
\begin{aligned}
\frac{1}{2}|f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}|^{2} &=\frac{1}{2}(f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x})^{T}(f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}) \
&=\frac{1}{2}\left(|f(\boldsymbol{x})|{2}^{2}+2 f(\boldsymbol{x})^{T} \boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}+\Delta \boldsymbol{x}^{T} \boldsymbol{J}(\boldsymbol{x})^{T} \boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}\right)
\end{aligned}
$$
对 $\Delta x$ 求导并令其为 0 :
$$
2 \boldsymbol{J}(\boldsymbol{x})^{T} f(\boldsymbol{x})+2 \boldsymbol{J}(\boldsymbol{x})^{T} \boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}=\mathbf{0}
$$
得到方程组:
$$
\boldsymbol{J}(\boldsymbol{x})^{T} \boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}=-\boldsymbol{J}(\boldsymbol{x})^{T} f(\boldsymbol{x})
$$
高斯牛顿方程/正规方程/增量方程
$$
\boldsymbol{J}(\boldsymbol{x})^{T} \boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}=-\boldsymbol{J}(\boldsymbol{x})^{T} f(\boldsymbol{x}) \boldsymbol{H} \Delta \boldsymbol{x}=\boldsymbol{g}
$$
与牛顿法的对比
对比牛顿法可见, Gauss-Newton 用 $\boldsymbol{J}^{T} J$ 作为牛顿法中 二阶 Hessian 矩阵的近似, 从而省略了计算 $\boldsymbol{H}$ 的过 程。
步骤小结
不足
- 可能出现 $\boldsymbol{J}^{T} \boldsymbol{J}$ 为奇异矩阵或者病态的情况, 此时增量稳定性较差, 算法不收敛
- 即使 $\boldsymbol{H}=\boldsymbol{J}^{T} \boldsymbol{J}$ 非奇异也非病态, 如果求出来的步长 $\Delta x$ 太大, 也会导致采用的局部近似不够准确
Levenberg-Marquadt
信赖区域方法(Trust Region Method) - 提出
Gauss-Newton 方法中采用的近似二阶泰勒展开只能在展开点附近有较好效果, 所以我们很自然地想到应该给 $\Delta x$ 添加一个信赖区域 (Trust Region), 不能让它太大而使得近似不准确。在信赖区域里边, 我们认为近似是有效 的; 出了这个区域, 近似可能会出问题。 - 信赖区域范围的确定
那么如何确定这个信赖区域的范围呢? 一个比较好的方法是根据我们的近似模型跟实 际函数之间的差异来确定这 个范围: 如果差异小, 我们就让范围尽可能大: 如果差异大, 我就缩小这个近似范围。因此, 考虑使用
$$
\rho=\frac{f(\boldsymbol{x}+\Delta \boldsymbol{x})-f(\boldsymbol{x})}{\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}}=\frac{\text { 实际函数下降的值 }}{\text { 近似模型下降的值 }}=\left{\begin{array}{l}
1, \text { 近似较好 } \
0, \text { 近似较差 }
\end{array}\right.
$$
改良版非线性优化框架
- 近似范围扩大的倍数和嘓值都是经验值
- $\left|\boldsymbol{D} \Delta \boldsymbol{x}_{k}\right|^{2} \leq \mu$ 中的 $\boldsymbol{D}$ 把增量限定于椭球中
- Levenberg: $D$ 取成单位阵 $I$
- Marquardt：D取成非负数对角阵，实际上通常用 $J^{T} J$ 的对角元素平方根，使得在梯度小的维度上约束 范围更大一些
问题无约束化
在 $\mathrm{L}-\mathrm{M}$ 优化中, 我们都需要解式 (6.24) 那样一个子问题来获得梯度, 这 个子问题是带不等式约束的优化问 题, 我们用 Lagrange 乘子将它转化为一个无约束优化问题:
$$
\min {\Delta x{k}} \frac{1}{2}\left|f\left(\boldsymbol{x}{k}\right)+\boldsymbol{J}\left(\boldsymbol{x}{k}\right) \Delta \boldsymbol{x}{k}\right|^{2}+\frac{\lambda}{2}|\boldsymbol{D} \Delta \boldsymbol{x}|^{2}
$$
展开后化简为:
$$
\left(\boldsymbol{H}+\lambda \boldsymbol{D}^{T} \boldsymbol{D}\right) \Delta \boldsymbol{x}=\boldsymbol{g}
$$
可以看到, 增量方程相比于 Gauss-Newton, 多了一项 $\lambda D^{T} D{\circ}$ 如果考虑它的简化形式, 即 $D=I$, 那么相当 于求解:
$$
(\boldsymbol{H}+\lambda \boldsymbol{I}) \Delta \boldsymbol{x}=\boldsymbol{g}=\left{\begin{array}{l}
\lambda \text { 较小, } \boldsymbol{H} \text { 主导, 二次近似模型较好, 接近于 } G-N \text { 法 } \
\lambda \text { 较大, } \lambda \boldsymbol{I} \text { 主导, 一次近似模型较好, 接近于一阶梯度下降法法 }
\end{array}\right.
$$
优点
一定程度上避免线性方程组的稀疏矩阵的非奇异和病态问题, 提供更稳定更准确的增量 $\Delta x$问题无约束化
在 $\mathrm{L}-\mathrm{M}$ 优化中, 我们都需要解式 (6.24) 那样一个子问题来获得梯度, 这 个子问题是带不等式约束的优化问 题, 我们用 Lagrange 乘子将它转化为一个无约束优化问题:
$$
\min {\Delta x{k}} \frac{1}{2}\left|f\left(\boldsymbol{x}{k}\right)+\boldsymbol{J}\left(\boldsymbol{x}{k}\right) \Delta \boldsymbol{x}{k}\right|^{2}+\frac{\lambda}{2}|\boldsymbol{D} \Delta \boldsymbol{x}|^{2}
$$
展开后化简为:
$$
\left(\boldsymbol{H}+\lambda \boldsymbol{D}^{T} \boldsymbol{D}\right) \Delta \boldsymbol{x}=\boldsymbol{g}
$$
可以看到, 增量方程相比于 Gauss-Newton, 多了一项 $\lambda D^{T} D{\circ}$ 如果考虑它的简化形式, 即 $D=I$, 那么相当 于求解:
$$
(\boldsymbol{H}+\lambda \boldsymbol{I}) \Delta \boldsymbol{x}=\boldsymbol{g}=\left{\begin{array}{l}
\lambda \text { 较小, } \boldsymbol{H} \text { 主导, 二次近似模型较好, 接近于 } G-N \text { 法 } \
\lambda \text { 较大, } \lambda \boldsymbol{I} \text { 主导, 一次近似模型较好, 接近于一阶梯度下降法法 }
\end{array}\right.
$$
优点- 一定程度上避免线性方程组的稀疏矩阵的非奇异和病态问题, 提供更稳定更准确的增量 $\Delta x$
小结
非线性优化问题框架
- Line Search
- 固定搜索方向→在该方向寻找步长
- 最速下降法、Gauss-Newton法
- Trust Region
- 固定搜索区域，考虑找该区域内的最优点
- Levenberg-Marquadti法
初始值问题
初始值的选择应该有科学依据, 视觉SLAM问题中, $I C P, P n P$ 等算法
线性增量方程组求解
由于 $\Delta x$ 维度很大, 求逆困难, 故需数值求解方法, 矩阵分解方法:
$Q R$, Cholesky等分解方法常用
但是视觉SLAM中矩阵往往有特定稀疏形式