视觉slam第六讲笔记：非线性优化、最小二乘、Gauss-Newton、LM、Ceres、g2o

视觉SLAM第六讲笔记

一.回顾：

如何在有噪声的数据中进行准确的状态估计？

根据第二讲的运动和观测模型
$$x_k=f(x_{k-1},u_k)+w_k$$$$z_{k,j}=h(y_j,x_k)+v_{k,j}$$
其中，$w_k，v_{k,j}$为噪声
根据第五讲的李代数 ，可以将观测方程改写为：$s{z}_{k,j}=Kexp(\hat{\xi })y_i$
假设$w_k，v_{k,j}$噪声服从零均值高斯分布

$$w_k\sim N(0,R_k),v_k\sim N(0,Q_{k,j})$$

状态估计问题

问题：所有未知量放在状态变量中 $x=\{x_1,...,x_N,y_1,...,y_M\}$，
已知输入 $u$ 和观测数据 $z$ ,求解 $x$ 的条件概率分布：
$$P(x|z,u)$$若没有运动，只有观测，则为$P(x|z)$.根据贝叶斯法则,且$P(z)$与$x$无关，所以：
$$P(x|z)=\frac{P(z|x)P(x)}{P(z)}\propto P(z|x)P(x)$$$$后验=似然\ast 先验$$后验：$P(x|z)$:在 观测值 $z$ 发生的情况下，出现 状态 $x$ 的概率
似然：$P(z|x)$:在 $z$ 发生的情况下，出现 $x$ 的概率
先验： $P(x)$
问题转化为后验概率最大化MAP：
$$x_{MAP}^{\ast }=argmaxP(x|z)=argmaxP(z|x)P(x)$$若先验$P(x)$未知，则问题转化为最大似然估计MAP(Maximize a Posterior):
$$x_{MLE}^{\ast }=argmaxP(z|x)$$意义：在什么样的状态下，最有可能产生现在观测到的数据。

二、最小二乘

经过一系列推导…看书去
问题转化为最小化二次型的问题：
$$x^{\ast }=argmin((z_{k,j}-h(x_k,y_j){)}^T{Q}_{k,j}^{-1}((z_{k,j}-h(x_k,y_j)))$$因此对于所有的运动和任意的观测，定义数据与估计值之间的误差为
$$e_{v,k}=x_k-f(x_{k-1},u_k)$$$$e_{y,j,k}=z_{k,j}-h(x_k,y_j)$$求该误差的平方和：
$$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}$$$$w_k\sim N(0,R_k),v_k\sim N(0,Q_{k,j})$$于是得到最小二乘问题(Least Square Problem),最小二乘的最优解等价于状态的最大似然估计。

非线性最小二乘：
$$\underset{x}{\min }||f(x)|{|}_2^2$$直接求该函数的导数，找使之最小的$x$，不好算。方法：迭代，不断寻找梯度并下降

1.给定某个初始值$x_0$
2.对于第k次迭代，寻找一个增量$\Delta x_k$,使得$||f(x_k+\Delta x_k)|{|}_2^2$达到极小
3.若$\Delta x_k$足够小，则停止
4.否则，令$x_{k+1}=x_k+\Delta x_k$,返回第2步

三.解决下降方向$\Delta x_k$如何确定的问题

3.1 一阶和二阶梯度法

对$f(x{)}^2$进行泰勒展开：
$$||f(x_k+\Delta x_k)|{|}_2^2\approx ||f(x)|{|}_2^2+J(x)\Delta x_k+\frac{1}{2}\Delta x_k^{T}H\Delta x_k$$ $J$：雅各比矩阵，$||f(x)|{|}_2^2$一阶导；
$H$：海塞矩阵，$||f(x)|{|}_2^2$二阶导

3.1.1 一阶梯度=最速下降法

$$\Delta x=-J^T(x)$$意义：沿着反向梯度方向前进 ，通常计算该方向上的一个ie步长，求得最快的下降方式

3.1.2 二阶梯度=牛顿法

$$H\Delta x=-J^T$$

3.2 高斯牛顿法：

对$f(x)$进行泰勒展开：
$$f(x+\Delta x)\approx f(x)+J(x)\Delta x$$$J$：雅各比矩阵，$f(x)$关于x的一阶导；
最小二乘问题转化为 : 求下降矢量$\Delta x$，使$||f(x+\Delta x_k)|{|}^2$最小。
$$\Delta x=arg\underset{\Delta x}{\min }||f(x)+J(x)\Delta x|{|}^2$$将函数展开，令其关于$\Delta x$的导数为0，得到增量方程(高斯牛顿方程、正规方程)：
$$J(x{)}^{T}J(x)\Delta x=J(x{)}^{T}f(x)$$简化写为
$$H\Delta x=g$$比起牛顿法求二阶导数的H，高斯牛顿法的H算到一阶导数乘法即可。
高斯牛顿法的关键在于求解增量方程

算法步骤：

1.给定某个初始值$x_0$
2.对于第k次迭代，计算当前的$J(x_k)$和$f(x_k)$
3.求解增量方程$H\Delta x=g$
4.若$\Delta x$足够小，则停止；否则，令$x_{k+1}=x_k+\Delta x_k$,返回第2步

缺点：实际求解中的 $H$ 不是正规矩阵，算法稳定性差且不收敛。

3.3 Levenberg–Marquardt法

信赖区域方法：泰勒展开只能在展开点附近有较好的近似效果，故需要给$\Delta x$添加一个信赖区域(Trust Region)，认为信赖区域内近似有效。
信赖区域的确定：根据近似模型和实际函数之间的差异确定，若差异小，则让范围大；若差异大，则让范围小。
$$\rho =\frac{f(x+\Delta x)-f(x)}{J(x)\Delta x}$$
$f(x+\Delta x)-f(x)$ ： 实际下降值
$J(x)\Delta x$ ： 近似下降值
若 $\rho$ 近似 $1$，效果好；
若 $\rho$ 太小，采用的近似下降太大，需要缩小近似范围；
若 $\rho$ 太大，采用的近似下降太小，需要扩大近似范围；

算法步骤：

1.给定初始值 $x_0$，初始优化半径 $\mu$
2.对于第k次迭代，求解$$\Delta x_k=arg\underset{\Delta x_k}{\min }||f(x_k)+J(x_k)\Delta x_k|{|}^2,s.t.||D\Delta x_k||⩽\mu$$

3.计算$\rho$
4.若$\rho$ > $\frac{3}{4}，则\mu =2\mu$
5.若$\rho$ < $\frac{1}{4}，则\mu =0.5\mu$
6.若$\rho$ > threshold，则近似可行，令$x_{k+1}=x_k+\Delta x_k$
7.判断算法是否收敛，不收敛则返回第二步，否则结束

将有约束的问题转为无约束的问题 (拉格朗日乘子$\lambda$)
$$\Delta x_k=arg\underset{\Delta x_k}{\min }||f(x_k)+J(x_k)\Delta x_k|{|}^2,s.t.||D\Delta x_k||⩽\mu$$

$$\Delta x_k=arg\underset{\Delta x_k}{\min }||f(x_k)+J(x_k)\Delta x_k|{|}^2+\frac{\lambda }{2}||D\Delta x_k||$$化为增量方程的形式：
$$(H+\lambda D^{T}D)\Delta x=g$$

Levenberg设D为单位阵，将$\Delta x$约束在球里
Marquardt设D为非负数对角阵，实际中常用$J^{T}J$的对角元素平方根。
$$(H+\lambda I)\Delta x=g$$
$\lambda$小，二次近似效果好，LM法类似高斯牛顿法
$\lambda$大，二次近似效果差，LM法类似一阶梯度下降

四. Ceres优化库

已知方程曲线$y=exp(a{x}^2+bx+c)+w$的N个关于x，y的观测点，如何求a，b，c？
将问题转化为求解最小二乘问题：
$$\underset{a,b,c}{\min }||y_i-exp(a{x}_i^2+b{x}_i+c)|{|}^2$$

第一步：定义一个代价函数的结构体struct
第二步：增加一个残差块（Problem.AddResidualBlock）将误差项加入到问题中
第三步：指定误差项和优化变量的纬度
第四步：问题求解（Problem.Solve）

cmake_minimum_required(VERSION 3.14)
project(useceres)
set(CMAKE_CXX_STANDARD 14)

find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )

find_package( Ceres REQUIRED )
include_directories( ${CERES_INCLUDE_DIRS} )

include_directories( /usr/include/eigen3)
include_directories( /usr/local/include/ceres)

add_executable(useceres main.cpp)
target_link_libraries( useceres ${OpenCV_LIBS} ${CERES_LIBRARIES} )

#include 
#include 
#include 

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, T* residual ) const{
        //y-exp(ax^+bx+c)
        residual[0] = T(_y) -ceres::exp(abc[0]*T(_x)*T(_x)+ abc[1]*T(_x) + abc[2]);
        return true;
    }
    const double _x,_y;
};


int main() {
    double a=1.0, b=2.0, c=1.0;    // abc真实值
    int N=100;
    double v_sigma = 1.0;
    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(v_sigma));
    }

    // 构建最小二乘问题
    ceres::Problem problem;
    for (int i = 0; i < N; ++i) {
        problem.AddResidualBlock( //向问题中添加误差项
            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; // 增量方程采用QR求解方式
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;               //优化信息
    ceres::Solve( options, &problem, &summary);   //开始优化
    cout<< summary.BriefReport()<<endl;           //输出结果
    cout<< "estimated a,b,c = ";
    for( auto a:abc) cout<< a<< " ";
    cout << endl;


    return 0;
}

查看结果

build/curve_fitting

五.g2o库General Graph Optimization

图优化：是把优化问题表现成图的形式，通常分解为两个任务：

• 1.构建图。机器人位姿作为顶点，位子关系作为边。
• 2.优化图。调整机器人的位子（顶点）来尽量满足边的约束，使得误差最小。

5.1 图解g2o框架

核心（SparseOptimizer）：是OptimizableGraph，也是HyperGraph（超图）。
顶点（HyperGraph::Vertex）：优化变量，继承于OptimizableGraph::Vertex(也是Base Vertex)。
边(HyperGraph::Edge)：误差项，继承于OptimizableGraph::Edge(也是单边（BaseUnaryEdge）、双边（BaseBinaryEdge）、多边（BaseMultiEdge）)。
优化算法OptimizationAlgorithm是通过OptimizationWithHessian实现的，其中有迭代方法GaussNewton，LM，Powell’s dogleg。
求解器Solver：由BlockSolver组成，其中BlockSolver包含两部分，分别是：
SparseBlockMatrix:用于计算稀疏的雅可比矩阵 $J$ 和Hessian矩阵 $H$
LinearSolver:是线性求解器，用于计算迭代过程中最关键的$H\Delta x=-b$，方法包括有PCG，CSparse，Choldmod

5.2 g2o编程

5.2.1 源码:

#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 
#include 
#include 
#include 

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;

};

int main() {
    double a=1.0, b=2.0, c=1.0;
    int N=100;
    double w_sigma = 1.0;
    cv::RNG rng;
    double abc[3]= {0,0,0};

    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
    // 矩阵块： 每个误差项优化变量纬度为3， 误差值纬度为1
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1>> Block;
    std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverDense<Block::PoseMatrixType>() );
    std::unique_ptr<Block> solver_ptr (new Block( std::move(linearSolver) ));
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::move(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;
    optimizer.initializeOptimization();
    optimizer.optimize(200);

    //输出优化值
    Eigen::Vector3d abc_estimate = v->estimate();
    cout<< "estimate model:" <<abc_estimate.transpose()<<endl;
    return 0;
}

5.2.2 代码框架解析

5.2.2.1代码框架解析

1. 定义顶点类和边类：（其中顶点类定义了虚函数：oplusImpl:用于顶点更新，计算$x_{k+1}=x_k+\Delta x$,当该计算不位于向量空间时，需要自定义增量如何加到现有估计上，比如：按照扰动模型，需要用左乘更新或右乘更新；此外还定义了重置函数：setToOriginImpl,此处将估计值设为0。）------（其中边类定义了误差计算函数：computeError：取出边所连接的顶点的当前估计值，根据曲线模型，与其观测值进行比较，也就是误差模型）----（二者都定义了存盘和读盘函数：read、write，用于处理读写操作，此处为空）
2. 随即生成N对数据x,y
3. 构建求解器、优化器（SparseOptimizer）
4. 将顶点和边添加到SparseOptimizer中
5. 设置优化参数，开始执行优化

5.2.2.2 源码中g2o的核心部分

1. 创建一个线性求解器LinearSolver
2. 创建BlockSolver。并用LinearSolver初始化
3. 创建总求解器solver。选择GN, LM或 DogLeg，并用BlockSolver初始化
4. 创建g2o核心（SparseOptimizer）
5. 定义图的顶点和边。并添加到SparseOptimizer中
6. 设置优化参数，开始执行优化

// 每个误差项优化变量维度为3，误差值维度为1
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  
// 第1步：创建一个线性求解器LinearSolver
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); 

// 第2步：创建BlockSolver。并用上面定义的线性求解器初始化
Block* solver_ptr = new Block( linearSolver );      

// 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );

// 第4步：创建终极大boss 稀疏优化器（SparseOptimizer）
g2o::SparseOptimizer optimizer;     // 图模型
optimizer.setAlgorithm( solver );   // 设置求解器
optimizer.setVerbose( true );       // 打开调试输出

// 第5步：定义图的顶点和边。并添加到SparseOptimizer中
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 );
}

// 第6步：设置优化参数，开始执行优化
optimizer.initializeOptimization();
optimizer.optimize(100);

具体g2o内部详细结构可以参考：
从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码

小结

将位姿估计问题转为最小二乘问题，然后用不同的方法(梯度下降法、高斯牛顿法、LM法)来求解下降的步长。分别用ceres和g2o实现非线性优化。

参考

[1]: Xiang Gao, Tao Zhang, Yi Liu, Qinrui Yan, 14 Lectures on Visual SLAM: From Theory to Practice, Publishing House of Electronics Industry, 2017.
[2]: 视频教程
[3]: 高斯牛顿(Gauss Newton)、列文伯格-马夸尔特(Levenberg-Marquardt)最优化算法
[4]: 深入理解图优化与g2o：图优化篇
[5]:从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码

强烈安利《视觉slam十四讲》，这是极好的一本书