视觉slam十四讲学习笔记（五）非线性优化

已经知道，方程中的位姿可以由变换矩阵来描述，然后用李代数进行优化。观测方程由相机成像模型给出，其中内参是随相机固定的，而外参则是相机的位姿。

---

目录

前言

一、状态估计问题

1 最大后验与最大似然

2 最小二乘的引出

二、非线性最小二乘

1 一阶和二阶梯度法

2 Gauss-Newton

3 Levenberg-Marquadt

三、实践：Ceres

1 Ceres 简介

2 安装 Ceres

3 使用 Ceres 拟合曲线

四、实践：g2o

1 图优化理论简介

2 g2o 的编译与安装

3 使用 g2o 拟合曲线

总结

---

前言

1. 理解最小二乘法的含义和处理方式。

2. 理解 Gauss-Newton, Levenburg-Marquadt 等下降策略。

3. 学习 Ceres 库和 g2o 库的基本使用方法。

哔哩哔哩课程链接：视觉SLAM十四讲ch6_哔哩哔哩_bilibili

---

一、状态估计问题

1 最大后验与最大似然

SLAM经典模型：

考虑数据受噪声的影响后，会发生什么改变。在运动和观测方程中，通常假设两个噪声项 wk, vk,j 满足零均值的高斯分布：

在这些噪声的影响下，希望通过带噪声的数据 z 和 u，推断位姿 x 和地图 y（以及它们的概率分布），这构成了一个状态估计问题。

在视觉SLAM（Simultaneous Localization and Mapping，即同时定位与地图构建）中，最大后验估计（Maximum A Posteriori Estimation，MAP）和最大似然估计（Maximum Likelihood Estimation，MLE）是两种常用的参数估计方法。

1. 最大似然估计（MLE）：

   最大似然估计是一种基于观测数据来估计模型参数的方法。在SLAM中，模型参数通常表示相机的位姿、地图中特征点的位置等。MLE的目标是选择参数值，使得观测数据出现的概率最大。数学上，这可以表示为找到参数θ，使得观测数据D的似然函数P(D|θ)最大化。

   在SLAM中，最大似然估计通常基于一个测量方程和运动方程，考虑传感器测量和运动控制输入。然后，通过最大化这些方程的联合概率，可以得到对系统状态（相机位姿、地图）的估计。

2. 最大后验估计（MAP）：

   最大后验估计考虑了先验知识，即在考虑观测数据之前对参数的已知信息。在SLAM中，先验信息可能来自于地图的初始估计、相机的初始位姿等。最大后验估计的目标是找到在给定观测数据的条件下，使得后验概率P(θ|D)最大的参数值。

   最大后验估计可以看作是在最大似然估计的基础上引入了先验概率，通过贝叶斯定理来计算后验概率。数学上，这可以表示为找到参数θ，使得P(θ|D) ∝ P(D|θ) * P(θ) 最大化。

在实际应用中，选择使用最大似然估计还是最大后验估计取决于问题的性质以及可用的先验信息。当有可靠的先验信息时，最大后验估计可能更合适；否则，最大似然估计可能是一个更简单的选择。

SLAM 可以看作是图像具有时间先后顺序的，需要实时求解一个 SfM 问题。为了估计状态变量的条件分布，利用贝叶斯法则，有：

贝叶斯法则左侧通常称为后验概率。它右侧的 P(z|x) 称为似然，另一部分 P(x) 称为先验。直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下，后验概率最大化（Maximize a Posterior，MAP），则是可行的：

请注意贝叶斯法则的分母部分与待估计的状态 x 无关，因而可以忽略。根据贝叶斯法则，求解最大后验概率，相当于最大化似然和先验的乘积。进一步，当然也可以不知道机器人位姿大概在什么地方，此时就没有了先验。那么，可以求解x 的最大似然估计（Maximize Likelihood Estimation, MLE）：

直观地说，似然是指“在现在的位姿下，可能产生怎样的观测数据”。由于知道观测数据，所以最大似然估计，可以理解成：“在什么样的状态下，最可能产生现在观测到的数据”。这就是最大似然估计的直观意义。

2 最小二乘的引出

在高斯分布的假设下，最大似然能够有较简单的形式。回顾观测模型，对于某一次观测：

假设了噪声项 vk ∼ N (0, Qk,j )，所以观测数据的条件概率为：

它依然是一个高斯分布。为了计算使它最大化的 xk, yj，往往使用最小化负对数的方式，来求一个高斯分布的最大似然。

高斯分布在负对数下有较好的数学形式。考虑一个任意的高维高斯分布 x ∼ N(µ