史上最简SLAM零基础解读(10.1) - g2o(图优化)→简介环境搭建(slam十四讲第二版为例)

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

针对与 g2o(图优化) 的讲解，主要分成三个部分，分别为: 理论讲解，环境搭建，代码分析。那么现在我们开始第一步，理论讲解吧！

论文链接：g2o: A General Framework for Graph Optimization

1、背景知识

        SLAM的后端一般分为两种处理方法，一种是以扩展卡尔曼滤波（EKF）为代表的滤波方法，一种是以图优化为代表的非线性优化方法。不过，目前SLAM研究的主流热点几乎都是基于图优化的。滤波方法很早就有了，前人的研究也很深。为什么现在图优化变成了主流了？
        滤波方法尤其是EKF方法，在SLAM发展很长的一段历史中一直占据主导地位，早期的大神们研究了各种各样的滤波器来改善滤波效果，那会入门SLAM，EKF是必须要掌握的。顺便总结下滤波方法的优缺点：优点→在当时计算资源受限、待估计量比较简单的情况下，EKF为代表的滤波方法比较有效，经常用在激光SLAM中。缺点→它的一个大缺点就是存储量和状态量是平方增长关系，因为存储的是协方差矩阵，因此不适合大型场景。而现在基于视觉的SLAM方案，路标点（特征点）数据很大，滤波方法根本吃不消，所以此时滤波的方法效率非常低。
        那图优化在视觉SLAM中效率很高吗？很久很久以前，其实就是不到十年前吧(感觉好像很久)，大家还都是用滤波方法，因为在图优化里，Bundle Adjustment（后面简称BA）起到了核心作用。但是那会SLAM的研究者们发现包含大量特征点和相机位姿的BA计算量其实很大，根本没办法实时。
        后来SLAM研究者们发现了其实在视觉SLAM中，虽然包含大量特征点和相机位姿，但其实BA是稀疏的，稀疏的就好办了，就可以加速了啊！比较代表性的就是2009年，几个大神发表了自己的研究成果《SBA：A software package for generic sparse bundle adjustment》，而且计算机硬件发展也很快，因此基于图优化的视觉SLAM也可以实时了！
 

2、核心要点

图优化里的图就是数据结构里的图，一个图由若干个$顶点(Vertex)$，以及连接这些$顶点的边(Edge)$组成，举例，先看下图：

图一

比如一个机器人在房屋里移动，它在某个时刻 t 的位姿（pose→上图三角形）就是一个顶点，这个也是待优化的变量。而位姿之间的关系就构成了一个边(上图蓝色实线)，比如时刻 t 和时刻 t+1 之间的相对位姿变换矩阵就是边，边通常表示误差项。在SLAM里，图优化一般分解为两个任务：

$(1)$ 构建图。机器人位姿作为顶点，位姿间关系作为边。
$(2)$ 优化图。调整机器人的位姿（顶点）来尽量满足边的约束，使得误差最小。

下面就是一个直观的例子。我们根据机器人位姿来作为图的顶点，这个位姿可以来自机器人的编码器，也可以是ICP匹配得到的，图的边就是位姿之间的关系。由于误差的存在，实际上机器人建立的地图是不准的，如下图左。我们通过设置边的约束，使得图优化向着满足边约束的方向优化，最后得到了一个优化后的地图（如下图中所示），它和真正的地图（下图右）非常接近。

图二
 

二、g2o 框架

前面我们简单介绍了图优化，也看到了它的神通广大，那如何编程实现呢？在SLAM领域，基于图优化的一个用的非常广泛的库就是g2o，它是General Graphic Optimization 的简称，是一个用来优化非线性误差函数的c++框架。这个库可以满足你调包侠的梦想第一次接触g2o，确实有这种感觉，而且官网文档写的也比较“不通俗不易懂”，不过如果你能捋顺了它的框架，再去看代码，应该很快能够入手了

其实g2o帮助我们实现了很多内部的算法，只是在进行构造的时候，需要遵循一些规则，在我看来这是可以接受的，毕竟一个程序不可能满足所有的要求，因此在以后g2o的使用中还是应该多看多记，这样才能更好的使用这个库。我们首先看一下下面这个图，是g2o的基本框架结构。如果你查资料的话，你会在很多地方都能看到。看图的时候要注意箭头类型

1、图的核心

上面这个图，一眼看过去，感觉东西还是很多的，不太好理解。那么我们先找到他的源头，也就是最左边中间部分的 SparseOptimizer。SparseOptimizer是整个图的核心，我们注意右上角的 is-a 实心箭头，这 SparseOptimizer 它是一个 Optimizable Graph，从而也是一个超图(HyperGraph)。这个呢，就不去研究了，不然可能黄花菜都凉了。先暂时只需要了解一下它们的名字，有些以后用不到，有些以后用到了再回看。目前如果遇到重要的部分会具体解释。
 

2、顶点和边

先来看上图的结构吧。注意看 has-many 箭头，超图(左上角HyperGraph)包含了许多顶点(HyperGraph::Vertex)和边(HyperGraph::Edge)。而这些顶点顶点继承自 Base Vertex，也就是OptimizableGraph::Vertex，而边可以继承自 BaseUnaryEdge（单边）, BaseBinaryEdge（双边）或BaseMultiEdge（多边），它们都叫做OptimizableGraph::Edge。可能这样说起来比较头昏，不过没有关系，只要知道有顶点和边这个名字即可。因为顶点和边在编程中很重要的，关于顶点和边的定义我们以后会详细说的。下面我们来看底部的结构。
 

3、配置SparseOptimizer的优化算法和求解器

整个图的核心SparseOptimizer 包含一个优化算法（OptimizationAlgorithm）的对象。OptimizationAlgorithm是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton（高斯牛顿法，简称GN）,Levernberg-Marquardt（简称LM法）, Powell’s dogleg 三者中间选择一个（常用的是GN和LM）
 

4、如何求解

OptimizationWithHessian 内部包含一个求解器（Solver），这个Solver实际是由一个BlockSolver组成的。这个BlockSolver有两个部分，一个是SparseBlockMatrix ，用于计算稀疏的雅可比和Hessian矩阵；一个是线性方程的求解器（LinearSolver），它用于计算迭代过程中最关键的一步HΔx=−b，LinearSolver有几种方法可以选择：PCG, CSparse, Choldmod，具体定义后面会介绍
 

三、环境搭建

通过上面的介绍，或许大家依然觉得比较蒙蔽，不过没有关系，下面来讲解几个示例代码，相信就比较透彻了。当然，在讲解示代码之前，需要搭建好示例代码运行的环境。本人的基本系统配置环境(docker)如下：

1、docker容器

docker pull ubuntu:18.04  # 拉取ubuntu18.04镜像

# 创建容器并且映射端口与目录
docker run  -dit --restart=always  --privileged   -v /tmp/.X11-unix:/tmp/.X11-unix   -v /work/4.my_work/1.zwh:/my_work      -p 12570:22    -e DISPLAY=:0    -e LANG=C.UTF-8  --shm-size 64G   --name  ub18.04-g2o-zwh   -w /  35b3f4f76a24   /bin/bash

docker exec -it  ub18.04-g2o-zwh /bin/bash # 进入容器 

apt-get update # 更新操作

apt-get install gcc g++ cmake # 安装基本软件

2、源码准备

首先下载好，并解压slam十四讲第二版的源码：
slam十四讲：https://github.com/gaoxiang12/slambook2

另外，还需要下载第三方库g2o：
g2o：https://github.com/RainerKuemmerle/g2o/tree/9b41a4ea5ade8e1250b9c1b279f3a9c098811b5a

也可以通过slam十四讲第二版的源码的源码链接进入

解压之后把 g2o-9b41a4ea5ade8e1250b9c1b279f3a9c098811b5a 文件重命名为 g2o，然后替换掉slam十四讲第二版源码下的 3rdparty/g2o 目录。

最终本人的目录分布如下：

 

3、opencv安装

首先我们需要安装opencv

官网网址：https://opencv.org/
github：https://github.com/opencv/opencv

本人下载的版本为：https://github.com/opencv/opencv/tree/3.4.18
参考编译文档：https://docs.opencv.org/3.4.18/d7/d9f/tutorial_linux_install.html

下载好 3.4.18 版本之后，进行解压，进入到 opencv-3.4.18 根目录，执行如下指令：

apt-get install build-essential
apt-get install libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev
apt-get install libtbb2 libtbb-dev libjpeg-dev libpng-dev libtiff-dev

cd ~/opencv
mkdir build
cd build
cmake ..
make -j7 # runs 7 jobs in parallel
make install

等待执行完成之后执行如下指令：

echo $(pkg-config --cflags --libs opencv)

输出打印如下，则表示安装成功：
 

4、g2o安装

依赖安装：

apt-get install libeigen3-dev  libsuitesparse-dev qtdeclarative5-dev qt5-qmake libqglviewer-dev-qt5

然后进入到 slambook2 源码根目录

cd 3rdparty/g2o/
mkdir build 
cd build
cmake ../
make -j7 # runs 7 jobs in parallel
make install

打印如下表示成功：

 

四、示例代码执行

准备好源码，并且大家所需的环境之后。进入到 slambook2 的 ch6 目录下，首先呢，需要修改 CMakeLists.txt，因为 Ceres 与 gaussNewton 我们是不需要的，所以注释了一些代码，最终结果如下：

cmake_minimum_required(VERSION 2.8)
project(ch6)

set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)

# OpenCV
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

# Ceres
#find_package(Ceres REQUIRED)
#include_directories(${CERES_INCLUDE_DIRS})

# g2o
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

# Eigen
include_directories("/usr/include/eigen3")

#add_executable(gaussNewton gaussNewton.cpp)
#target_link_libraries(gaussNewton ${OpenCV_LIBS})

#add_executable(ceresCurveFitting ceresCurveFitting.cpp)
#target_link_libraries(ceresCurveFitting ${OpenCV_LIBS} ${CERES_LIBRARIES})

add_executable(g2oCurveFitting g2oCurveFitting.cpp)
target_link_libraries(g2oCurveFitting ${OpenCV_LIBS} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY})

编写完成之后，执行指令如下：

cmake .
make -j7 # runs 7 jobs in parallel

如果报错如下：

[ 50%] Linking CXX executable g2oCurveFitting
CMakeFiles/g2oCurveFitting.dir/g2oCurveFitting.cpp.o: In function `main':
g2oCurveFitting.cpp:(.text.startup+0xca): undefined reference to `cv::RNG::gaussian(double)'
collect2: error: ld returned 1 exit status
CMakeFiles/g2oCurveFitting.dir/build.make:96: recipe for target 'g2oCurveFitting' failed
make[2]: *** [g2oCurveFitting] Error 1
CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/g2oCurveFitting.dir/all' failed
make[1]: *** [CMakeFiles/g2oCurveFitting.dir/all] Error 2
Makefile:83: recipe for target 'all' failed
make: *** [all] Error 2

则还需要修改 CMakeLists.txt 文件，添加如下内容:

#list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
list(APPEND CMAKE_MODULE_PATH /my_work/slambook2-master/3rdparty/g2o/cmake_modules)
set(G2O_ROOT /usr/local/include/g2o)
find_package(G2O REQUIRED)
include_directories(
${G2O_INCLUDE_DIRS}
"/usr/include/eigen3"
)

这里需要注意其上的 /my_work/slambook2-master/3rdparty/g2o/cmake_modules 需要替换成你本人的路径。然后重新执行 make -j7，本人打印如下：

在目录下，可以看到生成了可执行文件 g2oCurveFitting，执行指令 ./g2oCurveFitting，本人打印结果如下：

 

五、结语

通过该篇博客，对 g2o 了进行了简单的介绍，搭建好了示例代码的环境并且运行其可执行文件。下面就是对代码进行细致的讲解了，也就是理论结合实践。