视觉SLAM十四讲——ch6实践（非线性优化）

一、实践操作前的准备工作

1. 安装Ceres库
先安装依赖环境，终端输入：

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

在slambook2/3rdparty/ceres-solver文件夹下打开终端并输入：

mkdir build
cd build/
cmake ..
make -j4
sudo make install

说明： Ceres库来自谷歌。是一个广泛使用的最小二乘问题求解库。

在Ceres库中，最为用户，一定要按照步骤定义待解的优化问题，然后交给求解器计算
需要的操作：

1. 定义每个参数块；
2. 定义残差块的计算方式；
3. 残差块往往也需要定义雅可比的计算方式；
4. 把所有的参数块和残差块加入Ceres定义的Problem对象中，调用Solve函数求解即可。

2. 安装g2o
先安装依赖环境，终端输入：

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

在slambook2/3rdparty/g2o文件夹下打开终端并输入：

mkdir build
cd build/
cmake ..
make -j4
sudo make install

说明： 它是一个基于图优化的库（图优化是一种将非线性优化与图论结合起来的理论）。为了使用g2o，首先要将曲线拟合问题抽象成图优化——节点为优化变量、边为误差。

作为g2o库的用户，要做的是主要为：

1. 定义顶点和边的类型；
2. 构建图；
3. 选择优化算法
4. 调用g2o进行优化，返回结果

3. 在终端中进入ch5文件夹下，顺序执行以下命令进行编译。

mkdir build
cd build
cmake ..
//注意，j8还是其他主要看自己的电脑情况
make -j8

在执行make -j8的时候会抛出一些警告，这些警告不用关心。不会影响最终结果。

4. 在build文件中执行。

二、曲线拟合问题

1. 手写高斯牛顿

在build中执行语句： ./gaussNewton
获得结果：

total cost: 3.19575e+06,                update: 0.0455771  0.078164 -0.985329           estimated params: 2.04558,-0.921836,4.01467
total cost: 376785,             update:  0.065762  0.224972 -0.962521           estimated params: 2.11134,-0.696864,3.05215
total cost: 35673.6,            update: -0.0670241   0.617616  -0.907497                estimated params: 2.04432,-0.0792484,2.14465
total cost: 2195.01,            update: -0.522767   1.19192 -0.756452           estimated params: 1.52155,1.11267,1.3882
total cost: 174.853,            update: -0.537502  0.909933 -0.386395           estimated params: 0.984045,2.0226,1.00181
total cost: 102.78,             update: -0.0919666   0.147331 -0.0573675                estimated params: 0.892079,2.16994,0.944438
total cost: 101.937,            update: -0.00117081  0.00196749 -0.00081055             estimated params: 0.890908,2.1719,0.943628
total cost: 101.937,            update:   3.4312e-06 -4.28555e-06  1.08348e-06          estimated params: 0.890912,2.1719,0.943629
total 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.003122 seconds.
estimated abc = 0.890912, 2.1719, 0.943629

2. 使用Ceres进行曲线拟合

在build中执行语句： ./ceresCurveFitting
获得结果：

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  1.597873e+06    0.00e+00    3.52e+06   0.00e+00   0.00e+00  1.00e+04        0    1.07e-03    1.80e-02
   1  1.884440e+05    1.41e+06    4.86e+05   9.88e-01   8.82e-01  1.81e+04        1    2.82e-03    2.36e-02
   2  1.784821e+04    1.71e+05    6.78e+04   9.89e-01   9.06e-01  3.87e+04        1    5.29e-05    2.38e-02
   3  1.099631e+03    1.67e+04    8.58e+03   1.10e+00   9.41e-01  1.16e+05        1    8.11e-05    2.39e-02
   4  8.784938e+01    1.01e+03    6.53e+02   1.51e+00   9.67e-01  3.48e+05        1    5.79e-05    2.40e-02
   5  5.141230e+01    3.64e+01    2.72e+01   1.13e+00   9.90e-01  1.05e+06        1    1.16e-04    2.42e-02
   6  5.096862e+01    4.44e-01    4.27e-01   1.89e-01   9.98e-01  3.14e+06        1    7.10e-05    2.44e-02
   7  5.096851e+01    1.10e-04    9.53e-04   2.84e-03   9.99e-01  9.41e+06        1    5.58e-05    2.45e-02
solve time cost = 0.0253849 seconds.
Ceres Solver Report: Iterations: 8, Initial cost: 1.597873e+06, Final cost: 5.096851e+01, Termination: CONVERGENCE
estimated a,b,c = 0.890908 2.1719 0.943628

3. 使用g2o进行曲线拟合

在build中执行语句： ./g2oCurveFitting
获得结果：

start optimization
iteration= 0     chi2= 376785.128234     time= 0.0014862         cumTime= 0.0014862      edges= 100      schur= 0
iteration= 1     chi2= 35673.566018      time= 1.52e-05  cumTime= 0.0015014      edges= 100      schur= 0
iteration= 2     chi2= 2195.012304       time= 1.38e-05  cumTime= 0.0015152      edges= 100      schur= 0
iteration= 3     chi2= 174.853126        time= 1.31e-05  cumTime= 0.0015283      edges= 100      schur= 0
iteration= 4     chi2= 102.779695        time= 1.7e-05   cumTime= 0.0015453      edges= 100      schur= 0
iteration= 5     chi2= 101.937194        time= 1.31e-05  cumTime= 0.0015584      edges= 100      schur= 0
iteration= 6     chi2= 101.937020        time= 2.32e-05  cumTime= 0.0015816      edges= 100      schur= 0
iteration= 7     chi2= 101.937020        time= 1.29e-05  cumTime= 0.0015945      edges= 100      schur= 0
iteration= 8     chi2= 101.937020        time= 1.49e-05  cumTime= 0.0016094      edges= 100      schur= 0
iteration= 9     chi2= 101.937020        time= 1.93e-05  cumTime= 0.0016287      edges= 100      schur= 0
solve time cost = 0.0047804 seconds.
estimated model: 0.890912   2.1719 0.943629

三、遇到的问题

我在对此章节进行操作的时候没有出现问题，如果有问题可以评论。
但，这个过程中值得注意的是，书中所带的源码并没有可视化的过程，所以如果想出来可视化图像，需要自己进行更改。