引言

ch6. 放在前面：节点为优化变量,边为误差项。

1.General Graph Optimization

在g2o中选择优化方法需三个步骤：

• 1.选择一个线性方程求解器:PCG, CSparse, Choldmod;
• 2.选择一个BlockSolver, 包含SparseBlockMatrix，用于计算稀疏雅可比和海塞；BlockSolver用于计算迭代过程中最关键的一步HΔx=−b为一个线性方程的求解器。
• 3.选择一个迭代策略:GN, LM, Doglog。

图是由顶点（Vertex）和边（Edge）组成的结构，而图论则是研究图的理论。我们记一个图为G={V,E}，其中V为顶点集，E为边集。
　　顶点想象成普通的点即可。边：一条边连接着若干个顶点，表示顶点之间的一种关系。边可以是有向的或是无向的，对应的图称为有向图或无向图。边也可以连接一个顶点（Unary Edge，一元边）、两个顶点（Binary Edge，二元边）或多个顶点（Hyper Edge，多元边）。最常见的边连接两个顶点。当一个图中存在连接两个以上顶点的边时，称这个图为超图（Hyper Graph）。

1.1.图优化的流程

• 1.选择想要的图里的节点与边的类型，确定它们的参数化形式；
• 2.往图里加入实际的节点和边；
• 3.选择初值，开始迭代；
• 4.每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；
• 5.求解稀疏线性方程HkΔx=−bk，得到梯度方向；
• 6.继续用GN或LM进行迭代。如果迭代结束，返回优化值。

实际上，g2o能帮助做好第3-6步，只需要做的只是前两步而已。

1.2.核函数

引入核函数的原因，是因为SLAM中可能给出错误的边。SLAM中的数据关联让科学家头疼了很长时间。出于变化、噪声等原因，机器人并不能确定它看到的某个路标，就一定是数据库中的某个路标。万一认错了呢？把一条原本不应该加到图中的边给加进去了，会怎么样？那优化算法可就慒逼了，它会看到一条误差很大的边，然后试图调整这条边所连接的节点的估计值，使它们顺应这条边的无理要求。由于这个边的误差真的很大，往往会抹平了其他正确边的影响，使优化算法专注于调整一个错误的值。
　　于是就有了核函数的存在。核函数保证每条边的误差不会大的没边，掩盖掉其他的边。具体的方式是，把原先误差的二范数度量，替换成一个增长没有那么快的函数，同时保证自己的光滑性质（不然没法求导）。因为它们使得整个优化结果更为鲁棒，所以又叫它们为robust kernel（鲁棒核函数）。
　　很多鲁棒核函数都是分段函数，在输入较大时给出线性的增长速率，例如cauchy核，huber核等等。核函数在许多优化环境中都有应用，有一大堆人在机器学习算法里加各种各样的核，现在常用的svm也会带个核函数。

2.g2oCurveFitting

图优化：

• 图优化
• G2oGithub
• 参考链接1
• 参考链接2
• 参考链接3
• 参考链接4
• 参考链接5

数值优化算法

• 原理介绍
• eigen3例子

题目：

为了使用 g2o,首先要做的是将曲线拟合问题抽象成图优化。这个过程中,只要记住节点为优化变量,边为误差项即可。（一元边：只连接一个顶点。）

扫描二维码关注公众号，回复： 6090829 查看本文章

代码步骤：

• 1.定义顶点
• 2.定义边
• 3.配置BlockSolver(LinerSolverType)：PCG, CSparse, Choldmod,Dense;
• 4.配置OptimizationAlgorithm：GN, LM, Doglog
• 5.配置Optimizer
• 6.添加顶点
• 7.添加边
• 8.启动优化

code：

#include <iostream>
#include <g2o/core/base_vertex.h>// 顶点类型
#include <g2o/core/base_unary_edge.h>//一元边类型
#include <g2o/core/block_solver.h>//求解器的实现。主要来自choldmod, csparse。在使用g2o时要先选择其中一种。
#include <g2o/core/optimization_algorithm_levenberg.h>//莱文贝格－马夸特方法（Levenberg–Marquardt algorithm）能提供数非线性最小化（局部最小）的数值解。
#include <g2o/core/optimization_algorithm_gauss_newton.h>//高斯牛顿法
#include <g2o/core/optimization_algorithm_dogleg.h>//Dogleg（狗腿方法）
#include <g2o/solvers/linear_solver_dense.h>//
#include <Eigen/Core>//矩阵库
#include <opencv2/core/core.hpp>//opencv2
#include <cmath>//数学库
#include <chrono>//时间库

/**
 
#########################
 牛顿法：
 求 f(x)=0
 f(x+det) = f(x)  + f'(x)*det=0  一阶泰勒展开
 det = x - x0 = -f(x0)/f'(x0)    
 迭代公式：
 
 xn  =  xn-1 -  f(xn-1)/f'(xn-1)          通过迭代将零点邻域内一个任选的点收敛至该零点 
#########################
 牛顿下山法：
 xn  =  xn-1 -  w * f(xn-1)/f'(xn-1)    w =1,. 逐次减半..，0    调整步长逐步降低  避免跳过最优接
 
#########################
 最优化问题
 min(f(x))     求 f'(x)=0
 xn =  xn-1 -  f'(xn-1)/f''(xn-1)      迭代公式   
 
#########################
 高斯牛顿法
在讲牛顿法的时候，我们举的例子x是一维的，若如果我们遇到多维的x该如何办呢？
这时我们就可以利用雅克比，海赛矩阵之类的来表示高维求导函数了。
比如
f(X)=0,其中X=[x0,x1,...,xn]
所以我们有雅克比矩阵 Jf：  n*n   第一列 f对x0偏导   第n列 f对xn求偏导      一阶偏导数
有海赛矩阵 Hf：                       n*n  对应行列位置ij   f对xi 和 xj 的偏导             二阶偏导

所以高维牛顿法解最优化问题又可写成：
Xn+1=Xn −  (Hf(xn))逆 * Jf * f(xn)

梯度 ∇ Jf   代替了低维情况中的一阶导   
Hessian矩阵代替了二阶导
求逆代替了除法
xn  =  xn-1 -  1/f'(xn-1) * f(xn-1)
而近似下有  Hf(xn)  = Jf 转置 * Jf 
##########################
xn  =  xn-1 -   ( Jf 转置 * Jf )逆 * Jf * f(xn-1)

##########################
Levenberg-Marquardt算法
莱文贝格－马夸特方法（Levenberg–Marquardt algorithm）能提供数非线性最小化（局部最小）的数值解。
此算法能借由执行时修改参数达到结合高斯-牛顿算法以及梯度下降法的优点，
并对两者之不足作改善（比如高斯-牛顿算法之反矩阵不存在或是初始值离局部极小值太远）

在我看来，就是在高斯牛顿基础上修改了一点。
在高斯牛顿迭代法中，我们已经知道
xn  =  xn-1 -   ( Jf 转置 * Jf )逆 * Jf * f(xn-1)

在莱文贝格－马夸特方法算法中则是
xn  =  xn-1 -   ( Jf 转置 * Jf + lamd * 单位矩阵  )逆 * Jf * f(xn-1)

然后Levenberg-Marquardt方法的好处就是在于可以调节:
如果下降太快，使用较小的λ，使之更接近高斯牛顿法
如果下降太慢，使用较大的λ，使之更接近梯度下降法


############################
Dogleg（狗腿方法）
http://blog.csdn.net/xyz599/article/details/54344354
 */

using namespace std; 
/**
 * 详解 https://www.cnblogs.com/gaoxiang12/p/5304272.html
 * 代码 双目BA实例 https://github.com/gaoxiang12/g2o_ba_example
 g2o全称general graph optimization，是一个用来优化非线性误差函数的c++框架。
 SparseOptimizer 是我们最终要维护的东东。它是一个Optimizable Graph 优化图，从而也是一个 Hyper Graph 超图。
 一个 SparseOptimizer 含有很多个顶点(HyperGraph::Vertex) （都继承自 BaseVertex<D,T> --> OptimizableGraph::Vertex ）和
 很多个边(HyperGraph::Edge)（继承自 一元边BaseUnaryEdge, 二元边BaseBinaryEdge 或 多元边BaseMultiEdge —-> OptimizableGraph::Edge）。
 这些 BaseVertex 和 Base Edge 都是抽象的基类，而实际用的顶点和边，都是它们的派生类。
 我们用 SparseOptimizer.addVertex 和  SparseOptimizer.addEdge 向一个图中添加顶点和边，
 最后调用 SparseOptimizer.optimize 完成优化。
 
 在优化之前，需要指定我们用的求解器和迭代算法。
 一个 SparseOptimizer 拥有一个 迭代算法 OptimizationAlgorithm，
 继承自Gauss-Newton,   (OptimizationAlgorithmGaussNewto)
 Levernberg-Marquardt,(OptimizationAlgorithmLevenberg)
 Powell's dogleg      ( OptimizationAlgorithmDogleg )三者之一（我们常用的是GN或LM）。
 
 同时，这个 Optimization Algorithm 拥有一个 求解器 Solver，它含有两个部分。
 一个是 SparseBlockMatrix ，用于计算稀疏的雅可比和海塞；
 一个是用于计算迭代过程中最关键的一步 
      H * Δx = −b
 这就需要一个线性方程的求解器。而这个求解器，可以从 PCG, CSparse, Choldmod 三者选一。
 
 综上所述，在g2o中选择优化方法一共需要三个步骤：
    选择一个线性方程求解器，从 PCG, CSparse, Choldmod中选，实际则来自 g2o/solvers 文件夹中定义的东东。
    选择一个 BlockSolver 求解 雅克比和海塞矩阵 。
    选择一个迭代策略，从GN, LM, Doglog中选。

 
 */
///节点为优化变量,边为误差项
///步骤1.定义顶点。待优化变量——曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>//定点类型  a b c三维变量
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 类成员 有Eigen  变量时需要 显示 加此句话 宏定义
    virtual void setToOriginImpl()  // 虚函数 重置
    {
        _estimate << 0,0,0;         // 初始化定点  优化变量值初始化//estimate估计
    }  
    virtual void oplusImpl( const double* update ) // 更新
    {
        _estimate += Eigen::Vector3d(update);//迭代更新 变量
    }
    //虚函数  存盘和读盘：留空
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

///步骤1.定义边（误差项）
// 误差模型—— 曲线模型的边, 模板参数：观测值维度(输入的参数维度)，类型，连接顶点类型(创建的顶点)
/// 一元边 BaseUnaryEdge<1,double,CurveFittingVertex>
/// 二元边 BaseBinaryEdge<2,double,CurveFittingVertex>
/// 多元边 BaseMultiEdge<>
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex> //基础一元边  边类型(误差项)//继承
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW// 类成员 有Eigen  变量时需要 显示 加此句话 宏定义
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}//初始化函数   直接赋值  _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) ) ;
        ///一个误差项 _measurement为测量值；测量值 - 理论值
    }
    // 存盘和读盘：留空
    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;                          // 数据点，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]<<"\t"<<y_data[i]<<endl;
    }
    
    ////步骤2.配置BlockSolver(LinerSolverType)
    ///配置OptimizationAlgorithm
    ///配置Optimizer
    ///构建图优化解决方案，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  /// 顶点每个误差项优化变量维度为3，误差值维度为1（abc）
    // 线性方程求解器   H * Δx = −b
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
    // 稀疏矩阵块求解器 用于求解 雅克比J ( 得到右边 b = e转置 *  Ω * J ) 和  海塞矩阵 H  左边 H = J转置* Ω * J   
    Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    // 迭代算法:梯度下降方法，GN,  LM, DogLeg 中选;详看下面三行代码
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );  //LM
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );  //GN
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );  //DogLeg
    g2o::SparseOptimizer optimizer;     //稀疏 优化模型
    optimizer.setAlgorithm( solver );   // 设置求解器
    optimizer.setVerbose( true );       // 打开调试输出
    
    // 往图中增加顶点,
    CurveFittingVertex* v = new CurveFittingVertex();//曲线拟合 新建 顶点类型
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);  //id
    optimizer.addVertex( v );  //添加顶点
    
    // 往图中增加边，100个数据，增加100条边
    for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );//新建 边 带入 观测数据
        edge->setId(i);//id
        edge->setVertex( 0, v );           // 设置连接的顶点
        edge->setMeasurement( y_data[i] ); // 观测数值
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) );
        // 信息矩阵：单位阵协方差矩阵之逆 (个误差项权重)  就一个误差项_error(0,0)
        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;
}
//iteration  迭代

用的之前的g2o， 新版g2o比较庞大，不想安装，从orb-slam三方库文件夹中拷贝过来放在当前文件夹下面，之前编译过的，不用编译，直接修改CMakeLists.txt进行使用。

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
#工程名
project( g2o_curve_fitting )

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

# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )

# OpenCV 库文件
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS}  ${PROJECT_SOURCE_DIR}/g2o/ )

add_executable( curve_fitting main.cpp )
# 与G2O和OpenCV链接
target_link_libraries( curve_fitting 
    ${OpenCV_LIBS}
    ${PROJECT_SOURCE_DIR}/g2o/lib/libg2o.so
)