【slam十四讲第二版】【课后习题】【第九讲~后端1】

最新推荐文章于 2023-10-18 17:38:41 发布

踏破万里无云

最新推荐文章于 2023-10-18 17:38:41 发布

阅读量960

点赞数 1

分类专栏：视觉SLAM14讲文章标签： ceres slam 后端 g2o

本文链接：https://blog.csdn.net/qq_45954434/article/details/126010257

版权

视觉SLAM14讲专栏收录该内容

33 篇文章 83 订阅

订阅专栏

【slam十四讲第二版】【课后习题】【第九讲~后端1】

0 前言
1 课后习题

0 前言

参考：

https://blog.csdn.net/weixin_53660567/article/details/121458640

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 其他文件

参考【slam十四讲第二版】【课本例题代码向】【第九讲~后端Ⅰ】【安装Meshlab】【BAL数据集格式】【ceres求解BA】【g2o求解BA】的3 使用ceres实现BA

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通过优化的方式自动指定消元顺序，因此采用自动指定顺序优化时间更短。

参考：视觉SLAM十四讲（第二版）第9讲习题解答

1.4 证明S矩阵为半正定矩阵

参考：视觉SLAM十四讲CH9代码解析及课后习题详解

1.5 阅读参考文献[36]，看看g2o对核函数是如何处理的。与ceres中的Loss function有何联系

参考：视觉SLAM十四讲（第二版）第9讲习题解答

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

踏破万里无云

关注

1
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
【slam十四讲第二版】【课后习题】【第九讲~后端1】

https首先对数据进行修改，从上述的修改可以发现，我们使用的数据集中并没有新增的fy,p1,p2三个参数，因此我们需要改写数据集，在原先相机模型对应的参数中加入上述三个参数，因为不知道具体的数值fy的数值用fx的代替，p1,p2均取0。以下是笔者写的数据集修改程序的构造函数部分，基本的思路就是边读边写，不同的地方就是在写入相机参数时，每到第9个参数时，需要在其后面额外写入fy,p1,p2。处理后的数据集命名为problem-16-22106-pre-all.txt代码自取链接。...
复制链接

扫一扫

专栏目录