读书笔记：非线性优化
在前面几章，我们介绍了经典SLAM 模型的运动方程和观测方程。现在我们已经知
道，方程中的位姿可以由变换矩阵来描述，然后用李代数进行优化。观测方程由相机成像模型给出，其中内参是随相机固定的，而外参则是相机的位姿。于是，我们已经弄清了经典SLAM 模型在视觉情况下的具体表达。
然而，由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的。尽管相机可以非常好地符合针孔模型，但遗憾的是，我们得到的数据通常是受各种未知噪声影响的。即使我们有着高精度的相机，运动方程和观测方程也只能近似的成立。所以，与其假设数据必须符合方程，不如来讨论，如何在有噪声的数据中进行准确的状态估计。
大多现代视觉SLAM 算法都不需要那么高成本的传感器，甚至也不需要那么昂贵的
处理器来计算这些数据，这全是算法的功劳。由于在SLAM 问题中，同一个点往往会被一个相机在不同的时间内多次观测，同一个相机在每个时刻观测到的点也不止一个。这些因素交织在一起，使我们拥有了更多的约束，最终能够较好地从噪声数据中恢复出我们需要的东西。本节就将介绍如何通过优化处理噪声数据，并且由这些表层逐渐深入到图优化本质，提供图优化的解决算法初步介绍并且提供训练实例。
批量状态估计问题
非线性最小二乘
改进版的G-N：
小结：
- 非线性优化是个很大的主题，研究者们为之奋斗多年
- 主要方法：最速下降、牛顿、G-N、L-M、DogLeg等
- 与线性规划不同，非线性需要针对具体问题具体分析
- 问题非凸时，对初值敏感，会陷入局部最优（目前没有非凸问题的通用最优值的寻找办法；问题凸时，二阶方法通常一两步就能收敛）
实践部分
需要安装Ceres 库和g2o库，在3rdparty文件夹中都有，都是cmake的工程，安装之前一样的安装方式安装即可。
gaussNewton
代码运行没有问题，结果如下：
/home/wh/shenlan/slambook2/ch6/cmake-build-debug/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.000179568 seconds.
estimated abc = 0.890912, 2.1719, 0.943629
Process finished with exit code 0
ceresCurveFitting：
运行结果如下：
/home/wh/shenlan/slambook2/ch6/cmake-build-debug/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 2.72e-05 9.42e-05
1 1.884440e+05 1.41e+06 4.86e+05 9.88e-01 8.82e-01 1.81e+04 1 5.20e-05 1.89e-04
2 1.784821e+04 1.71e+05 6.78e+04 9.89e-01 9.06e-01 3.87e+04 1 2.41e-05 2.20e-04
3 1.099631e+03 1.67e+04 8.58e+03 1.10e+00 9.41e-01 1.16e+05 1 2.19e-05 2.47e-04
4 8.784938e+01 1.01e+03 6.53e+02 1.51e+00 9.67e-01 3.48e+05 1 2.22e-05 2.74e-04
5 5.141230e+01 3.64e+01 2.72e+01 1.13e+00 9.90e-01 1.05e+06 1 2.22e-05 3.00e-04
6 5.096862e+01 4.44e-01 4.27e-01 1.89e-01 9.98e-01 3.14e+06 1 2.22e-05 3.26e-04
7 5.096851e+01 1.10e-04 9.53e-04 2.84e-03 9.99e-01 9.41e+06 1 2.00e-05 3.50e-04
solve time cost = 0.000375516 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
Process finished with exit code 0
g2oCurveFitting：
运行结果如下：
/home/wh/shenlan/slambook2/ch6/cmake-build-debug/g2oCurveFitting
start optimization
iteration= 0 chi2= 376785.128234 time= 1.4727e-05 cumTime= 1.4727e-05 edges= 100 schur= 0
iteration= 1 chi2= 35673.566018 time= 7.161e-06 cumTime= 2.1888e-05 edges= 100 schur= 0
iteration= 2 chi2= 2195.012304 time= 5.977e-06 cumTime= 2.7865e-05 edges= 100 schur= 0
iteration= 3 chi2= 174.853126 time= 5.882e-06 cumTime= 3.3747e-05 edges= 100 schur= 0
iteration= 4 chi2= 102.779695 time= 6.14e-06 cumTime= 3.9887e-05 edges= 100 schur= 0
iteration= 5 chi2= 101.937194 time= 2.9009e-05 cumTime= 6.8896e-05 edges= 100 schur= 0
iteration= 6 chi2= 101.937020 time= 6.149e-06 cumTime= 7.5045e-05 edges= 100 schur= 0
iteration= 7 chi2= 101.937020 time= 6.092e-06 cumTime= 8.1137e-05 edges= 100 schur= 0
iteration= 8 chi2= 101.937020 time= 6.099e-06 cumTime= 8.7236e-05 edges= 100 schur= 0
iteration= 9 chi2= 101.937020 time= 6.14e-06 cumTime= 9.3376e-05 edges= 100 schur= 0
solve time cost = 0.000410354 seconds.
estimated model: 0.890912 2.1719 0.943629
Process finished with exit code 0
课后习题
1. 证明线性方程Ax = b 当系数矩阵A 超定时，最小二乘解为
2. 调研最速下降法、牛顿法、GN 和LM 各有什么优缺点。除了我们举的Ceres 库和g2o 库，还有哪些常用的优化库？你可能会找到一些MATLAB 上的库。
最速下降法比较直观，但过于贪心，容易走出锯齿路线，反而增加迭代次数，导致局部收敛速度下降。
牛顿法在最速下降的基础上引入二阶导数，这样就处理了最速下降法一阶导数为0的情况，但是需要计算目标函数的海塞矩阵，二阶求导运算量不小，在问题规模较大时非常困难。
高斯牛顿法则对牛顿法进行了简化，避免了二阶求导，但实际中该近似只有半正定性，容易产生病态方程。
列文伯格—马夸尔特算法在高斯牛顿法引入阻尼项，一定程度上避免系数矩阵的非奇异和病态问题，但收敛速度较慢。
除了Ceres库和g2o库，还有NLopt库、liblbfgs、slam++库等等。
3. 为什么GN 的增量方程系数矩阵可能不正定？不正定有什么几何含义？为什么在这种情况下解就不稳定了？
多元函数的Hessian矩阵就类似一元函数的二阶导。多元函数Hessian矩阵半正定就相当于一元函数二阶导非负，半负定就相当于一元函数二阶导非正。如果这个类比成立的话，凸函数的Hessian恒半正定就非常容易理解了——这是一元凸函数二阶导必非负的多元拓展。
增量方程系数是一个二阶求导的海森矩阵。举证属于一种向量到向量的映射关系的描述，这矩阵的不正定意味着增量方程对向量迭代方向的不确定。当一阶导数趋于零也就是最优点时，计算机在进行小值运算是容易出现精度缺失的情况，所以容易锯齿化，然后二阶运算会将这种震动状态放大，解就非常不稳定。
高斯牛顿法的实质也是用泰勒展开得到的，二次型来代替原函数，利用二次型的极值点的情况，逼近原函数的极值点。其实也是求一个极小值点，说白了也是一个凸函数的极值点。这里的增量矩阵就是凸函数的海塞矩阵。因此要满足高斯牛顿法有极小值点，必须要求增量矩阵半正定性。引用高翔视觉SLAM十四讲中内容，当增量矩阵为半正定型时，可能会出现奇异矩阵和病态的情况，此时增量的稳定性较差，因此高斯牛顿法要求增量矩阵为正定性，但实际情况下是半正定。
4. DogLeg 是什么？它与GN 和LM 有何异同？请搜索相关的材料。
DogLeg介绍
Dogleg属于Trust Region优化方法，即用置信域的方法在最速下降法和高斯牛顿法之间进行切换（将二者的搜索步长及方向转化为向量，两个向量进行叠加得到新的方向和置信域内的步长），相当于是一种加权求解。
相关材料
5. 阅读Ceres 的教学材料以更好地掌握它的用法：教程
真的是好多啊！
6. 阅读g2o 自带的文档，你能看懂它吗？如果还不能完全看懂，请在第十、十一两讲之后回来再看。
没看懂，等以后再来重新看！
7.* 请更改曲线拟合实验中的曲线模型，并用Ceres 和g2o 进行优化实验。例如，你可以使用更多的参数和更复杂的模型。
其实吧，直接改y.data.push_back里的函数就行。
