小强ROS机器人教程(17)___利用ORB_SLAM2建立环境三维模型

%E5%88%A9%E7%94%A8orb_slam2%E5%BB%BA%E7%AB%8B%E7%8E%AF%E5%A2%83%E4%B8%89%E7%BB%B4%E6%A8%A1%E5%9E%8B”>http://community.bwbot.org/topic/60/%E5%B0%8F%E5%BC%BAros%E6%9C%BA%E5%99%A8%E4%BA%BA%E6%95%99%E7%A8%8B-17-%E5%88%A9%E7%94%A8orb_slam2%E5%BB%BA%E7%AB%8B%E7%8E%AF%E5%A2%83%E4%B8%89%E7%BB%B4%E6%A8%A1%E5%9E%8B
想要实现视觉导航，空间的三维模型是必须的。ORB_SLAM就是一个非常有效的建立空间模型的算法。这个算法基于ORB特征点的识别，具有准确度高，运行效率高的特点。我们在原有算法的基础上进行了修改，增加了地图的保存和载入功能，使其更加适用于实际的应用场景。下面就介绍一下具体的使用方法。

准备工作

在启动ORB_SLAM2之前，请先确认小强的摄像头工作正常。
ORB_SLAM2建图过程中需要移动小车，移动小车过程中ORB_SLAM2的运行状态不方便显示(ssh方式比较卡顿，也不可能拖着显示器)，因此请先安装好小强图传遥控windows客户端。

启动ORB_SLAM2

更改配置文件
ORB_SLAM2的配置文件位于/home/xiaoqiang/Documents/ros/workspace/src/ORB_SLAM2/Examples/ROS/orb_slam2/Data文件夹内。更改setting.yaml其中的LoadMap的值，将其设置为0。当设置为1的时候程序会在启动后自动从/home/xiaoqiang/slamdb文件夹内载入地图数据。当设置成0时，就不会载入地图数据。由于我们是要创建地图，所以LoadMap要设置为0。
0_1489828772487_4.png
ssh方式进入小强，执行以下指令

ssh -X [email protected]

2017年之前购买用户,请执行

roslaunch ORB_SLAM2 start.launch

2017年之后购买用户，请执行

roslaunch orb_slam2 start.launch
0_1489829013495_5.png

开始建立环境三维模型

打开小强图传遥控windows客户端，点击“未连接“按钮连接小强。在图传窗口右键打开”原始图像“和”ORB_SLAM2的特征点图像“
0_1489829229351_1.png
0_1489829544143_0.png
上图中，左侧图像是摄像头原始彩色图像，右侧是ORB_SLAM2处理后的黑白图像。当前ORB_SLAM2还没有初始化成功，所以黑白图像没有特征点。
按住”w”键开始遥控小强往前缓慢移动，使ORB_SLAM2初始化成功，即黑白图像开始出现红绿色特征点
0_1489829731061_2.png
现在就可以开始遥控小强对周围环境建图，遥控过程中需要保证黑白图像一直存在红绿色特征点，不存在则说明视觉lost了，需要遥控小强退回到上次没lost的地方找回视觉位置。
客户端操作演示视频

使用rviz查看建图效果

在本地虚拟机hosts中添加小强ip地址,然后新开一个终端打开rviz

export ROS_MASTER_URI=http://xiaoqiang-desktop:11311
rviz
打开/home/xiaoqiang/Documents/ros/src/ORB_SLAM/Data/rivz.rviz配置文件
0_1489831624309_6.png
如下图所示，红黑色点是建立的三维模型（稀疏特征点云），蓝色方框是keyframe可以表示小强轨迹
0_1489831763020_7.png

保存地图

当地图建立的范围满足自己要求后，在虚拟机新开一个命令窗口，输入下列命令保存地图

ssh -X [email protected]
rostopic pub /map_save std_msgs/Bool ‘{data: true}’ -r 0.1
这个命令每隔10s触发1次保存任务，因此当看到下图时，请关闭上面的窗口
在虚拟机新开一个命令窗口，输入下列命令侦测地图保存命令有没有发出

ssh -X [email protected]
rostopic echo /map_save
0_1489832316715_8.png

地图文件会被保存进用户主目录的slamdb文件夹内。

地图的载入

保存地图之后可以再次载入ORB_SLAM2程序中。地图载入后程序可以非常迅速的定位出摄像头的具体位置。这样就可以开发出基于视觉定位的导航算法了。
载入地图的方式也非常简单。将/home/xiaoqiang/Documents/ros/workspace/src/ORB_SLAM2/Examples/ROS/orb_slam2/Data文件夹内的setting.yaml文件中的LoadMap的值设置为1。

地图的后期处理

在建立地图之后，想要使用这个地图进行导航，还需要对地图文件做进一步的操作。比如创建导航的线路，标记导航路径的行走方式等等。这一部分内容可以参见这篇文章视觉导航路径编辑器使用教程。