【slam十四讲第二版】【课本例题代码向】【第六讲~非线性优化】【安装对应版本ceres2.0.0和g2o教程】【手写高斯牛顿、ceres2.0.0、g2o拟合曲线及报错解决方案】

【slam十四讲第二版】【课本例题代码向】【第三~四讲刚体运动、李群和李代数】【eigen3.3.4和pangolin安装，Sophus及fim的安装使用】【绘制轨迹】【计算轨迹误差】
【slam十四讲】【课本例题代码向】【第五讲~相机与图像】【OpenCV、图像去畸变、双目和RGB-D、遍历图像像素14种】

0 前言

由于前面已经安装好一些类的配置（eigen3.3.4、pangolin、Sophus等等），安装教程可参考【slam十四讲】【第三讲和第四讲配置】eigen3.3.4和pangolin安装，Sophus及fim的安装使用

由于前面安装的时候已经吃了，slam14讲 提供的安装包版本不适配的问题，所以这里也敢确定，就不使用它所提供的了，

• 本文介绍slam14讲：

1. 第二本书第六讲后面的三个代码实现
2. ceres2.0.0 ，安装包自取链接：https://pan.baidu.com/s/1UdU9KsS5yKVG_aBrs6enBg 提取码：pd3u
   这里之所以选择2.0.0 很大一个原因是因为之后学习cartographer算法的时候就需要安装ceres2.0.0版本。防止版本冲突。事实证明，只要eigen版本在3.3.2之上，就可以安装ceres2.0.0，且对讲中使用
3. 如果你想尝试ceres1.14.0在这里可以不可以，那么你可以自己尝试，这里提高一下ceres安装包的下载链接链接https://pan.baidu.com/s/1hgFZRZMx8cz41b05h0MQcw 提取码：krfd

1 手写高斯牛顿法

1.1 gaussNewton.cpp

#include 
#include 
#include 
#include 
#include 

using namespace std;

int main(int argc,char ** argv) {
    double ar = 1.0, br = 2.0, cr = 1.0;  // true parameter value 真实参数值
    double ae = 2.0, be = -1.0, ce = 5.0; // estimated parameter value 估计参数值
    int N = 100; // the data points 数据点
    double w_sigma = 1.0; //sigma of noise 噪声sigma值
    double inv_sigma = 1.0 / w_sigma;
    cv::RNG rng; // OpenCV随机数产生器   OpenCV random number generator

    vector<double> x_data, y_data; //data 数据
    for (int i = 0; i < N; i++)
    {
        double x = i/100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar*x*x+br*x+cr) + rng.gaussian(w_sigma*w_sigma) );
    }

    //开始高斯牛顿迭代    start gauss-Newton iteration
    int iterations = 100;// the number of iterations 迭代次数
    double cost = 0, lastCost = 0;//本次迭代的cost和上一次迭代的cost   cost of the current iteration and cost of the previous iteration
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    for(int iter = 0; iter < iterations; iter++)
    {
        Eigen::Matrix3d H = Eigen::Matrix3d::Zero(); // Hession = J^T W^{-1} J in Gauss-Newton
        Eigen::Vector3d b = Eigen::Vector3d::Zero(); // bias
        cost = 0;

        for(int i = 0; i < N; i++)
        {
            double xi = x_data[i], yi = y_data[i]; // the ith data point 第i个数据点
            double error = yi - exp(ae * xi * xi + be * xi + ce);
            Eigen::Vector3d J;//Jacobian matrix 雅可比矩阵

            J[0] = -xi * xi * exp( ae * xi * xi + be * xi + ce );
            J[1] = -xi * exp(ae * xi * xi + be * xi + ce);
            J[2] = -exp(ae * xi * xi + be * xi + ce);

            H += inv_sigma * inv_sigma * J * J.transpose();
            b += -inv_sigma * inv_sigma * J * error;

            cost += error * error;
        }

        //求解线性方程Hx = b;  Solve the linear equation Hx = b;
        Eigen::Vector3d dx = H.ldlt().solve(b);

        if(isnan(dx[0]))
        {
            cout << " result is nan!" << endl;
            break;
        }

        if(iter > 0 && cost >= lastCost)
        {
            cout << "cost: " << cost << ">=last_cost: " << lastCost << ", break." << endl;
            break;
        }

        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        lastCost = cost;

        cout << iter <<"total cost: " << cost << ", \t\tupdate: " << dx.transpose() << "\t\testimated params: " << ae <<"," << be << "," << ce << endl;
    }

    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 << "estimated abc：" <<  ae << "," << be << "," << ce << endl;
    return 0;
}

1.2 CMakeLists.txt

cmake_minimum_required(VERSION 2.8)

project(gaussNewton)

set(CMAKE_CXX_FLAGS "-std=c++11")

find_package(OpenCV 3 REQUIRED)

include_directories("/usr/include/eigen3")
include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(gaussNewton src/gaussNewton.cpp)

target_link_libraries(gaussNewton ${OpenCV_LIBRARIES})

1.3 输出

/home/bupo/my_study/slam14/slam14_my/cap6/gaussNewton/cmake-build-debug/gaussNewton
0total cost: 3.19575e+06, 		update: 0.0455771  0.078164 -0.985329		estimated params: 2.04558,-0.921836,4.01467
1total cost: 376785, 		update:  0.065762  0.224972 -0.962521		estimated params: 2.11134,-0.696864,3.05215
2total cost: 35673.6, 		update: -0.0670241   0.617616  -0.907497		estimated params: 2.04432,-0.0792484,2.14465
3total cost: 2195.01, 		update: -0.522767   1.19192 -0.756452		estimated params: 1.52155,1.11267,1.3882
4total cost: 174.853, 		update: -0.537502  0.909933 -0.386395		estimated params: 0.984045,2.0226,1.00181
5total cost: 102.78, 		update: -0.0919666   0.147331 -0.0573675		estimated params: 0.892079,2.16994,0.944438
6total cost: 101.937, 		update: -0.00117081  0.00196749 -0.00081055		estimated params: 0.890908,2.1719,0.943628
7total cost: 101.937, 		update:   3.4312e-06 -4.28555e-06  1.08348e-06		estimated params: 0.890912,2.1719,0.943629
8total cost: 101.937, 		update: -2.01204e-08  2.68928e-08 -7.86602e-09		estimated params: 0.890912,2.1719,0.943629
cost: 101.937>=last_cost: 101.937, break.
solve time cost = 0.00364509seconds.
estimated abc：0.890912,2.1719,0.943629

进程已结束,退出代码0

2 使用ceres进行曲线拟合

2.1 需要安装ceres

• 我们选用的版本是2.0.0，安装包可以在ceres官网上下，也可以在前面提供的网盘链接自取https://pan.baidu.com/s/1UdU9KsS5yKVG_aBrs6enBg
  提取码：pd3u
• 下面是安装过程 ：

1. 首先安装依赖

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

基本肯定会出现问题：E: Unable to locate package libcxsparse3.1.2
解决方法就是：

sudo gedit /etc/apt/sources.list

将下面代码添加到第一行

deb http://cz.archive.ubuntu.com/ubuntu trusty main universe 

然后更新后重新安装依赖

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

2. 解压并创建build，安装前先看第4步，确保自己没有已经安装ceres

cd ceres-solver-2.0.0
mkdir build
cd build
cmake ..
make -j4

如果重现报错，可能是运算不够，重新编译一次试试看

3. 安装

sudo make install

安装主要参考链接ubuntu20.04安装eigen3.4.0(两种方式)和ceres-solver2.0.0

4. 删除卸载原有ceres

如果你需要验证自己是否已经安装了ceres，并且要删除卸载，请参考Ceres库安装踩坑（SLAM十四讲）

2.2 main.cpp

#include 
#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,     // 模型参数，有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;
}

下面附上一个超多注释的，代码和上面一摸一样，看一个就行了

#include 
#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,     // 模型参数，有3维 当没有必要分类的时候 就用一个数组来存储未知的系数,方便管理，而不是设3个变量，之后在()重载函数的形式参数个数变为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<
    }

    // 构建最小二乘问题
    ceres::Problem problem;
    for ( int i=0; i<N; i++ )
    {
            /* 第一个参数 CostFunction* : 描述最小二乘的基本形式即代价函数 例如书上的137页fi(.)的形式
50          * 第二个参数 LossFunction* : 描述核函数的形式 例如书上的ρi（.）
51          * 第三个参数 double* :       待估计参数（用数组存储）
52          * 这里仅仅重载了三个参数的函数，如果上面的double abc[3]改为三个double a=0 ,b=0,c = 0;
53          * 此时AddResidualBlock函数的参数除了前面的CostFunction LossFunction 外后面就必须加上三个参数 分别输入&a,&b,&c
54          * 那么此时下面的 ceres::AutoDiffCostFunction<>模板参数就变为了 后面三个1代表有几类未知参数
55          * 我们修改为了a b c三个变量，所以这里代表了3类，之后需要在自己写的CURVE_FITTING_COST类中的operator()函数中，
56          * 把形式参数变为了 const T* const a, const T* const b, const T* const c ,T* residual
57          * 上面修改的方法与本例程实际上一样，只不过修改的这种方式显得乱，实际上我们在用的时候，一般都是残差种类有几个，那么后面的分类 就分几类
58          * 比如后面讲的重投影误差，此事就分两类 一类是相机9维变量，一类是点的3维变量，然而残差项变为了2维
59          *
60          * （1）: 修改后的写法（当然自己定义的代价函数要对应修改重载函数的形式参数，对应修改内部的残差的计算）：
61          *      ceres::CostFunction* cost_function
62          *              = new ceres::AutoDiffCostFunction(
63          *                  new CURVE_FITTING_COST ( x_data[i], y_data[i] ) );
64          *      problem.AddResidualBlock(cost_function,nullptr,&a,&b,&c);
65          * 修改后的代价函数的计算模型:
66          *   struct CURVE_FITTING_COST
67          *   {
68          *       CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
69          *       // 残差的计算
70          *       template 
71          *       bool operator() (
72          *          const T* const a,
73          *          const T* const b,
74          *          const T* const c,
75          *          T* residual   ) const     // 残差
76          *       {
77          *           residual[0] = T ( _y ) - ceres::exp ( a[0]*T ( _x ) *T ( _x ) + b[0]*T ( _x ) + c[0] ); // y-exp(ax^2+bx+c)
78          *           return true;
79          *       }
80          *       const double _x, _y;    // x,y数据
81          *   };//代价类结束
82          *
83          *
84          * （2）: 本例程下面的语句通常拆开来写(看起来方便些):
85          * ceres::CostFunction* cost_function
86          *              = new ceres::AutoDiffCostFunction(
87          *                  new CURVE_FITTING_COST ( x_data[i], y_data[i] ) );
88          * problem.AddResidualBlock(cost_function,nullptr,abc)
89          * */
        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 (是一个非线性最小二乘的求解器)
    ceres::Solver::Options options;// 这里有很多配置项可以填  Options类嵌入在Solver类中 ，在Options类中可以设置关于求解器的参数
    options.linear_solver_type = ceres::DENSE_QR;  // 增量方程如何求解  这里的linear_solver_type 是一个Linear_solver_type的枚举类型的变量
    options.minimizer_progress_to_stdout = true;   // 输出到cout 为真时 内部错误输出到cout，我们可以看到错误的地方，默认情况下，会输出到日志文件中保存


    ceres::Solver::Summary summary;                // 优化信息
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//记录求解时间间隔

         /*下面函数需要3个参数：
110      * 1、 const Solver::Options& options <----> options
111      * 2、 Problem* problem               <----> &problem
112      * 3、 Solver::Summary* summary       <----> &summart (即使默认的参数也需要定义该变量 )
113      * 这个函数会输出一些迭代的信息。
114      * */
    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 = ";
    /*auto a:abc  或者下面的方式都可以*/
    for ( auto a:abc ) cout<<a<<" ";
    cout<<endl;

    return 0;
}

2.3 CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( ceres_curve_fitting )

set( CMAKE_BUILD_TYPE "Release" )
set(CMAKE_CXX_STANDARD 14)

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

# 寻找Ceres库并添加它的头文件
find_package( Ceres REQUIRED )
include_directories( ${CERES_INCLUDE_DIRS} )

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

add_executable( curve_fitting main.cpp )
# 与Ceres和OpenCV链接
target_link_libraries( curve_fitting ${CERES_LIBRARIES} ${OpenCV_LIBS} )

• 上面的# 添加cmake模块以使用ceres库 list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )并不是必须要的，没有实际文件也可以，但是如果你想要，请自取链接：https://pan.baidu.com/s/1oCbM6t2WHZ1BWvAwmhj-Yw
  提取码：mwo7

• 此外在这里遇到一个很奇怪的问题，当我使用命令行进行编译的时候，没有报错，但是我使用clion进行编译时，就会出现下面报错

• 会出现报错：
  FAILED: CMakeFiles/curve_fitting.dir/main.cpp.o /usr/bin/c++ -DCERES_EXPORT_INTERNAL_SYMBOLS -DGFLAGS_IS_A_DLL=0 -isystem /usr/include/eigen3 -isystem /usr/local/include/opencv -std=c++11 -O3 -O3 -DNDEBUG -MD -MT CMakeFiles/curve_fitting.dir/main.cpp.o -MF CMakeFiles/curve_fitting.dir/main.cpp.o.d -o CMakeFiles/curve_fitting.dir/main.cpp.o -c /home/bupo/shenlan/slam14/slambook/ch6/ceres_curve_fitting/main.cpp

• 经过查找文献，发现把
  set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
  修改为
  set(CMAKE_CXX_STANDARD 11)
  或者set(CMAKE_CXX_STANDARD 14)，
  应该是使用C++14标准，即set(CMAKE_CXX_STANDARD 14)，但这里两种我都可以

• 报错解决参考文献使用ceres编译报错 error: ‘integer_sequence’ is not a member of ‘std‘

2.4 输出

/home/bupo/my_study/slam14/slam14_my/cap6/ceresCurveFitting/cmake-build-debug/curve_fitting
generating data: 
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  1.824887e+04    0.00e+00    1.38e+03   0.00e+00   0.00e+00  1.00e+04        0    1.60e-05    5.29e-05
   1  2.748700e+39   -2.75e+39    1.38e+03   7.67e+01  -1.52e+35  5.00e+03        1    1.81e-05    8.89e-05
   2  2.429783e+39   -2.43e+39    1.38e+03   7.62e+01  -1.35e+35  1.25e+03        1    7.87e-06    1.03e-04
   3  1.213227e+39   -1.21e+39    1.38e+03   7.30e+01  -6.73e+34  1.56e+02        1    7.15e-06    1.15e-04
   4  1.852387e+37   -1.85e+37    1.38e+03   5.56e+01  -1.03e+33  9.77e+00        1    6.91e-06    1.26e-04
   5  6.714689e+31   -6.71e+31    1.38e+03   2.96e+01  -3.85e+27  3.05e-01        1    5.96e-06    1.36e-04
   6  9.500531e+12   -9.50e+12    1.38e+03   9.50e+00  -8.39e+08  4.77e-03        1    5.96e-06    1.46e-04
   7  1.776982e+04    4.79e+02    1.83e+03   2.58e-01   1.18e+00  1.43e-02        1    1.79e-05    1.68e-04
   8  1.599969e+04    1.77e+03    3.45e+03   5.53e-01   1.46e+00  4.29e-02        1    1.60e-05    1.87e-04
   9  1.060557e+04    5.39e+03    7.62e+03   7.33e-01   1.68e+00  1.29e-01        1    1.60e-05    2.06e-04
  10  3.669783e+03    6.94e+03    9.60e+03   5.25e-01   1.39e+00  3.86e-01        1    1.50e-05    2.24e-04
  11  5.397541e+02    3.13e+03    5.00e+03   2.66e-01   1.12e+00  1.16e+00        1    1.41e-05    2.41e-04
  12  1.484444e+02    3.91e+02    1.22e+03   8.46e-02   1.02e+00  3.48e+00        1    1.50e-05    2.60e-04
  13  1.216815e+02    2.68e+01    3.76e+02   4.17e-02   1.01e+00  1.04e+01        1    1.41e-05    2.77e-04
  14  9.290109e+01    2.88e+01    2.42e+02   9.10e-02   1.01e+00  3.13e+01        1    1.41e-05    2.95e-04
  15  6.674330e+01    2.62e+01    1.09e+02   1.33e-01   1.00e+00  9.39e+01        1    1.41e-05    3.13e-04
  16  5.936574e+01    7.38e+00    2.14e+01   1.08e-01   9.94e-01  2.82e+02        1    1.50e-05    3.31e-04
  17  5.653118e+01    2.83e+00    1.36e+01   1.57e-01   9.98e-01  8.45e+02        1    1.41e-05    3.49e-04
  18  5.310764e+01    3.42e+00    8.50e+00   2.81e-01   9.89e-01  2.53e+03        1    1.50e-05    3.67e-04
  19  5.125939e+01    1.85e+00    2.84e+00   2.98e-01   9.90e-01  7.60e+03        1    1.50e-05    3.85e-04
  20  5.097693e+01    2.82e-01    4.34e-01   1.48e-01   9.95e-01  2.28e+04        1    1.48e-05    4.02e-04
  21  5.096854e+01    8.39e-03    3.24e-02   2.87e-02   9.96e-01  6.84e+04        1    1.50e-05    4.20e-04
solve time cost = 0.000439784 seconds. 
Ceres Solver Report: Iterations: 22, Initial cost: 1.824887e+04, Final cost: 5.096854e+01, Termination: CONVERGENCE
estimated a,b,c = 0.891943 2.17039 0.944142 

进程已结束,退出代码0

3 使用g2o进行曲线拟合

3.1 g2o的编译和安装

• 在学习slam14讲的时候我用的是最新版，也就是下面的安装，但是之后学习任佬的多传感器融合时候，尝试了之前slam的版，看情况自取：github里，网盘链接: https://pan.baidu.com/s/1LcGPuEI3XJElWbIApCl_aQ 提取码: iw8n ，该版本的依赖为sudo apt-get install cmake libeigen3-dev libsuitesparse-dev qtdeclarative5-dev qt5-qmake libqglviewer-dev，但是我的电脑不幸

• 最终我问同学要了他可以的g2o版本，网盘自取：链接: https://pan.baidu.com/s/1ZqdjN6tEO4hFLTYlKHn0pg?pwd=jtfw 提取码: jtfw

• 这里提供g2o最新的github网址：g2o的github网址https://github.com/RainerKuemmerle/g2o.git，可以看到要求：

• 当然这里为了保版本一致，我也特意附上了我当时安装时的安装包，请自取：链接：https://pan.baidu.com/s/1L6j8b0Mwyi1J-alAY-sfig 提取码：0a4h

• 然后就是安装

0. 如果安装错版本，就卸载重装，g2o会安装在usr\local\bin,usr\local\include,usr\local\lib三个文件夹中，进入这三个文件夹，在终端输入sudo rm -rf *g2o*，实际验证，可行！sudo rm -rf /usr/local/lib/*g2o* /usr/local/include/*g2o* /usr/local/bin/*g2o*
卸载参考文章《SLAM十四讲》中g2o的安装

1. 首先安装依赖项

sudo apt-get install qt5-qmake qt5-default libqglviewer-dev-qt5 libsuitesparse-dev libcxsparse3 libcholmod3

如果有找不到的依赖项可以安装2.1改一下，我都找到了
2. 安装并下载

cd g2o
mkdir build
cd build
sudo ldconfig//⼀定要在编译前进⼊build，进⾏sudo ldconfig
cmake ..//这里提示我cmake版本太低，我的是3.10.2的，要求是3.14的，进入CMakeLists.txt文件，修改第一句话，将3.14修改为3.10，（我的猜测是但是这样会有缺点，后面一个报错可能和这个有关）
make -j4   // 注意，这里尽量使用更多的j，否则g2o安装很慢（-j4  -j6等等），但是我的运行多了就会崩，量力而行
sudo make install

.
3. 使用g2o需要list一个文件包cmake_modules，自取链接：https://pan.baidu.com/s/1KceXzAXpDQ8UoRAGyf69EQ 提取码：egx6
4. 解决问题
4.1. 首先遇到了C++版本的问题，还是和ceres一样，把
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
修改为set(CMAKE_CXX_STANDARD 14)，
只能是使用C++14标准，即set(CMAKE_CXX_STANDARD 14)

4.2 然后遇到了报错：error: no matching function for call to ‘g2o::BlockSolver >::BlockSolver(g2o::BlockSolver >::LinearSolverType*&)’ Block* solver_ptr = new Block( linearSolver ); // 矩阵块求解器
和error: no matching function for call to ‘g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(Block*&)’ rithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr ); ^

经过查阅别人的博客，大概知道是因为智能指针的原因，但是大部分是修改的第一版的好像，最终也是找到对应第六章中的：

// 构建图优化，先设定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 );      // 矩阵块求解器
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );

改为：

//第一种实现方法start
    // 构建图优化，先设定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 );      // 矩阵块求解器
    Block* solver_ptr = new Block( unique_ptr<Block::LinearSolverType>(linearSolver) );      // 矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( unique_ptr<Block>(solver_ptr) );
//第一种实现方法end

或者改为：

//第二种实现方法start
    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; //线性求解器类型
    // 梯度下降方法，从GN, LM, DogLeg 中选
    auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
//第二种实现方法end

然后重新编译就可，该错误参考文章：[Bug集合] error: no matching function for call to ‘g2o::OptimizationAlgorithmLevenberg::OptimizationA

4.3. 但是使用命令行运行可执行文件，报错：terminate called after throwing an instance of 'std::bad_alloc' what(): std::bad_alloc 已放弃 (核心已转储)
一直也没有找到原因，等以后再说吧，但是看c ++ terminate called after throwing an instance of ‘std::bad_alloc‘ what(): std::bad_alloc错误这篇文章有说这个报错什么意思；我觉得可能和我之前一个操作有关系，就是安装g2o的时候，我的cmake版本较低，我强行修改之后仍编译安装g2o，可能是这个原因，之后有时间在进行验证

但是，我是用clion的发布模式编译运行的时候，就可以正常运行，很奇怪，

3.2 main.cpp

• 这是修改后的文件

#include 
#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 void linearizeOplus() override{
        const CurveFittingVertex *v = static_cast (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        double y = exp(abc[0] * _x *_x + abc[1] * _x + abc[2]);
        _jacobianOplusXi[0] = -_x * _x * y;
        _jacobianOplusXi[1] = -_x * y;
        _jacobianOplusXi[2] = -y;
 }
 */
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

int main( int argc, char** argv )
{
    double ar=1.0, br=2.0, cr=1.0;         // 真实参数值
    double ae = 2.0, be = -1.0, ce = 5.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 ( ar*x*x + br*x + cr ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }


//第一种实现方法start
    // 构建图优化，先设定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 );      // 矩阵块求解器
    Block* solver_ptr = new Block( unique_ptr<Block::LinearSolverType>(linearSolver) );      // 矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( unique_ptr<Block>(solver_ptr) );
//第一种实现方法end

/*
//第二种实现方法start
    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1
    typedef g2o::LinearSolverDense LinearSolverType; //线性求解器类型
    // 梯度下降方法，从GN, LM, DogLeg 中选
    auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique(g2o::make_unique()));
//第二种实现方法end
*/


    //梯度下降方法，从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(ae,be,ce) );//迭代5次满足迭代阈值
    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;
}

这里也提供一个有详细注释的版本，除了注释和上面以及提供的完整包没有区别，：

#include 
#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//表示在利用Eigen库的数据结构时new的时候 需要对齐，所以加入EIGEN特有的宏定义即可实现
    //下面几个虚函数都是覆盖了基类的对应同名同参数的函数
    virtual void setToOriginImpl() // 重置  这个虚函数override 覆盖了Vertex类的对应函数 函数名字和参数都是一致的，是多态的本质
    {
        _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 {}
};

// 误差模型 模板参数：观测值维度，类型，连接顶点类型  //这里观测值维度是1维，如果是124页6.12式，则观测值维度是2
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW//表示在利用Eigen库的数据结构时new的时候 需要对齐，所以加入EIGEN特有的宏定义即可实现
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 计算曲线模型误差
    void computeError()
    {
        /*       _vertices是std::vector类型的变量，我们这里把基类指针_vertices【0】强制转换成const CurveFittingVertex* 自定义子类的常量指针
        这里的转换是上行转换（子类指针转换到基类），对于static_cast 和dynamic_cast两种的结果都是一样的，但是对于这种下行转换则dynamic_cast比static_cast多了类型检查功能
        更安全些，但是dynamic_cast只能用在类类型的指针 引用，static_cast则不限制,即可以用在类型也可以用在其他类型，所以这里应该更改为dynamic_cast
        const CurveFittingVertex* v = dynamic_cast (_vertices[0]);//但是这里我没有修改，因为我不懂这块不敢乱该，如果你觉得有道理你 就修改试试，改了也是正常运行的
        */
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        //获取此时待估计参数的当前更新值 为下面计算误差项做准备
        const Eigen::Vector3d abc = v->estimate();
        //这里的error是1x1的矩阵，因为误差项就是1个 _measurement是测量值yi
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }

/*
    //计算雅可比矩阵，没有用到,ke yi yong
   virtual void linearizeOplus() override{
        const CurveFittingVertex *v = static_cast (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        double y = exp(abc[0] * _x *_x + abc[1] * _x + abc[2]);
        _jacobianOplusXi[0] = -_x * _x * y;
        _jacobianOplusXi[1] = -_x * y;
        _jacobianOplusXi[2] = -y;
 }
 */
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

int main( int argc, char** argv )
{
    double ar=1.0, br=2.0, cr=1.0;         // 真实参数值
    double ae = 2.0, be = -1.0, ce = 5.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: "<
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
                exp ( ar*x*x + br*x + cr ) + rng.gaussian ( w_sigma )
        );
        // cout<
    }

/*
原版错误方式 : 这样会出错
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1  后面的那个参数与误差变量无关 仅仅表示路标点的维度 这里因为没有用到路标点 所以为什么值都可以
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense(); // 线性方程求解器
    Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );//LM法
*/

//第一种实现方法start
/*
第一种解决方式: 将普通指针强制转换成智能指针 需要注意的是 转化之后 原来的普通指针指向的内容会有变化
 普通指针可以强制转换成智能指针，方式是通过智能指针的一个构造函数来实现的， 比如下面的Block( std::unique_ptr( linearSolver ) );
 这里面就是将linearSolver普通指针作为参数用智能指针构造一个临时的对象，此时原来的普通指针就无效了，一定不要再次用那个指针了，否则会有意想不到的错误，如果还想保留原来的指针
 那么就可以利用第二种方式 定义的时候就直接用智能指针就好，但是就如第二种解决方案那样，也会遇到类型转换的问题。详细见第二种方式说明
 */
    // 构建图优化，先设定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 );      // 矩阵块求解器
    Block* solver_ptr = new Block( unique_ptr<Block::LinearSolverType>(linearSolver) );      // 矩阵块求解器
    //g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( unique_ptr(solver_ptr) );
    g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(  unique_ptr<Block>(solver_ptr)  );
    //g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg(  unique_ptr(solver_ptr)  );
//第一种实现方法end

/*
//第二种实现方法start
    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1
    typedef g2o::LinearSolverDense LinearSolverType; //线性求解器类型
    // 梯度下降方法，从GN, LM, DogLeg 中选
    auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique(g2o::make_unique()));
//第二种实现方法end
*/

/*

/*第三种解决方案: 定义变量时就用智能指针 需要注意的是 需要std::move移动
 *下面可以这样做 std::make_unique<>是在c++14中引进的 而std::make_shared<>是在c++11中引进的,都是为了解决用new为智能指针赋值的操作。这种更安全。
 *  对于（2）将linearSovler智能指针的资源利用移动构造函数转移到新建立的Block中，此时linearSolver这个智能指针默认不能够访问以及使用了。
 *  对于(3)来说，因为solver_ptr是一个指向Block类型的智能指针，但是g2o::OptimizationAlgorithmLevenberg 构造函数接受的是std::unique_ptr的参数，引起冲突，但是智能指针指向不同的类型时，
 *  不能够通过强制转换，所以此时应该用一个std::move将一个solver_ptr变为右值，然后调用std::unique_ptr的移动构造函数，而这个函数的本身并没有限制指针
 *  指向的类型，只要是std::unique_ptr类的对象，我们就可以调用智能指针的移动构造函数进行所属权的移动。
 *
 * */
/*
//第三种实现方法start
    std::unique_ptrlinearSolver( new g2o::LinearSolverDense() );// 线性方程求解器(1)
    std::unique_ptr solver_ptr ( new  Block( std::move(linearSolver) ) );// 矩阵块求解器 (2)
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::move(solver_ptr) );//(3) LM法
//第三种实现方法end
    */


    //梯度下降方法，从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(ae,be,ce) );//迭代5次满足迭代阈值 //增加顶点的初始值，如果是位姿 则初始值是用ICP PNP来提供初始化值
    v->setId(0);//增加顶点标号 多个顶点要依次增加编号
    optimizer.addVertex( v );//将新增的顶点加入到图模型中

    // 往图中增加边 N个
    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] );      // 观测数值  经过高斯噪声的
        //这里的信息矩阵可以参考:http://www.cnblogs.com/gaoxiang12/p/5244828.html 里面有说明
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆 这里为1表示加权为1
        optimizer.addEdge( edge );
    }

    // 执行优化
    cout<<"start optimization"<<endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    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;
}

3.3 CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( g2o_curve_fitting )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_STANDARD 14)

# 添加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
)

3.4 整个文件包

自取：链接：
链接：https://pan.baidu.com/s/1LzZuCEuS50L1OYLud9rIgg
提取码：yeg1

3.5 输出：

/home/bupo/my_study/slam14/slam14_my/cap6/g2oCurveFitting/cmake-build-debug/curve_fitting
start optimization
solve time cost = 0.000371429 seconds. 
estimated model: 0.890912   2.1719 0.943629
iteration= 0	 chi2= 376785.194545	 time= 2.7101e-05	 cumTime= 2.7101e-05	 edges= 100	 schur= 0
iteration= 1	 chi2= 35673.583497	 time= 1.978e-05	 cumTime= 4.6881e-05	 edges= 100	 schur= 0
iteration= 2	 chi2= 2195.014705	 time= 1.9331e-05	 cumTime= 6.6212e-05	 edges= 100	 schur= 0
iteration= 3	 chi2= 174.853281	 time= 1.9155e-05	 cumTime= 8.5367e-05	 edges= 100	 schur= 0
iteration= 4	 chi2= 102.779698	 time= 1.9092e-05	 cumTime= 0.000104459	 edges= 100	 schur= 0
iteration= 5	 chi2= 101.937194	 time= 1.9193e-05	 cumTime= 0.000123652	 edges= 100	 schur= 0
iteration= 6	 chi2= 101.937020	 time= 1.9142e-05	 cumTime= 0.000142794	 edges= 100	 schur= 0
iteration= 7	 chi2= 101.937020	 time= 1.9146e-05	 cumTime= 0.00016194	 edges= 100	 schur= 0
iteration= 8	 chi2= 101.937020	 time= 1.9095e-05	 cumTime= 0.000181035	 edges= 100	 schur= 0
iteration= 9	 chi2= 101.937020	 time= 1.9169e-05	 cumTime= 0.000200204	 edges= 100	 schur= 0

进程已结束,退出代码0

4 DogLeg

• 因为视觉slam十四讲这本书课后题提了一下，这里就当挖一个坑，之后有时间再来学习
• Dogleg属于Trust Region优化方法，即用置信域的方法在最速下降法和高斯牛顿法之间进行切换（将二者的搜索步长及方向转化为向量，两个向量进行叠加得到新的方向和置信域内的步长），相当于是一种加权求解。
• 参考资料：https://www.numerical.rl.ac.uk/people/nimg/course/lectures/raphael/lectures/lec7slides.pdf
• 暴力拆解《Numerical Optimization》之信任域方法（下）——Dogleg（狗腿方法）

5 总结思考

• 虽然高博在SLAM14讲的第6讲最后结语说了他做的测试结果：g2o使用高斯-牛顿算法迭代9次之后得到优化结果，与cceres和手写高斯牛顿法相差无几。且从运动速度来看，实验结论是手写快于g2o，快于ceres。通用性和高效性是相互矛盾的，本实验中ceres使用了自动求导，且求解器配置与高斯牛顿还不完全一致，所以看起来慢一些。
• 但是，实际上，我做的结果从上面可以看出：手写：0.00364509s、ceres：0.000439784 seconds. 、g2o：0.000371429 s ，事实上，虽然每一次的结果都会有所不同，但是相差无几，从这个结果上看，运行速度排名：手写
• 我分析了几点原因：

1. 可能我所配置参数的问题，不够准确，不是最佳参数
2. 对于g2o的运行结果，可以显然看出其进行了多余的迭代，我猜测应该也可以配置参数，但是没有找到配置哪个参数，手动改为迭代七次的运行时间为：0.00031253 seconds. ；显然这也没有影响其运行速度第一的排名。
3. 这个问题留着以后有需求的时候研究

6 最后附上一个大佬的，可能版本不对的代码注释版当作参考

视觉slam十四讲ch6曲线拟合 代码注释（笔记版）