第六讲非线性优化

最新推荐文章于 2024-06-15 22:10:51 发布

喵喵不爱吃鱼

最新推荐文章于 2024-06-15 22:10:51 发布

阅读量982

点赞数 12

分类专栏：视觉SLAM十四讲文章标签：计算机视觉

本文链接：https://blog.csdn.net/weixin_42391513/article/details/138967535

版权

视觉SLAM十四讲专栏收录该内容

5 篇文章 0 订阅

订阅专栏

理解最小二乘法的含义和处理方式；理解高斯牛顿法、列文伯格-马夸尔特方法等下降策略；学习Ceres库和g2o库的基本使用方法。

1、状态估计问题

SLAM模型由一个运动方程和一个观测方程构成。 $\begin{cases} x_k = f(x_{k-1}, u_k) + w_k \\ z_{k,j} = h(y_j, x_k) + v_{k,j} \end{cases}$
- $u_k$ 可以理解为外部影响或操控信号，这些信号会影响系统从状态 $x_{k-1}$ 过渡到状态 $x_k$ 。例如：在机器人导航中，控制输入可能是机器人每个时间步的速度和转向角。
- $x_k$ 是相机位姿， $z_{k,j}$ 是在 $x_k$ 对路标 $y_j$ 进行一次观测，对应到图像上的像素位置。
- $w_k, v_k$ 是噪声，通常假设满足零均值的高斯分布。
- 在噪声影响下，希望通过带噪声的数据z和u推断位姿x和地图y，这构成了一个状态估计问题。
处理状态估计问题
- 大致分为两种：增量/渐进方法（滤波器，后端讲）；批量方法（当前主流方法，本讲重点）。
- 而在SLAM中实用方法通常是折衷的手段，比如固定一些历史轨迹，仅对当前时刻附近的一些轨迹进行优化，即滑动窗口估计法。
非线性优化为主的批量优化方法
- 对机器人状态的估计，从概率学看，就是已知输入数据u和观测数据z的条件下，求x,y的条件概率分布： $P(x,y|z,u)$
  - 所有时刻的机器人位姿和路标点坐标为： $x = \{ x_1,..., x_N \}, y = \{ y_1, ..., y_M \}$ ；u标识所有时刻的输入；z表示所有时刻的观测数据。
  - 特别的，当不知道控制输入时，相当于估计 $P(x,y|z)$ 的条件概率分布，也称为SfM（Structure from Motion），即如何从许多图像中重新三维空间结构。
- 利用贝叶斯法则，有： $P(x,y|z,u) = \frac{P(z,u|x,y)P(x,y)}{P(z,u)} \propto {P(z,u|x,y)P(x,y)}$
  - 最大后验概率等价于最大化似然和先验的乘积： $(x,y)^*_{MAP} = arg max P(x,y|z,u) = arg max P(z,u|x,y)P(x,y)$
  - 当没有先验时，可以求解最大似然估计（Maximize Likelihood Estimation, MLE）： $(x,y)^*_{MLE} = arg max P(z,u|x,y)$
  - 这里的最大似然估计可以理解成：“在什么样的状态下，最可能产生现在观测到的数据”.
怎样求最大似然估计？
- 在噪声为高斯分布的假设下，对于某一次观测，观测数据的条件概率依然是一个高斯分布： $P(z_{j,k}|x_k,y_j) = N(h(y_j,x_k),Q_{k,j})$ 。
- 考虑单次观测的最大似然估计，可以用最小化负对数来求一个高斯分布的最大似然。
  - 代入SLAM， $(x,y)^*= arg max N(h(y_j,x_k),Q_{k,j}) = arg min ((z_{k,j} - h(x_k,y_j))^TQ_{k,j}^{-1}(z_{k,j} - h(x_k,y_j)))$
  - 等价于最小化噪声项的一个二次型，称为马氏距离。
  - 也可以看做 $Q_{k,j}^{-1}$ （也称作信息矩阵，即高斯分布协方差矩阵之逆）加权后的欧式距离。
- 考虑批量时刻的数据。通常假设各个时刻的输入和观测相互独立，可以对联合分布进行因式分解： $P(z,u|x,y) = \prod P(u_k|x_{k-1},x_k) \prod P(z_{k,j}|x_k,y_j)$
  - 各次输入和观测数据与模型之间的误差： $e_{u,k} = x_k - f(x_{k-1}, u_k) \\ e_{z,j,k} = z_{k,j} - h(x_k,y_j)$
  - 最小化所有时刻估计值和真实值之间的马氏距离，等价于求最大似然估计。
  - 负对数将乘积变成求和： $min J(x,y) = \sum_{\substack{k}} e_{u,k}^TR_k^{-1}e_{u,k} + \sum_{\substack{k}}\sum_{\substack{j}} e_{z,k,j}^TQ_{k,j}^{-1}e_{z,k,j}$
  - 这样，得到一个最小二乘问题，它的解等价于状态的最大似然估计。如何求解，还需要一些非线性优化的基本知识。

2、非线性最小二乘

考虑一个简单的最小二乘问题： $min F(x) = \frac{1}{2}||f(x)||_2^2$ （也就是 $\frac{1}{2}f(x)^Tf(x)$ ）
- 解析形式求解方法：令目标函数的导数为0，求解x的最优值。
- 对于不方便直接求解的最小二乘问题，通过迭代方法，从一个初始值出发，不断更新当前的优化变量，使目标函数下降。这就让求解导函数为0的问题变成一个不断寻找下降增量 $\triangle x_k$ 的问题。
如何寻找增量 $\triangle x_k$ ：一阶和二阶梯度法
- 目标函数在 $x_k$ 处进行泰勒展开： $F(x_k+\triangle x_k) = F(x_k) + J(x_k)^T\triangle x_k + 1/2 \triangle x_k^TH(x_k) \triangle x_k$
  - J 是F(x)关于x的一阶导数（梯度、雅克比矩阵）
  - H则是二阶导数（海塞矩阵）
- 一阶和二阶梯度法
  - 保留一阶梯度信息，一阶梯度法，最速下降法。 $\triangle x^* = -J(x_k)$
  - 保留二阶梯度信息，二阶梯度法，牛顿法。 $H\triangle x = -J$
- 最速下降法和牛顿法虽然直观，但在实用中存在一些缺点：
  - 最速下降法过于贪心，容易走出锯齿路线，增加迭代次数。
  - 牛顿法迭代次数少，但是需要计算复杂的Hessian矩阵
更实用的方法：高斯牛顿法和列文伯格-马夸尔特方法
- 高斯牛顿法
  - 将f(x)进行一阶泰勒展开，而不是目标函数F(x)。最终可得到： $J(x)J^T(x) \triangle x = -J(x)f(x)$
  - 上个方程是关于变量 $\triangle x_k$ 的线性方程组，我们称它为增量方程，也可以成为高斯牛顿方程或正规方程。
  - 将左边的系数成为H，右边定义为g，上式子就变成： $H\triangle x = g$
  - 高斯牛顿法用 $JJ^T$ 作为牛顿法中二阶海塞矩阵的近似。
  - 求解增量方程 $H\triangle x = g$ 是整个优化问题的核心所在。
  - 缺点：求解需要H可逆，但H只有半正定性，可能出现奇异矩阵或病态情况，因此稳定性较差导致算法不收敛。
  - 还有一些变种方法一定程度修正了它的缺点：线搜索方法、列文伯格-马夸尔特方法（阻尼牛顿法，收敛速度慢）。
- 列文伯格-马夸尔特方法
  - 给 $\triangle x_k$ 增加一个范围，称为信赖区域，定义了什么情况下二阶近似有效。也称为信赖区域法。
  - 高斯牛顿基础上，加上信赖区域，求解 $\min\limits_{\triangle x_k} \frac{1}{2}||f(x_k)+J(x_k)^T \triangle x_k||^2, s.t. ||D\triangle x_k||^2 < \mu$ ， $\mu$ 是信赖区域，D是系数矩阵。
    - 我们将增量限定与一个半径为 $\mu$ 的球中，认为只有在这个球内有效，带上D之后，这个球可以看成一个椭球。
      - 列文伯格优化方法：把D取成单位阵I，相当于直接把 $\triangle x_k$ 约束在一个球中。
      - 马夸尔特优化方法：将D取成非负数对角阵——实际中通常用 $JJ^T$ 的对角元素平方根，使得在梯度小的维度上约束范围更大一些。
    - 若求解该带不等式约束的优化问题，我们用拉格朗日乘子把约束项放到目标函数中，构成拉格朗日函数： $(\triangle x_k, \lambda) = \frac{1}{2}||f(x_k)+J(x_k)^T \triangle x_k||^2 + \frac{\lambda}{2}(||D\triangle x_k||^2-\mu)$
      - $\lambda$ 为拉格朗日乘子
      - 令该拉格朗日函数关于 $\triangle x_k$ 的导数为0，核心仍是计算增量的线性方程： $(H + \lambda D^TD)\triangle x_k=g$ 。若D=I，则相当于求解： $(H + \lambda I)\triangle x_k=g$ ，相当于在H对角线 $\lambda$ ，增强H的正定性。
  - 迭代中如何确定信赖区域范围？根据近似模型与实际函数之间的差异。
    - 定义指标 $\rho$ 刻画近似好坏： $\rho = \tfrac{f(x+\triangle x)-f(x)}{J(x)^T\triangle x}$ 。如果 $\rho$ 接近1，近似是好的。 $\rho$ 太小，需要缩小近似范围； $\rho$ 太大，可以放大近似范围。
- Dog-Leg
如何求解线性增量方程？
- 矩阵求逆，运算量大。
- 数值求解。
- 矩阵分解：QR、Cholesky
在最优计算时，初始值的设置，SLAM一般会通过ICP、PnP提供初始值。

3、概念和原理知识

最大似然估计（Maximum Likelihood Estimation, MLE）是一种用统计模型参数估计方法。通过最大化似然函数来找到最符合观测数据的参数值。一般为了简化计算，通常对似然函数取对数，然后最大化对数似然函数。
拉格朗日乘数法：在优化问题中，拉格朗日乘数法是处理带约束优化问题的一种强大方法。通过引入拉格朗日乘数，将约束优化问题转化为无约束优化问题。通过求解拉格朗日函数的驻点并结合KKT条件，找到最优解。

4、编译代码踩坑记录

安装ceres库，cmake提示版本过低，换了一个ceres代码版本解决。参考Ubuntu18.04安装Ceres，图文详解_ceres安装-CSDN博客
安装g2o库，首先要安装依赖库，参考ubuntu18.04安装g2o-CSDN博客，遇到“E: Package 'libqglviewer-dev' has no installation candidate ”。参考E: 软件包 libqglviewer-dev 没有可安装候选解决方法-CSDN博客解决。g2o cmake也提示版本过低，也换了一个g2o代码解决。

喵喵不爱吃鱼

关注

12
点赞
踩
21

收藏

觉得还不错? 一键收藏
0
评论
第六讲非线性优化

拉格朗日乘数法：在优化问题中，拉格朗日乘数法是处理带约束优化问题的一种强大方法。对于不方便直接求解的最小二乘问题，通过迭代方法，从一个初始值出发，不断更新当前的优化变量，使目标函数下降。而在SLAM中实用方法通常是折衷的手段，比如固定一些历史轨迹，仅对当前时刻附近的一些轨迹进行优化，即。在噪声影响下，希望通过带噪声的数据z和u推断位姿x和地图y，这构成了一个。在最优计算时，初始值的设置，SLAM一般会通过ICP、PnP提供初始值。的球中，认为只有在这个球内有效，带上D之后，这个球可以看成一个椭球。
复制链接

扫一扫