《视觉SLAM十四讲 第二版》笔记及课后习题（第十讲）

读书笔记：后端2

上讲我们重点介绍了以BA 为主的图优化。BA 能精确地优化每个相机位姿与特征点位置。不过在更大的场景中，大量特征点的存在会严重降低计算效率，导致计算量越来越大以至于无法实时化。本讲介绍两种在更大场景下使用的后端优化方法：位姿图。

位姿图（Pose Graph）

Pose Graph即是省略了特征点的Bundle Adjustment。
Pose Graph 示意图。当我们不再优化Bundle Adjustment 中的路标点，仅把它们看成对姿态节点的约束时，就得到了一个计算规模减小很多的Pose Graph。


小结：
球的例子是一个比较有代表性的案例。它具有和实际中相似的里程计边（Odometry）和回环边（Loop Closure），这也正是实际SLAM 中，一个位姿图中可能有的东西。同时，“球”也具有一定的计算规模：它总共有2,500 个位姿节点和近10,000 条边，我们发现优化它费了不少时间（相对于实时性要求很强的前端来说）。另一方面，一般认为位姿图是结构最简单的图之一。在我们不假设机器人如何运动的前提下，很难再进一步讨论它的稀疏性了——因为机器人可能会直线往前运动，形成带状的位姿图，是稀疏的；也可能是“左手右手一个慢动作”，形成了大量的小型回环需要优化（Loopy motion），从而变成像“球”那样比较稠密的位姿图。无论如何，在没有进一步的信息之前，我们似乎无法再利用位姿图的求解结构了。

自从PTAM[81] 提出以来，人们就已经意识到，后端的优化没必要实时地响应前端的图像数据。人们倾向于把前端和后端分开，运行于两个独立线程之中，历史上称为跟踪（Tracking）和建图（Mapping）——虽然如此叫，建图部分主要是指后端的优化内容。通俗地说，前端需要实时响应视频的速度，例如每秒30Hz；而优化可以慢悠悠地运行，只要在优化完成时把结果返回给前端即可。所以我们通常不会对后端优化提出很高的速度要求。

实践部分

本章实验需要安装g2o库，我按照3rdparty安装之后没有g2o_viewer，显示g2o_viewer：未找到命令
，我怀疑是不是版本问题？依赖项问题？所以又只得去官方的github去重新安装g2o。安装过程如下：

sudo apt-get install libeigen3-dev
sudo apt-get install libsuitesparse-dev
sudo apt-get install qtdeclarative5-dev
sudo apt-get install qt5-qmake
sudo apt-get install libqglviewer-dev
git clone https://github.com/RainerKuemmerle/g2o.git
cd g2o
mkdir build
cd build
cmake ..
make -j4
sudo make install

安装完成之后再输入：g2o_viewer出现了报错：

g2o_viewer: error while loading shared libraries: libg2o_viewer.so: cannot open shared object file: No such file or directory

从错误来看，应该是没有找到正确的路径。上网搜了一下，具体解决方案如下：

sudo gedit /etc/ld.so.conf

添加如下代码：

/usr/local/lib

运行：

sudo ldconfig

再次运行g2o_viewer，可以看到启动成功！

pose_graph_g2o_lie_algebra：
首先在ch10目录下，用g2o_viewer sphere.g2o命令打开预先生成的仿真位姿图：

点击Optimize优化后变成一个球：

程序编译执行结果如下：

/home/wh/shenlan/slambook2/ch10/cmake-build-debug/pose_graph_g2o_lie /home/wh/shenlan/slambook2/ch10/sphere.g2o
read total 2500 vertices, 9799 edges.
optimizing ...
iteration= 0	 chi2= 674837160.579968	 time= 0.481116	 cumTime= 0.481116	 edges= 9799	 schur= 0	 lambda= 6658.554263	 levenbergIter= 1
iteration= 1	 chi2= 234706314.970484	 time= 0.539592	 cumTime= 1.02071	 edges= 9799	 schur= 0	 lambda= 2219.518088	 levenbergIter= 1
iteration= 2	 chi2= 142146174.348537	 time= 0.497943	 cumTime= 1.51865	 edges= 9799	 schur= 0	 lambda= 739.839363	 levenbergIter= 1
iteration= 3	 chi2= 83834595.145595	 time= 0.440281	 cumTime= 1.95893	 edges= 9799	 schur= 0	 lambda= 246.613121	 levenbergIter= 1
iteration= 4	 chi2= 41878079.903257	 time= 0.517399	 cumTime= 2.47633	 edges= 9799	 schur= 0	 lambda= 82.204374	 levenbergIter= 1
iteration= 5	 chi2= 16598628.119946	 time= 0.475482	 cumTime= 2.95181	 edges= 9799	 schur= 0	 lambda= 27.401458	 levenbergIter= 1
iteration= 6	 chi2= 6137666.739408	 time= 0.487153	 cumTime= 3.43897	 edges= 9799	 schur= 0	 lambda= 9.133819	 levenbergIter= 1
iteration= 7	 chi2= 2182986.250589	 time= 0.425716	 cumTime= 3.86468	 edges= 9799	 schur= 0	 lambda= 3.044606	 levenbergIter= 1
iteration= 8	 chi2= 732676.668220	 time= 0.473114	 cumTime= 4.3378	 edges= 9799	 schur= 0	 lambda= 1.014869	 levenbergIter= 1
iteration= 9	 chi2= 284457.115176	 time= 0.552213	 cumTime= 4.89001	 edges= 9799	 schur= 0	 lambda= 0.338290	 levenbergIter= 1
iteration= 10	 chi2= 170796.109734	 time= 0.397699	 cumTime= 5.28771	 edges= 9799	 schur= 0	 lambda= 0.181974	 levenbergIter= 1
iteration= 11	 chi2= 145466.315841	 time= 0.405493	 cumTime= 5.6932	 edges= 9799	 schur= 0	 lambda= 0.060658	 levenbergIter= 1
iteration= 12	 chi2= 142373.179501	 time= 0.41694	 cumTime= 6.11014	 edges= 9799	 schur= 0	 lambda= 0.020219	 levenbergIter= 1
iteration= 13	 chi2= 137485.756901	 time= 0.393385	 cumTime= 6.50353	 edges= 9799	 schur= 0	 lambda= 0.006740	 levenbergIter= 1
iteration= 14	 chi2= 131202.175667	 time= 0.387076	 cumTime= 6.8906	 edges= 9799	 schur= 0	 lambda= 0.002247	 levenbergIter= 1
iteration= 15	 chi2= 128006.202531	 time= 0.373559	 cumTime= 7.26416	 edges= 9799	 schur= 0	 lambda= 0.000749	 levenbergIter= 1
iteration= 16	 chi2= 127587.860945	 time= 0.367812	 cumTime= 7.63198	 edges= 9799	 schur= 0	 lambda= 0.000250	 levenbergIter= 1
iteration= 17	 chi2= 127578.599359	 time= 0.394361	 cumTime= 8.02634	 edges= 9799	 schur= 0	 lambda= 0.000083	 levenbergIter= 1
iteration= 18	 chi2= 127578.573853	 time= 0.376254	 cumTime= 8.40259	 edges= 9799	 schur= 0	 lambda= 0.000028	 levenbergIter= 1
iteration= 19	 chi2= 127578.573840	 time= 0.368586	 cumTime= 8.77118	 edges= 9799	 schur= 0	 lambda= 0.000018	 levenbergIter= 1
iteration= 20	 chi2= 127578.573840	 time= 0.368996	 cumTime= 9.14017	 edges= 9799	 schur= 0	 lambda= 0.000012	 levenbergIter= 1
iteration= 21	 chi2= 127578.573840	 time= 0.727279	 cumTime= 9.86745	 edges= 9799	 schur= 0	 lambda= 0.000016	 levenbergIter= 2
iteration= 22	 chi2= 127578.573840	 time= 0.367009	 cumTime= 10.2345	 edges= 9799	 schur= 0	 lambda= 0.000011	 levenbergIter= 1
iteration= 23	 chi2= 127578.573840	 time= 0.367201	 cumTime= 10.6017	 edges= 9799	 schur= 0	 lambda= 0.000007	 levenbergIter= 1
iteration= 24	 chi2= 127578.573840	 time= 3.56046	 cumTime= 14.1621	 edges= 9799	 schur= 0	 lambda= 263185475683.280334	 levenbergIter= 10
saving optimization results ...

Process finished with exit code 0

在ch10/cmake-build-debug目录下输入：g2o_viewer result_lie.g2o ，可以看到我们自己编写的程序所得到的优化结果基本是与软件得到的是一致的：

课后习题：

1. 如果将位姿图中的误差定义为： $\Delta\xi_{i,j}=\xi_i\circ\xi_j^{-1}$，推导按照此定义下的左乘扰动雅可比矩阵。

参考

2. 参照g2o 的程序，在Ceres 中实现对“球”位姿图的优化。

见第一题中的参考

3. 对“球”中的信息按照时间排序，分别喂给g2o 和gtsam 优化，比较它们的性能差异。

4. *阅读isam 相关论文，理解它是如何实现增量式优化的。