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

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

一、前言

针对与 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、核心要点

图优化里的图就是数据结构里的图,一个图由若干个 顶点 ( V e r t e x ) \color{red}顶点 (Vertex) 顶点(Vertex),以及连接这些 顶点的边 ( E d g e ) \color{red} 顶点的边(Edge) 顶点的边(Edge)组成,举例,先看下图:
史上最简SLAM零基础解读(10.1) - g2o(图优化)→简介环境搭建(slam十四讲第二版为例)_第1张图片

图一

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

( 1 ) \color{blue} (1) (1) 构建图。机器人位姿作为顶点,位姿间关系作为边。
( 2 ) \color{blue} (2) (2) 优化图。调整机器人的位姿(顶点)来尽量满足边的约束,使得误差最小。

下面就是一个直观的例子。我们根据机器人位姿来作为图的顶点,这个位姿可以来自机器人的编码器,也可以是ICP匹配得到的,图的边就是位姿之间的关系。由于误差的存在,实际上机器人建立的地图是不准的,如下图左。我们通过设置边的约束,使得图优化向着满足边约束的方向优化,最后得到了一个优化后的地图(如下图中所示),它和真正的地图(下图右)非常接近。
史上最简SLAM零基础解读(10.1) - g2o(图优化)→简介环境搭建(slam十四讲第二版为例)_第2张图片

图二

 

二、g2o 框架

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

其实g2o帮助我们实现了很多内部的算法,只是在进行构造的时候,需要遵循一些规则,在我看来这是可以接受的,毕竟一个程序不可能满足所有的要求,因此在以后g2o的使用中还是应该多看多记,这样才能更好的使用这个库。我们首先看一下下面这个图,是g2o的基本框架结构。如果你查资料的话,你会在很多地方都能看到。看图的时候要注意箭头类型
史上最简SLAM零基础解读(10.1) - g2o(图优化)→简介环境搭建(slam十四讲第二版为例)_第3张图片

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十四讲第二版的源码的源码链接进入
史上最简SLAM零基础解读(10.1) - g2o(图优化)→简介环境搭建(slam十四讲第二版为例)_第4张图片

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

最终本人的目录分布如下:
史上最简SLAM零基础解读(10.1) - g2o(图优化)→简介环境搭建(slam十四讲第二版为例)_第5张图片
 

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

打印如下表示成功:
史上最简SLAM零基础解读(10.1) - g2o(图优化)→简介环境搭建(slam十四讲第二版为例)_第6张图片
 

四、示例代码执行

准备好源码,并且大家所需的环境之后。进入到 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,本人打印结果如下:
史上最简SLAM零基础解读(10.1) - g2o(图优化)→简介环境搭建(slam十四讲第二版为例)_第7张图片
 

五、结语

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

 
 
 

你可能感兴趣的:(史上最全slam从零开始,人工智能,计算机视觉,机器学习)