视觉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）是两种常用的参数估计方法。

最大似然估计（MLE）：

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

在SLAM中，最大似然估计通常基于一个测量方程和运动方程，考虑传感器测量和运动控制输入。然后，通过最大化这些方程的联合概率，可以得到对系统状态（相机位姿、地图）的估计。
最大后验估计（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(µ, Σ)，它的概率密度函数展开形式为:

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

该式等价于最小化噪声项（即误差）的平方（Σ 范数意义下）。因此，对于所有的运动和任意的观测，我们定义数据与估计值之间的误差：

这就得到了一个总体意义下的最小二乘问题（Least Square Problem）。

直观来讲，由于噪声的存在，当我们把估计的轨迹与地图代入 SLAM 的运动、观测方程中时，它们并不会完美的成立。这时候怎么办呢？把状态的估计值进行微调，使得整体的误差下降一些。当然这个下降也有限度，它一般会到达一个极小值。这就是一个典型非线性优化的过程。

SLAM 中的最小二乘问题具有一些特定的结构：

• 首先，整个问题的目标函数由许多个误差的（加权的）平方和组成。虽然总体的状态变量维数很高，但每个误差项都是简单的，仅与一两个状态变量有关。例如运动误差只与 xk−1, xk 有关，观测误差只与 xk, yj 有关。每个误差项是一个小规模的约束，之后会谈论如何对它们进行线性近似，最后再把这个误差项的小雅可比矩阵块放到整体的雅可比矩阵中。由于这种做法，称每个误差项对应的优化变量为参数块（Parameter Block）。

• 整体误差由很多小型误差项之和组成的问题，其增量方程的求解会具有一定的稀疏性（会在第十讲详细讲解），使得它们在大规模时亦可求解。

• 其次，如果使用李代数表示，则该问题是无约束的最小二乘问题。但如果用旋转矩阵（变换矩阵）描述位姿，则会引入旋转矩阵自身的约束（旋转矩阵必须是正交阵且行列式为 1）。额外的约束会使优化变得更困难。这体现了李代数的优势。

• 最后，使用了平方形式（二范数）度量误差，它是直观的，相当于欧氏空间中距离的平方。但它也存在着一些问题，并且不是唯一的度量方式。亦可使用其他的范数构建优化问题。

二、非线性最小二乘

先来考虑一个简单的最小二乘问题：

这里自变量 x ∈ R n，f 是任意一个非线性函数，我们设它有 m 维：f(x) ∈ R m。下面讨论如何求解这样一个优化问题。

如果 f 是个数学形式上很简单的函数，那问题也许可以用解析形式来求。令目标函数的导数为零，然后求解 x 的最优值，就和一个求二元函数的极值一样：

解此方程，就得到了导数为零处的极值。它们可能是极大、极小或鞍点处的值，只要挨个儿比较它们的函数值大小即可。

对于不方便直接求解的最小二乘问题，我们可以用迭代的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降。具体步骤可列写如下：

1 一阶和二阶梯度法

最速下降法（Steepest Descent Method），也称为梯度下降法，是一种常用的优化算法，用于求解无约束优化问题。该算法的目标是通过沿着目标函数梯度的反方向迭代来寻找函数的局部最小值。

下面是最速下降法的基本步骤：

初始化：选择初始点 x0 和迭代次数 0k=0。
计算梯度：计算目标函数 f(xk) 在当前点 xk 处的梯度，即gk=∇f(xk)。
选择步长：确定一个适当的步长或学习率 αk。这个步长通常是一个正数，它决定了在梯度方向上迭代的距离。
更新参数：更新参数 xk+1=xk−αkgk。这个更新规则沿梯度的反方向减小目标函数值。
收敛判断：检查收敛条件，如果满足停止条件则停止迭代，否则返回步骤 2。

重复步骤 2 到步骤 5，直到达到预定的停止条件，如目标函数收敛到一个小的值或迭代次数达到限制。

最速下降法的优点是简单易实现，但缺点是可能收敛速度较慢，特别是在目标函数具有弯曲形状的情况下。在实际应用中，可以结合其他优化算法或调整学习率来改善收敛性能。

2 Gauss-Newton

Gauss Newton 是最优化算法里面最简单的方法之一。它的思想是将 f(x) 进行一阶的泰勒展开（请注意不是目标函数 f(x) 2）：

这里 J(x) 为 f(x) 关于 x 的导数，实际上是一个 m × n 的矩阵，也是一个雅可比矩阵。根据前面的框架，当前的目标是为了寻找下降矢量 ∆x，使得 ∥f (x + ∆x)∥ 2 达到最小。为了求 ∆x，需要解一个线性的最小二乘问题：

根据极值条件，将上述目标函数对 ∆x 求导，并令导数为零。由于这里考虑的是 ∆x 的导数（而不是 x），最后将得到一个线性的方程。为此，先展开目标函数的平方项：

注意，要求解的变量是 ∆x，因此这是一个线性方程组，我们称它为增量方程，也可以称为高斯牛顿方程 (Gauss Newton equations) 或者正规方程 (Normal equations)。把左边的系数定义为 H，右边定义为 g，那么上式变为：

这里把左侧记作 H 是有意义的。对比牛顿法可见，Gauss-Newton 用 J T J 作为牛顿法中二阶 Hessian 矩阵的近似，从而省略了计算 H 的过程。求解增量方程是整个优化问题的核心所在。如果能够顺利解出该方程，那么 Gauss-Newton 的算法步骤可以写成：

3 Levenberg-Marquadt

由于 Gauss-Newton 方法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以很自然地想到应该给 ∆x 添加一个信赖区域（Trust Region），不能让它太大而使得近似不准确。非线性优化种有一系列这类方法，这类方法也被称之为信赖区域方法 (Trust Region Method)。在信赖区域里边，我们认为近似是有效的；出了这个区域，近似可能会出问题。

那么如何确定这个信赖区域的范围呢？一个比较好的方法是根据我们的近似模型跟实际函数之间的差异来确定这个范围：如果差异小，我们就让范围尽可能大；如果差异大，就缩小这个近似范围。因此，考虑使用：

来判断泰勒近似是否够好。ρ 的分子是实际函数下降的值，分母是近似模型下降的值。如果 ρ 接近于 1，则近似是好的。如果 ρ 太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果 ρ 比较大，则说明实际下降的比预计的更大，可以放大近似范围。

于是，构建一个改良版的非线性优化框架，该框架会比 Gauss Newton 有更好的效果：

三、实践：Ceres

1 Ceres 简介

Ceres Solver是一个开源的C++库，用于求解大规模的非线性最小二乘问题。它由Google开发并维护，旨在提供高效、灵活且易于使用的工具，用于解决各种优化问题，特别是在计算机视觉、机器人学和地图构建等领域中的应用。

以下是Ceres Solver的一些主要特点和功能：

非线性最小二乘问题求解：Ceres Solver专注于解决非线性最小二乘问题，这些问题可以描述为最小化残差的平方和。这种类型的问题在许多科学和工程领域中都很常见，如图像配准、结构从运动和参数估计等。
灵活的接口：Ceres提供了灵活的接口，使用户可以自定义优化问题的各个方面，包括残差函数、参数块、参数化方式以及优化算法。
自动求导：Ceres能够自动计算残差函数对参数的导数，这使得用户无需手动实现导数计算，简化了问题的定义和求解过程。
支持多种优化算法：Ceres提供了多种优化算法，包括最速下降法、Levenberg-Marquardt法、拟牛顿法等，用户可以根据问题的性质选择合适的算法。
可扩展性：Ceres具有良好的可扩展性，可以轻松地集成新的优化算法、残差类型和参数化方法。
跨平台性：Ceres可以在多种操作系统上运行，包括Linux、Windows和macOS，同时支持多种编译器，如GCC、Clang和MSVC。

总的来说，Ceres Solver为解决大规模非线性最小二乘问题提供了一个强大而灵活的工具，并在许多领域中得到了广泛的应用。

2 安装 Ceres

建议去 github 上下载 Ceres：https://github.com/ceres-solver/ceres-solver。

与之前碰到的库一样，Ceres 是一个 cmake 工程。先来安装它的依赖项，在 Ubuntu中都可以用 apt-get 安装，主要是谷歌自己使用的一些日志和测试工具：

sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags-dev libgoogle-glog-dev libgtest-dev

然后，进入 Ceres 库，使用 cmake 编译并安装它。这安装完成后，在/usr/local/include/ceres 下找到 Ceres 的头文件，并在/usr/local/lib/下找到名为 libceres.a 的库文件。有了头文件和库文件，就可以使用 Ceres进行优化计算了。

3 使用 Ceres 拟合曲线

假设有一条满足以下方程的曲线：

这部分我没有实操，大家可以看书一步一步走！

/home/yang/slam/slambook/ch6/ceres_curve_fitting/main.cpp

#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

// 代价函数的计算模型
struct CURVE_FITTING_COST
{
    CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
    // 残差的计算
    template <typename T>
    bool operator() (
        const T* const abc,     // 模型参数，有3维
        T* residual ) const     // 残差
    {
        residual[0] = T ( _y ) - ceres::exp ( abc[0]*T ( _x ) *T ( _x ) + abc[1]*T ( _x ) + abc[2] ); // y-exp(ax^2+bx+c)
        return true;
    }
    const double _x, _y;    // x,y数据
};

int main ( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据

    cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }

    // 构建最小二乘问题
    ceres::Problem problem;
    for ( int i=0; i<N; i++ )
    {
        problem.AddResidualBlock (     // 向问题中添加误差项
        // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
            new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> ( 
                new CURVE_FITTING_COST ( x_data[i], y_data[i] )
            ),
            nullptr,            // 核函数，这里不使用，为空
            abc                 // 待估计参数
        );
    }

    // 配置求解器
    ceres::Solver::Options options;     // 这里有很多配置项可以填
    options.linear_solver_type = ceres::DENSE_QR;  // 增量方程如何求解
    options.minimizer_progress_to_stdout = true;   // 输出到cout

    ceres::Solver::Summary summary;                // 优化信息
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve ( options, &problem, &summary );  // 开始优化
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

    // 输出结果
    cout<<summary.BriefReport() <<endl;
    cout<<"estimated a,b,c = ";
    for ( auto a:abc ) cout<<a<<" ";
    cout<<endl;

    return 0;
}

四、实践：g2o

1 图优化理论简介

用三角形表示相机位姿节点，用圆形表示路标点，它们构成了图优化的顶点；同时，蓝色线表示相机的运动模型，红色虚线表示观测模型，它们构成了图优化的边。也可以做去掉孤立顶点或优先优化边数较多（或按图论的术语，度数较大）的顶点这样的改进。但是最基本的图优化，是用图模型来表达一个非线性最小二乘的优化问题。可以利用图模型的某些性质，做更好的优化。

2 g2o 的编译与安装

可以从 github 下载它：https://github.com/RainerKuemmerle/g2o

sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev libsuitesparse-dev libcxsparse3.1.2 libcholmod-dev

然后，按照 cmake 的方式对 g2o 进行编译安装即可。安装完成后，g2o 的头文件将在/usr/local/g2o 下，库文件在/usr/local/lib/下。

3 使用 g2o 拟合曲线

在曲线拟合问题中，整个问题只有一个顶点：曲线模型的参数 a, b, c；而每个带噪声的数据点，构成了一个个误差项，也就是图优化的边。但这里的边与平时想的边不太一样，它们是一元边（Unary Edge），即只连接一个顶点——因为整个图只有一个顶点。所以在图中，就只能把它画成自己连到自己的样子了。事实上，图优化中一条边可以连接一个、两个或多个顶点，这主要反映在每个误差与多少个优化变量有关。在稍微有些玄妙的说法中，把它叫做超边（Hyper Edge），整个图叫做超图（Hyper Graph）。

这部分我没有实操，大家可以看书一步一步走！

cmake_minimum_required( VERSION 2.8 )
project( g2o_curve_fitting )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

# 寻找G2O
find_package( G2O REQUIRED )
include_directories( 
    ${G2O_INCLUDE_DIRS}
    "/usr/include/eigen3"
)

# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )

add_executable( curve_fitting main.cpp )
# 与G2O和OpenCV链接
target_link_libraries( curve_fitting 
    ${OpenCV_LIBS}
    g2o_core g2o_stuff
)

#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
using namespace std; 

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() // 重置
    {
        _estimate << 0,0,0;
    }
    
    virtual void oplusImpl( const double* update ) // 更新
    {
        _estimate += Eigen::Vector3d(update);
    }
    // 存盘和读盘：留空
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 计算曲线模型误差
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

int main( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据
    
    cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }
    
    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    // 梯度下降方法，从GN, LM, DogLeg 中选
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm( solver );   // 设置求解器
    optimizer.setVerbose( true );       // 打开调试输出
    
    // 往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );
    
    // 往图中增加边
    for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
        edge->setId(i);
        edge->setVertex( 0, v );                // 设置连接的顶点
        edge->setMeasurement( y_data[i] );      // 观测数值
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
        optimizer.addEdge( edge );
    }
    
    // 执行优化
    cout<<"start optimization"<<endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
    
    // 输出优化值
    Eigen::Vector3d abc_estimate = v->estimate();
    cout<<"estimated model: "<<abc_estimate.transpose()<<endl;
    
    return 0;
}