机器人SLAM与自主导航（三）——SLAM功能包

机器人SLAM与自主导航（三）——SLAM功能包

一、gmapping

1、gmapping功能包

输入：1.深度信息 2.IMU信息 3.里程计信息
输出：栅格地图

$ rosmsg show nav_msgs/OccupancyGrid 

2、栅格地图取值原理

3、gmapping安装

$ sudo apt-get install ros-kinetic-gmapping

4、配置gmapping节点

参数说明可参考： http://wiki.ros.org/gmapping

5、启动gmapping演示（深度信息基于激光雷达）

分别开启三个终端运行以下命令：

$ roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch    # 启动仿真环境
$ roslaunch mbot_navigation gmapping_demo.launch  # 启动建图节点，灰色地图建成，黑色障碍物
$ roslaunch mbot_teleop mbot_teleop.launch        #启动键盘控制节点 

建图完毕，保存地图

$ rosrun map_server map_saver -f cloister_gmapping  

# cloister_gmapping是文件名的意思，是自己保存文件名的意思
# 保存的路径在当前/home文件夹下,有两个文件.pgm和.yaml

查看生成地图文件

6、启动gmapping（kinect）

建图效果不佳（不推荐）

$ roslaunch mbot_gazebo mbot_kinect_nav_gazebo.launch
$ roslaunch mbot_navigation gmapping_demo.launch
$ roslaunch mbot_teleop mbot_teleop.launch

二、hector_slam

1、hector_slam功能包

输入：深度信息
输出：栅格地图

2、安装hector_slam

$ sudo apt-get install ros-kinetic-hector-slam

3、配置hector_mapping节点

参数说明可参考：http://wiki.ros.org/hector_slam

4、启动hector_slam演示（深度信息基于激光雷达）

第一个节点启动了gazebo仿真，它使用仿真的雷达给出了一个scan扫描数据；而后下面的节点订阅了这个数据并进行处理，最后得到一个二维栅格地图。

5、打滑现象

hector_slam功能包使用时要求机器人移速不能过快，否则运算跟不上会出现打滑

hector这个算法有一个很大的问题，就是在于它的自定位，它利用自身的激光雷达数据进行定位，如果上一次的扫描数据与下一次的扫描数据基本一致的话，它就很难确定自己是否在运动，例如当我给一个足够大的空间，这里有一条没有任何特征点的长廊，长度远远超过激光扫描的最大距离时，hector基本就失去了建图的能力。

三、cartographer

1、cartographer功能包

2、cartographer安装

方法一（新方法）：

sudo apt-get update
 
sudo apt-get install ros-<your ros version>-cartographer*  # 安装全部关于cartographer的包

方法二：
cartographer功能功能包没有集成到ROS的软件源里面，所以需要采用源码编译方式进行安装。为了不与其他安装包冲突，最好为cartographer专门创建一个工作空间，这里我们创建了一个工作空间catkin_google_ws：

cartographer功能包可能出现的问题

3、配置cartographer节点

4、启动2D slam demo演示

5、启动3D slam demo演示

6、参数配置

7、启动cartographer仿真

四、Karto_slam

1、Karto_slam概况

评价：
Karto_SLAM是基于图优化的思想，用高度优化和非迭代 cholesky分解进行稀疏系统解耦作为解。图优化方法利用图的均值表示地图，每个节点表示机器人轨迹的一个位置点和传感器测量数据集，每个新节点加入，就会进行计算更新。
Karto_SLAM的ROS版本，其中采用的稀疏点调整（the Spare Pose Adjustment(SPA)）与扫描匹配和闭环检测相关。landmark越多,内存需求越大,然而图优化方式相比其他方法在大环境下制图优势更大，因为他仅包含点的图(robot pose)，求得位姿后再求map。

2、Karto_slam具体介绍

整体程序框架

从上图中可以看出，其实流程还是相当简单的，slam传统软实时的运行机制，每进入一帧数据，即进行处理，然后返回。
1）扫描匹配
关键词：
兴趣区域：
匹配的方式是scanTomap，兴趣区域就是矩形形状的submap，也可以将这块区域理解为参考模型。
搜索区域：
以里程计估计的位置为中心的一个矩形区域，用以表示最终位置的可能范围，在匹配时，遍历搜索区域，获取响应值最高的位置。
查找表：
对于激光获得的数据信息，以一定的角分辨率和角偏移值进行投影，获取查找表，用以匹配。
Running-scans：
实时维护的局部激光数据链，首末两帧距离在一定距离范围内，且满足一定数据规模，否则需要删除末端数据帧。维护当前局部数据链。
生成submap(因为匹配的方式是ScanToMap)

生成查找表：
查找表存在的意义就是相比于暴力匹配，不要每次都重新计算每个激光数据信息，相同角度不同位置的激光数据信息只需要被索引一次。

由里程计预测可以得到当前预估的姿态角，真实的姿态角必定在附近；
以一定的分辨率和偏移值对原机器人坐标系下（局部坐标系）表示的激光信息进行不同角度的映射，获得一个包含n个角度的查找表。
其中：

查找表->submap（匹配）
移动机器人自身的状态包括 (x,y,θ)，通过查找表的方式有效解决了角度的问题，眼下仍需解决位置的问题，采用离散化搜索区域进行定位，并利用多分辨率提高搜索速度。

响应值的计算如下描述：将查找表以一定的为宜投到submap上，此时的submap已有running-scans生成，并利用高斯进行模糊，假设总共有n个点被查找表击中，击中的每个点得分不同（高斯模糊的作用），累加得分并除以可以达到的最高得分。

查找表->submap（粗匹配->精匹配）
为提高搜索效率，采用多分辨率的方式，即粗匹配时采用较低的分辨率搜索得到候选区域，再对候选区域进行划分得到精确求解。

计算节点均值
在每次匹配过程中（粗匹配和精匹配），选取几个拥有最优响应值的位姿状态，取平均值作为匹配结果。
最优响应值为一个，设立一个10^-6作为容忍度，一旦有其他位姿状态的响应值与最优响应值在容忍度内，即为相等；最终的节点为它们的平均值。
迭代：
粗匹配（获得均值）->（作为初值）精匹配（得到最终均值）
计算节点协方差——位置协方差（粗匹配）

计算节点协方差——角度协方差（粗匹配）

上述协方差存在和理论相背的情况，主要每次计算的分量需要乘上一个响应值。
2）添加顶点和边
添加顶点：即关键帧的位姿
添 加 边： 当前帧与前一帧、running-scans、near-chains建立连接。

添加边主要经过三个步骤：
（1）Link to previsous scan
（2）Link to Running scans
RunningScan chain：一定数量且距当前一定距离内的激光数据链。
（3）Link to other near chains
NearChain: 以当前节点开始广度优先的方式从graph中遍历相邻的一定距离范围内所有节点，依据当前id从sensorManager中分别递增与递减寻找一定范围内的chain， 生成nearLinkScans。
前两个步骤都好理解，主要是第三个，以当前节点为搜索中心，以一定范围（比如说4m）进行广度搜索，得到了所有关联的节点，然后利用节点生成数据链（数据链需要包含约5个ID连续的关键帧）（不包括当前结点，否则放弃该数据链），用当前节点进行匹配，若响应值达到一定阈值，产生一条边，这条边一端是当前节点，还有一端是数据链中质心距离当前节点质心最近的节点。

3）回环检测
回环检测的操作和添加邻近边类似，步骤较为繁琐：
（1）依据当前的节点, 从Graph中找到与之相邻的所有节点(一定距离范围内)。
（2）采取广度优先搜索的方式，将相邻（next）与相连（adjacentVertices）添加进nearLinkedScans。
（3）从sensorManager中取从前到后，依据id序号挑选与当前在一定距离范围内，且不在nearLinkedScans中的candidateScans, 当数量达到一定size，返回。
（4）loopScanMatcher进行scanTomap的匹配，当匹配response 和covariance达到一定要求认为闭环检测到。得到调整的correct pose。
（5）Add link to loop : 调整边（全局闭环）。
（6）触发correctPose: spa优化
第一步，首先去除那些和当前节点的时间相邻的节点，针对那些在搜索范围内，且在时间点上并不相邻的节点产生一个数据链，然后进行匹配，若响应值大于阈值，添加回环并进行全局优化。

4）优化求解

整个优化算法使用了LM优化算法，原文中，作者大篇幅的都是介绍如何得到海森矩阵，以及高效进行优化求解，LM系统如下：

海森矩阵的稀疏：

3、karto_slam总结

1）单从论文角度看，论文并没有关于整个框架的介绍，大部分的内容都是关于优化方法、黑森矩阵的构造等。关于扫描匹配、顶点和边、回环检测等所提甚少。
2）源代码包含主函数和库函数两部分，框架清晰，可读性强；除了eigen，不需要额外的依赖库。
3）关于karto_slam的介绍不如hector和gmapping多，可能是 没有采用主流的地图更新机制（概率地图）。只保存了pose节点和激光数据，作为pose-graph。

五、ORB_SLAM

1、ORB_SLAM功能包

基于特征点的实时单目SLAM系统
实时解算摄像机的移动轨迹
构建三维点云地图
不仅适用于手持设备获取的一组连续图像，也可以应用于汽车行驶过程中获取的连续图像

Raul Mur-Artal，J.M.M.Montiel和Juan D.Tardos于2015年发表在IEEE Transactions on Robotics上

2、ORB_SLAM2安装

1）、安装工具&下载源码：

sudo apt-get install libboost-all-dev libblas-dev liblapack-dev
git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2       # 克隆到home下

2）、安装eigen3.2.10

去官网下载：http://eigen.tuxfamily.org/index.php?title=Main_Page

解压源码包，并进入目录：

mkdir build

cd build

cmake ..

make

sudo make install

3）、编译g2o

cd ~/ORB_SLAM2/Thirdparty/g2o/

mkdir build

cd build

cmake .. -DCMAKE_BUILD_TYPE=Release

make

4）、编译DBoW2

cd ~/ORB_SLAM2/Thirdparty/DBoW2/

mkdir build && cd build 

cmake .. -DCMAKE_BUILD_TYPE=Release

make

5）、编译Pangolin

sudo apt-get install libglew-dev 

git clone https://github.com/stevenlovegrove/Pangolin.git

cd Pangolin

mkdir build && cd build 

cmake ..

cmake --build .

6）、编译ORM_SLAM

cd ~/ORB_SLAM2

mkdir build && cd build 

cmake .. -DCMAKE_BUILD_TYPE=Release

make

7）、编译功能包（配置ROS环境）

# 首先修改.bashrc文件
$ cd ~
$ gedit .bashrc

# 打开.bashrc文件在最后一行加入
source ~/ORB_SLAM2/Examples/ROS/ORB_SLAM2/build/devel/setup.bash

$ source ~/.bashrc

注意：第7步，编译功能包 出现的问题：

解决方法：

修改 ~/ORB_SLAM2/Examples/ROS/ORB_SLAM2/CMakeLists.txt,添加 -lboost_system

3、启动单目SLAM示例（基于数据包）

1）、首先下载数据包TUM：这个主要是室内，单目和RGB-D

https://vision.in.tum.de/data/datasets/rgbd-dataset/download#freiburg1_desk

在ORB-SLAM2下新建文件夹Data，把测试的数据解压在这里。
TUM数据集分为相机fr1，fr2，fr3，对应TUM1-3.yaml；
一般第一次测试用fr1/xyz这个数据集，这个就是x，y，z方向来回动，用来检测一下系统出没出什么问题。
其他的数据看名字就知道，比如desk就是在桌子附近来回转，room就是在房间里面扫来扫去。
值得注意的是，运行其他数据集的时候，单目不一定能追踪成功，在台式机上能成功的在虚拟机上也不一定能成功，这就需要我们进行一些调整，比如调整初始化需求数量等，这个关系到对SLAM系统的理解。

2）、分别打开三个终端运行下面的三个命令：

$ roscore

# 进入ORB_SLAM2目录下启动Mono功能节点。
# 看到开启的等待数据的可视化建图界面
$ rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml

# 进入数据包所在目录下，运行命令
# /camera/image_raw表示播放的数据要发布的话题名称，此时可以在界面中看到建图的效果
$ rosbag play rgbd_dataset_freiburg1_desk.bag /camera/rgb/image_color:=/camera/image_raw

PATH_TO_VOCABULARY：算法参数文件，在ORB_SLAM2/Vocabulary中，将其中的压缩包解压即可；

PATH_TO_SETTINGS_FILE：相机参数设置文件，需要对camera进行标定产生，也可以使用ORB_SLAM2/Examples/ROS/ORB_SLAM2中已有的设置文件Asus.yaml。

4、启动AR示例（基于数据包）

$ roscore

# 进入ORB_SLAM2目录下启动Mono功能节点。
# 看到开启的等待数据的可视化建图界面
$ rosrun ORB_SLAM2 MonoAR Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml

# 进入数据包所在目录下，运行命令
# /camera/image_raw表示播放的数据要发布的话题名称，此时可以在界面中看到建图的效果
$ rosbag play rgbd_dataset_freiburg1_desk.bag /camera/rgb/image_color:=/camera/image_raw

点击界面Insert Cube，看到正方形插入进来，展示AR效果

5、启动ORB_SLAM示例（真实摄像头）

$ roslaunch mbot_navigation usb_cam_remap.launch

# 进入ORB_SLAM2目录下启动Mono功能节点。
# 看到开启的等待数据的可视化建图界面
$ rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml

6、启动AR示例（真实摄像头）

摄像头要运动起来，来完成地图构建

$ roslaunch mbot_vision usb_cam_remap.launch

# 进入ORB_SLAM2目录下启动Mono功能节点。
# 看到开启的等待数据的可视化建图界面
$ rosrun ORB_SLAM2 MonoAR Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml