读书笔记:非线性优化
在前面几章,我们介绍了经典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
里的函数就行。