【slam十四讲第二版】【课后习题】【第九讲~后端1】
- 0 前言
- 1 课后习题
- 1.1 证明(p239 9.25)成立。提示:可能会用到SMW公式,参考文献[6,76]
- 1.2 对比使用g2o和ceres优化后目标函数的数值。指出为什么两者在Meshlab中效果一样但数值不同。
- 1.3 给Ceres当中的部分点云进行Schur消元,看看结果会有什么区别。
- 1.4 证明S矩阵为半正定矩阵
- 1.5 阅读参考文献[36],看看g2o对核函数是如何处理的。与ceres中的Loss function有何联系
- 1.6 在两个示例中,我们优化了相机位姿,以f、k1、k2为参数的相机内参及路标点。请考虑使用第5讲介绍的完整的相机模型进行优化,即、至少考虑fx、fy、p1、p2、k1、k2这些量。修改现在的ceres和g2o程序以完成实验。
0 前言
参考:
1 课后习题
1.1 证明(p239 9.25)成立。提示:可能会用到SMW公式,参考文献[6,76]
1.2 对比使用g2o和ceres优化后目标函数的数值。指出为什么两者在Meshlab中效果一样但数值不同。
- g2o优化后的cost是:
12336.97
- ceres优化后的cost是:
24815.047826
通过对比两组优化后的值,g2o优化后的数值约为Ceres的两倍左右,原因应该是两个优化库计算目标函数优化后数值的方法不一致(即计算用的范数不同);而不严格为2倍的原因与设置的迭代次数或收敛判断条件不同,出于计算成本的考虑并未做过多的迭代。但是两种优化库优化的目标函数是一致的,范数不同只会影响目标函数的数值,而实际待优化的变量的优化结果是一致的,因此最后得到在点云图像一致。
1.3 给Ceres当中的部分点云进行Schur消元,看看结果会有什么区别。
- 整个工程自取链接: https://pan.baidu.com/s/1cuaIyO5mCLTRjLDp7fkNMg 提取码: mj34
1.3.1 ceres_bufen_schur.cpp
1.3.2 CMakeLists.txt
1.3.3 其他文件
1.3.3.1 SnavelyReprojectionError.h
1.3.3.2 common.h
1.3.3.3 common.cpp
1.3.3.4 random.h
1.3.3.5 rotation.h
1.3.4 输出
/home/bupo/my_study/slam14/slam14_my/cap9/ceres_bufen_schur/cmake-build-debug/bundle_adjustment_ceres ./src/problem-16-22106-pre.txt
Header: 16 22106 83718bal problem file loaded...
bal problem have 16 cameras and 22106 points.
Forming 83718 observations.
Solving ceres BA ...
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.842900e+07 0.00e+00 2.04e+06 0.00e+00 0.00e+00 1.00e+04 0 5.52e-02 1.20e-01
1 1.449093e+06 1.70e+07 1.75e+06 2.16e+03 1.84e+00 3.00e+04 1 2.50e-01 3.70e-01
2 5.848543e+04 1.39e+06 1.30e+06 1.55e+03 1.87e+00 9.00e+04 1 1.95e-01 5.65e-01
3 1.581483e+04 4.27e+04 4.98e+05 4.98e+02 1.29e+00 2.70e+05 1 1.97e-01 7.61e-01
4 1.251823e+04 3.30e+03 4.64e+04 9.96e+01 1.11e+00 8.10e+05 1 1.95e-01 9.57e-01
5 1.240936e+04 1.09e+02 9.78e+03 1.33e+01 1.42e+00 2.43e+06 1 1.97e-01 1.15e+00
6 1.237699e+04 3.24e+01 3.91e+03 5.04e+00 1.70e+00 7.29e+06 1 1.94e-01 1.35e+00
7 1.236187e+04 1.51e+01 1.96e+03 3.40e+00 1.75e+00 2.19e+07 1 1.94e-01 1.54e+00
8 1.235405e+04 7.82e+00 1.03e+03 2.40e+00 1.76e+00 6.56e+07 1 1.95e-01 1.74e+00
9 1.234934e+04 4.71e+00 5.04e+02 1.67e+00 1.87e+00 1.97e+08 1 2.01e-01 1.94e+00
10 1.234610e+04 3.24e+00 4.31e+02 1.15e+00 1.88e+00 5.90e+08 1 1.97e-01 2.13e+00
11 1.234386e+04 2.24e+00 3.27e+02 8.44e-01 1.90e+00 1.77e+09 1 2.00e-01 2.33e+00
12 1.234232e+04 1.54e+00 3.44e+02 6.69e-01 1.82e+00 5.31e+09 1 1.96e-01 2.53e+00
13 1.234126e+04 1.07e+00 2.21e+02 5.45e-01 1.91e+00 1.59e+10 1 1.99e-01 2.73e+00
14 1.234047e+04 7.90e-01 1.12e+02 4.84e-01 1.87e+00 4.78e+10 1 1.98e-01 2.93e+00
15 1.233986e+04 6.07e-01 1.02e+02 4.22e-01 1.95e+00 1.43e+11 1 1.97e-01 3.13e+00
16 1.233934e+04 5.22e-01 1.03e+02 3.82e-01 1.97e+00 4.30e+11 1 1.98e-01 3.32e+00
17 1.233891e+04 4.25e-01 1.07e+02 3.46e-01 1.93e+00 1.29e+12 1 1.99e-01 3.52e+00
18 1.233855e+04 3.59e-01 1.04e+02 3.15e-01 1.96e+00 3.87e+12 1 1.95e-01 3.72e+00
19 1.233825e+04 3.06e-01 9.27e+01 2.88e-01 1.98e+00 1.16e+13 1 1.98e-01 3.92e+00
20 1.233799e+04 2.61e-01 1.17e+02 2.16e-01 1.97e+00 3.49e+13 1 1.98e-01 4.11e+00
21 1.233777e+04 2.18e-01 1.22e+02 1.15e-01 1.97e+00 1.05e+14 1 1.98e-01 4.31e+00
22 1.233760e+04 1.73e-01 1.10e+02 9.16e-02 1.89e+00 3.14e+14 1 1.96e-01 4.51e+00
23 1.233746e+04 1.37e-01 1.14e+02 1.14e-01 1.98e+00 9.41e+14 1 1.98e-01 4.71e+00
24 1.233735e+04 1.13e-01 1.17e+02 2.41e-01 1.96e+00 2.82e+15 1 1.94e-01 4.90e+00
WARNING: Logging before InitGoogleLogging() is written to STDERR
W0727 14:10:01.102447 20693 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
25 1.233735e+04 0.00e+00 1.17e+02 0.00e+00 0.00e+00 1.41e+15 1 1.28e-01 5.03e+00
26 1.233725e+04 9.50e-02 1.20e+02 5.09e-01 1.99e+00 4.24e+15 1 1.94e-01 5.22e+00
W0727 14:10:01.426414 20693 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
27 1.233725e+04 0.00e+00 1.20e+02 0.00e+00 0.00e+00 2.12e+15 1 1.30e-01 5.35e+00
W0727 14:10:01.550909 20693 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
28 1.233725e+04 0.00e+00 1.20e+02 0.00e+00 0.00e+00 5.30e+14 1 1.24e-01 5.48e+00
29 1.233718e+04 6.92e-02 5.75e+01 7.39e-02 1.70e+00 1.59e+15 1 1.97e-01 5.67e+00
W0727 14:10:01.873600 20693 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
30 1.233718e+04 0.00e+00 5.75e+01 0.00e+00 0.00e+00 7.94e+14 1 1.25e-01 5.80e+00
31 1.233714e+04 3.65e-02 5.88e+01 1.11e-01 1.93e+00 2.38e+15 1 1.96e-01 5.99e+00
32 1.233714e+04 0.00e+00 5.88e+01 0.00e+00 0.00e+00 1.19e+15 1 1.25e-01 6.12e+00
W0727 14:10:02.194772 20693 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
33 1.233714e+04 0.00e+00 5.88e+01 0.00e+00 0.00e+00 2.98e+14 1 1.26e-01 6.25e+00
W0727 14:10:02.320696 20693 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
34 1.233711e+04 3.32e-02 6.01e+01 4.17e-02 2.00e+00 8.94e+14 1 1.96e-01 6.44e+00
35 1.233708e+04 3.14e-02 6.07e+01 2.72e-01 2.00e+00 2.68e+15 1 1.99e-01 6.64e+00
W0727 14:10:02.843570 20693 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
36 1.233708e+04 0.00e+00 6.07e+01 0.00e+00 0.00e+00 1.34e+15 1 1.27e-01 6.77e+00
W0727 14:10:02.968645 20693 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
37 1.233708e+04 0.00e+00 6.07e+01 0.00e+00 0.00e+00 3.35e+14 1 1.25e-01 6.89e+00
38 1.233705e+04 2.50e-02 2.04e+01 7.55e-02 1.68e+00 1.01e+15 1 1.97e-01 7.09e+00
39 1.233704e+04 1.58e-02 2.05e+01 1.69e-01 1.95e+00 3.02e+15 1 1.99e-01 7.29e+00
40 1.233702e+04 1.51e-02 2.08e+01 8.05e-01 2.00e+00 9.05e+15 1 2.00e-01 7.49e+00
41 1.233702e+04 0.00e+00 2.08e+01 0.00e+00 0.00e+00 4.52e+15 1 1.30e-01 7.62e+00
W0727 14:10:03.694670 20693 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
42 1.233702e+04 0.00e+00 2.08e+01 0.00e+00 0.00e+00 1.13e+15 1 1.26e-01 7.75e+00
W0727 14:10:03.820885 20693 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
43 1.233701e+04 1.48e-02 2.06e+01 2.04e-01 1.99e+00 3.39e+15 1 1.99e-01 7.94e+00
44 1.233701e+04 0.00e+00 2.06e+01 0.00e+00 0.00e+00 1.70e+15 1 1.28e-01 8.07e+00
W0727 14:10:04.147991 20693 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
45 1.233700e+04 1.42e-02 2.05e+01 3.37e-01 1.99e+00 5.09e+15 1 1.94e-01 8.27e+00
46 1.233700e+04 0.00e+00 2.05e+01 0.00e+00 0.00e+00 2.54e+15 1 1.26e-01 8.39e+00
W0727 14:10:04.467901 20693 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
W0727 14:10:04.591131 20693 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
47 1.233700e+04 0.00e+00 2.05e+01 0.00e+00 0.00e+00 6.36e+14 1 1.23e-01 8.52e+00
48 1.233698e+04 1.39e-02 2.10e+01 1.17e-01 2.00e+00 1.91e+15 1 1.91e-01 8.71e+00
49 1.233697e+04 1.36e-02 2.00e+01 1.07e+00 2.00e+00 5.73e+15 1 1.95e-01 8.90e+00
50 1.233697e+04 0.00e+00 2.00e+01 0.00e+00 0.00e+00 2.86e+15 1 1.24e-01 9.03e+00
W0727 14:10:05.101768 20693 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
Solver Summary (v 2.0.0-eigen-(3.3.9)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-eigensparse-no_openmp)
Original Reduced
Parameter blocks 22122 22122
Parameters 66462 66462
Residual blocks 83718 83718
Residuals 167436 167436
Minimizer TRUST_REGION
Sparse linear algebra library SUITE_SPARSE
Trust region strategy LEVENBERG_MARQUARDT
Given Used
Linear solver SPARSE_SCHUR SPARSE_SCHUR
Threads 1 1
Linear solver ordering 11053,11069 11053,11069
Schur structure 2,3,9 2,3,9
Cost:
Initial 1.842900e+07
Final 1.233697e+04
Change 1.841667e+07
Minimizer iterations 51
Successful steps 37
Unsuccessful steps 14
Time (in seconds):
Preprocessor 0.064353
Residual only evaluation 0.467655 (36)
Jacobian & residual evaluation 1.561977 (37)
Linear solver 6.502632 (50)
Minimizer 8.964540
Postprocessor 0.004606
Total 9.033500
Termination: NO_CONVERGENCE (Maximum number of iterations reached. Number of iterations: 50.)
进程已结束,退出代码0
1.3.5 说明
- 从书中256页可以知道在Ceres中无法控制哪部分点云进行Schur消元,因为Ceres进行Schur消元是自动寻找的;只能通过操作ceres::parameterBlockOrdering::AddElementToGroup函数改变Schur消元的顺序,探讨Schur消元的顺序对结果的影响。对代码修改主要增加了如下部分
- 主要是利用ceres::ParameterBlockOrdering改变了Schur消元的顺序,将偶数索引的空间点变量设为优先消元变量,奇数索引的空间点、相机位姿与内参作为次优先级的消元变量。以下是优化后的点云图,与书上图9-13的彩色插图对比,可以看到Schur消元的顺序对点云优化效果基本没有影响。
- 通过下图优化结果的对比,可以发现目标函数优化值也是一致的,这也证明了优化结果的一致性;但是对比优化所用的时间,可以发现指定Schur消元的顺序后使得优化求解的时间增长不少,这是因为我指定的消元顺序是随意的,Ceres通过优化的方式自动指定消元顺序,因此采用自动指定顺序优化时间更短。
1.4 证明S矩阵为半正定矩阵
1.5 阅读参考文献[36],看看g2o对核函数是如何处理的。与ceres中的Loss function有何联系
1.6 在两个示例中,我们优化了相机位姿,以f、k1、k2为参数的相机内参及路标点。请考虑使用第5讲介绍的完整的相机模型进行优化,即、至少考虑fx、fy、p1、p2、k1、k2这些量。修改现在的ceres和g2o程序以完成实验。
- 首先对数据进行修改,从上述的修改可以发现,我们使用的数据集中并没有新增的fy ,p1 ,p2 三个参数,因此我们需要改写数据集,在原先相机模型对应的参数中加入上述三个参数,因为不知道具体的数值fy的数值用fx的代替, p1,p2 均取0。以下是笔者写的数据集修改程序的构造函数部分,基本的思路就是边读边写,不同的地方就是在写入相机参数时,每到第9个参数时,需要在其后面额外写入fy,p1 ,p2 。处理后的数据集命名为problem-16-22106-pre-all.txt
- 代码自取:链接: https://pan.baidu.com/s/1iPLOR0mveRpdySVTdAmrDw 提取码: v3ko
- 下面是ceres的修改
在这里我的已经修改后的自取:链接: https://pan.baidu.com/s/1awdH1WViBB1kH_FOH8OCYw 提取码: rcif - 下面是g2o的修改
在这里我的已经修改后的自取:链接: https://pan.baidu.com/s/1hk26B4gayqNrXCriM1ZQiA 提取码: 0mlt