视觉SLAM十四讲 读书编程笔记 Chapter11 后端2

位姿图

位姿图的意义

随着时间的流逝，空间点和相机位姿数据不断增多，BA的计算效率会不断下降。
实际上，经过若干次优化迭代之后，空间位置估计会收敛至一个值保持不动。因此，我们更倾向于在优化几次之后就把特征点固定住，只把它们看作位姿估计的约束，而不再实际地优化它们的空间位置。
我们可以构建一个只有轨迹的图优化，而位姿节点之间的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初始值。不同的是，一旦初始估计完成，就不再优化那些路标点的位置，从而构建了位姿图：

位姿图的优化

实践：位姿图优化

g2o原生位姿图

用g2o_viewer打开预先生成的仿真位姿图，位于slambook/ch11/sphere.go中。

该位姿图是由g2o自带的create sphere程序仿真生成的。它的真实轨迹为一个球，由从下往上的多个层组成。每层为一个正圆形，很多个大小不一的圆形层组成了一个完整的球体，共包含2500个位姿点。其中的边分两种：层与层之间的边（回环）与t-1到t时刻的边（里程计）
在用g2o_viewer打开sphere.go时，出现如下错误：

Error in `g2o_viewer’: realloc(): invalid pointer: 0x00007fb65d576820

解决办法：
参考这篇文章：https://blog.csdn.net/sinat_34156619/article/details/86489199
（1）修改： 在FindQGLViewer cmake module中添加下面的加粗代码
find_library(QGLVIEWER_LIBRARY_RELEASE
NAMES qglviewer-qt4 QGLViewer-qt4 qglviewer QGLViewer QGLViewer2
PATHS /usr/lib
/usr/local/lib
/usr/lib/x86_64-linux-gnu # Ubuntu 16.04 and derivatives
/opt/local/lib
/sw/lib

find_library(QGLVIEWER_LIBRARY_DEBUG
NAMES dqglviewer dQGLViewer dQGLViewer2 QGLViewerd2
PATHS /usr/lib
/usr/local/lib
/usr/lib/x86_64-linux-gnu # Ubuntu 16.04 and derivatives
/opt/local/lib
/sw/lib
（2）安装： sudo apt-get install libqglviewer-dev-qt4。
（3）清除： 清除已安装的g2o

sudo rm -r /usr/local/lib/libg2o* /usr/local/include/g2o /usr/local/lib/g2o /usr/local/bin/g2o*

（4）重新编译g2o并安装：

mkdir build
cd build
cmake ..
make
sudo make install

然后，重新运行g2o_viewer sphere.go即可
运行结果：
加入高斯噪声的正球：

优化之后的结果：

g2o优化位姿图

用g2o_viewer查看运行结果：

李代数上的位姿图优化