一起做RGB-D SLAM 第二季（一）

最新推荐文章于 2018-07-10 22:15:03 发布

老李lyj

最新推荐文章于 2018-07-10 22:15:03 发布

阅读量2k

点赞数

分类专栏： slam 文章标签： SLAM kinect

slam 专栏收录该内容

18 篇文章 1 订阅

订阅专栏

小萝卜：师兄！过年啦！是不是很无聊啊！普通人的生活就是赚钱花钱，实在是很没意思啊！

　　师兄：是啊……

　　小萝卜：他们都不懂搞科研和码代码的乐趣呀！

　　师兄：可不是嘛……

　　小萝卜：所以今年过年，我们再做一个SLAM吧！之前写的那个太烂了啦，我都不好意思说是我做的了！

　　师兄：嗯那可真是对不住你啊……

　　小萝卜：没事！你再写一个好一点的，我就原谅你了！写完再请我吃饭吧！

　　师兄：啊，好的……

　　小萝卜：师兄你别这么没精神啊！加油咯！

前言

　　在经过了一番激烈的思想斗争之后呢，师兄厌倦了年假的无聊生活，开始写《一起做RGBD SLAM》的第二季！在这一系列中，我们会讨论RGBD SLAM程序中一些更深入的话题，从而编写一个更快、更好用的程序。改进的地方大致如下：

多线程的优化：在建图算法计算时，定位算法没必要等待它结束。它们可以并行运行。
更好地跟踪：选取参考帧，并对丢失情况进行处理；
基于外观的回环检测：Appearance based loop closure；
八叉树建图：Octomap；
使用更快的特征：Orb；
使用TUM数据集，并与标准轨迹进行比较；
在线的Kinect demo；
代码会写得更像c++风格，而不是像上次的c风格；

　　这么一看，其实整体上问题还是挺多的。在第二季中，我们将致力于解决这些问题，同时我们的程序也会变得相对比较复杂。鉴于很多基础的问题我们在第一季中已经提过，本次我就不讲怎么安装OpenCV之类的事情啦。但是，为了保证大家能理解博客内容，我们和以往一样，给出实现过程中的所有代码和数据。

　　代码请参见：https://github.com/gaoxiang12/rgbd-slam-tutor2

　　TUM数据集网址：http://vision.in.tum.de/data/datasets/rgbd-dataset

　　本系列使用TUM中的一个数据：fr1_room。读者可以去TUM网站找，或者直接从我的百度云里下载： http://pan.baidu.com/s/1c1fviSS

　　TUM数据集的使用方法我们将在后文介绍。

关于代码

　　第二季中，我们仍使用C++和Cmake作为编程语言和框架。我使用的电脑是 Ubuntu 14.04 系统。读者也可以自行挑选其他linux操作系统，但是我只给出在ubuntu下安装各种工具的方式。

　　首先，请从github中下载这个系列用到的代码：

1 git clone https://github.com/gaoxiang12/rgbd-slam-tutor2.git

　　你会看到几个文件夹。和第一个系列一样，我们把不同的代码归类放置。几个文件夹的内容如下：

bin　　　　存放编译好的可执行文件；
src　　　　存放源代码；
include　　存放头文件；
experiment　　存放一些做实验与测试用的源文件；
config　　存放配置文件；
lib　　　　存放编译好的库文件；
Thirdparty　　一些小型的依赖库，例如g2o，dbow2，octomap等；

　　第一讲的代码还没有那么全。随着讲解的进行，我们会逐步将代码添加到各个文件夹中去。

　　我们构建代码的思路是这样的。把与slam相关的代码（include和src下）编译成一个库，把测试用的程序（experiment下）编译成可执行文件，并链接到这个slam库上。举例来说，我们会把orb特征的提取和匹配代码放到库中，然后在experiment里写一个程序，读一些具体的图片并提取orb特征。以后我们也将用这个方式来编写回环检测等模块。

　　至于为何要放Thirdparty呢？因为像g2o这样的库，版本有时会发生变化。所以我们就把它直接放到代码目录里，而不是让读者自己去找g2o的源码，这样就可以保证我们的代码在读者的电脑上也能顺利编译。但是像 opencv，pcl 这些大型又较稳定的库，我们就交给读者自行编译安装了。

　　除了Thirdparty下的库，请读者自行安装这样依赖库：

OpenCV 2.4.11　　请往opencv.org下载，注意我们没有使用3.1版本，而opencv2系列和3系列在接口上有较大差异。如果你用ubuntu，可以通过软件仓库来安装opencv：
```
sudo apt-get install libopencv-dev
```
PCL　1.7　　　　来自pointclouds.org。
Eigen3　　　　　　安装 sudo apt-get install libeigen3-dev

　　Thirdparty下的库，多为cmake工程，所以按照通常的cmake编译方式即可安装。它们的依赖基本可以在ubuntu的软件仓库中找到，我们会在用到时再加以介绍。

关于TUM数据集

　　本次我们使用tum提供的数据集。tum的数据集带有标准的轨迹和一些比较工具，更适合用来研究。同时，相比于nyud数据集，它也要更加困难一些。使用这个数据集时应当注意它的存储格式（当然使用任何数据集都应当注意）。

　　下面我们以fr1_room为例来说明TUM数据集的用法。fr1_room的下载方式见上面的百度云或者TUM官网。

　　下载我们提供的 “rgbd_dataset_freiburg1_room.tgz”至任意目录，解压后像这样：

　　rgb和depth文件夹下存放着彩色图和深度图。图像的文件名是以采集时间命名的。而rgb.txt和depth.txt则存储了所有图像的采集时间和文件名称，例如：

　　1305031910.765238 rgb/1305031910.765238.png

　　表示在机器时间1305031910.765238采集了一张RGB图像，存放于rgb/1305031910.765238.png中。

　　这种存储方式的一个特点是，没有直接的rgb-depth一一对应关系。由于采集时间的差异，几乎没有两张图像是同一个时刻采集的。然而，我们在处理图像时，需要把一个RGB和一个depth当成一对来处理。所以，我们需要一步预处理，找到rgb和depth图像的一一对应关系。

　　TUM为我们提供了一个工具来做这件事，详细的说明请看：http://vision.in.tum.de/data/datasets/rgbd-dataset/tools 该网页整理了一些常用工具，包括时间配对，ground-truth误差比对、图像到点云的转换等。对于现在预处理这一步，我们需要的是一个 associate.py 文件，如下（你可以直接把内容拷下来，存成本地的associate.py文件）：

 
   
 associate.py 

　　小萝卜：那么这个文件要怎么用呢？

　　如果读者熟悉python，就很容易看懂它的用法。实际上，只要给它两个文件名即可，它会输出一个匹配好的序列，像这样：

python associate.py rgb.txt depth.txt

　　输出则是一行一行的数据，如：

　　1305031955.536891 rgb/1305031955.536891.png 1305031955.552015 depth/1305031955.552015.png

　　小萝卜：我知道！这一行就是配对好的RGB图和深度图了，对吧！

　　师兄：对！程序默认时间差在0.02内的就可以当成一对图像。为了保存这个结果，我们可以把它输出到一个文件中去，如：

python associate.py rgb.txt depth.txt > associate.txt

　　这样，只要有了这个associate.txt文件，我们就可以找到一对对的RGB和彩色图啦！

　　小萝卜：配对配对什么的，总觉得像在相亲啊……

关于ground truth

　　ground truth是TUM数据集提供的标准轨迹，它是由一个外部的（很高级的）运动捕捉装置测量的，基本上你可以把它当成一个标准答案喽！ground truth的记录格式也和前面类似，像这样：

　　1305031907.2496 -0.0730 -0.4169 1.5916 0.8772 -0.1170 0.0666 -0.4608

　　各个数据分别是：时间，位置（x,y,z），姿态四元数（qx, qy, qz, qw），对四元数不熟悉的同学可以看看“数学基础”那几篇博客。那么这个轨迹长什么样呢？我们写个小脚本来画个图看看：

#!/usr/bin/env python
# coding=utf-8

import numpy as np
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d

f = open("./groundtruth.txt")
x = []
y = []
z = []
for line in f:
    if line[0] == '#':
        continue
    data = line.split()
    x.append( float(data[1] ) )
    y.append( float(data[2] ) )
    z.append( float(data[3] ) )
ax = plt.subplot( 111, projection='3d')
ax.plot(x,y,z)
plt.show()

把这部分代码复制存储成draw_groundtruth.py存放到数据目录中，再运行：

python draw_groundtruth.py

　　就能看到轨迹的形状啦：

　　第二件事，因为外部那个运动捕捉装置的记录频率比较高，得到的轨迹点也比图像密集很多，如何查找每个图像的真实位置呢？

　　还记得associate.py不？我们可以用同样的方式来匹配associate.txt和groundtruth.txt中的时间信息哦：

python associate.py associate.txt groundtruth.txt > associate_with_groundtruth.txt

　　这时，我们的新文件 associate_with_groundtruth.txt 中就含有每个帧的位姿信息了：

　　1305031910.765238 rgb/1305031910.765238.png 1305031910.771502 depth/1305031910.771502.png 1305031910.769500 -0.8683 0.6026 1.5627 0.8219 -0.3912 0.1615 -0.3811

　　是不是很方便呢？对于TUM中其他的序列也可以同样处理。

关于TUM中的相机

　　TUM数据集一共用了三个机器人，记成fr1, fr2, fr3。这三台相机的参数在这里： http://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats#intrinsic_camera_calibration_of_the_kinect

　　数据当中，深度图已经根据内参向RGB作了调整。所以相机内参以RGB为主：

Camera	fx	fy	cx	cy	d0	d1	d2	d3	d4
(ROS default)	525.0	525.0	319.5	239.5	0.0	0.0	0.0	0.0	0.0
Freiburg 1 RGB	517.3	516.5	318.6	255.3	0.2624	-0.9531	-0.0054	0.0026	1.1633
Freiburg 2 RGB	520.9	521.0	325.1	249.7	0.2312	-0.7849	-0.0033	-0.0001	0.9172
Freiburg 3 RGB	535.4	539.2	320.1	247.6	0	0	0	0	0

　　深度相机的scale为5000（和kinect默认的1000是不同的）。也就是depth/中图像像素值5000为真实世界中的一米。

　　因此，你下载了哪个序列，就要用对应的内参哦！

挑选一个IDE

　　现在让我们来写第一部分代码：读取tum数据集并以视频的方式显示出来。

　　嗯，在写代码之前呢，师兄还有一些话要啰嗦。虽然我们用linux的同学以会用vim和emacs为傲，但是写代码呢，还是希望有一个IDE可以用的。vim和emacs的编辑确实很方便，然而写c++，你还需要在类定义/声明里跳转，需要补全和提示。要让vim和emacs来做这种事，不是不可以，但是极其麻烦。这次师兄给大家推荐一个可以用于c++和cmake的IDE，叫做qtcreator。

　　安装qtcreator：

sudo apt-get install qtcreator

　　界面大概长这样：

　　这东西直接的好处是支持cmake。只要是cmake工程就可以丢进去编译。按住ctrl键可以在各个类定义/变量/实现之间快速导航。如果你的cmake设置成了debug模式，它还能进行断点调试，十分的好用！

　　此外，由于ROS使用的catkin也是cmake的形式，所以它还能用来调试ROS程序！

　　当然，因为叫qtcreator，自然还能写qt的程序……然而这似乎已经不重要了……具体配置请大家自行摸索啦！

使用qtcreator写一个hello slam

　　这件事情其实很简单的喽！

　　首先，随便找一个文件夹，作为你代码的根目录。在此目录下新建一个CMakeLists.txt，输入这些内容：

cmake_minimum_required( VERSION 2.8 )
project( rgbd-slam-tutor2 )

# 设置用debug还是release模式。debug允许断点，而release更快
#set( CMAKE_BUILD_TYPE Debug )
set( CMAKE_BUILD_TYPE Release )

# 设置编译选项
# 允许c++11标准、O3优化、多线程。match选项可避免一些cpu上的问题
set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -march=native -O3 -pthread" )

# 常见依赖库：cv, eigen, pcl
find_package( OpenCV REQUIRED )
find_package( Eigen3 REQUIRED )
find_package( PCL 1.7 REQUIRED )

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

# 二进制文件输出到bin
set( EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin )
# 库输出到lib
set( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib )

# 头文件目录
include_directories(
    ${PROJECT_SOURCE_DIR}/include
    )

# 源文件目录
add_subdirectory( ${PROJECT_SOURCE_DIR}/src/ )
add_subdirectory( ${PROJECT_SOURCE_DIR}/experiment/ )

重要部分已经加上注释。

　　然后，在src/和experiment/下也新建两个CMakeLists.txt，暂不填写内容：

touch src/CMakeLists.txt experiment/CMakeLists.txt

　　下面，用qtcreator菜单中的File->Open file or project，打开刚才写的CMakeLists.txt，它会识别出这是个cmake工程，并提示你要在何出构建。通常我们是新建一个build文件夹来构建的，所以这次也这么做好了：

　　这样就设置好啦。这时，点击左侧的小锤或按下Ctrl+B，就可以构建工程。但是由于现在工程是空的，并没有什么可以构建的。所以我们加一个helloslam试试。在experiment下新建一个helloslam.cpp文件，输入：

#include<iostream>
using namespace std;

int main()
{
    cout<<"Hello SLAM!"<<endl;
    return 0;
}

然后，修改experiment/CMakeLists.txt文件，告诉它我们要编译这个文件：

add_executable( helloslam helloslam.cpp )

　　然后，按下Ctrl+B，完成构建。此时会出现一个小绿条，提示你构建完毕。最后，点击左下绿色的三角按钮，运行此程序：

　　怎么样，是不是很轻松？

　　读者可以尝试按住Ctrl并点击变量，看看qtcreator是如何跳转的。或者人为加一句错误代码，看它会不会提示错误。也可以输入 cout. 看它会提示哪些东西。甚至可以调成Debug模式，设置断点，看程序是否会停在断点上。

下期预告

　　下期我们会讲基本的IO操作，包括参数文件的读取，TUM图像读取与显示，以及程序的测速等等。

问题

　　1. draw_groundtruth.py 跑不起来？

sudo apt-get install python-matplotlib python-numpy

　　再试试。

　　2.为什么我的qtcreator是白的？

　　黑色只是个配色，在Tools/optoins中进行修改。其实白的也挺好看的。

老李lyj

关注

0
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
一起做RGB-D SLAM 第二季（一）

小萝卜：师兄！过年啦！是不是很无聊啊！普通人的生活就是赚钱花钱，实在是很没意思啊！　　师兄：是啊……　　小萝卜：他们都不懂搞科研和码代码的乐趣呀！　　师兄：可不是嘛……　　小萝卜：所以今年过年，我们再做一个SLAM吧！之前写的那个太烂了啦，我都不好意思说是我做的了！　　师兄：嗯那可真是对不住你啊……　　小萝卜：没事！你再写一个好一点的，我就原谅你了！
复制链接

扫一扫