ORB_SLAM2配置——基于Ubuntu16.04+ROS+gazebo仿真

一、引言

ORB-SLAM2，它是基于单目、双目或RGB-D相机的一个完整的SLAM系统，其中包括地图重用、回环检测和重定位功能。这个系统可以适用于多种环境，无论是室内小型手持设备，还是工厂环境中飞行的无人机和城市中行驶的车辆，其都可以在标准CPU上实时运行。该系统的后端使用基于单目和双目观测的光束法平差法（bundle adjustment），这使得其可以精确估计轨迹的尺度。该系统包含一个轻量级的定位模式，它使用视觉里程计追踪未建图区域并匹配地图点，实现零漂移定位。
下文配置基于Ubuntu16.04系统，请线配置好系统。

二、ros（kinetic）系统的安装

OS系统目前有三大版本，分别是ROS Kinetic Kame（Ubuntu16.04）、ROS Melodic Morenia（Ubuntu18.04）、ROS Noetic Ninjemys（Ubuntu20.04）、ROS Humble（Ubuntu22.04） 。由于我们的Ubuntu系统是16.04版本，因此我们选择ROS Kinetic Kame进行安装。
链接：ROS系统配置

三、仿真环境搭建

1. 创建工作空间catkin_ws

（1）家目录下创建文件夹 catkin_ws。

mkdir catkin_ws

2）进入 catkin_ws文件夹，并在里面建立新的 src 文件夹。

cd catkin_ws
mkdir src

（3）进入src文件夹，并把当前文件夹初始化为workspace属性的文件夹。

cd src
catkin_init_workspace

如果/home/你的用户名/catkin_ws/src文件夹下出现了CMakeLists文件，说明工作空间生成成功。

（4）接下来对工作空间进行编译，打开一个新终端，进入catkin_ws文件夹。

cd  ~/catkin_ws
catkin_make

（5）产生install文件夹。

catkin_make install

（7）设置环境变量，打开一个新终端。

• 打开 ~/.bashrc

  sudo vim ~/.bashrc
  

• 并在 .bashrc 尾部添加

  source ~/catkin_ws/devel/setup.bash
  

• 刷新环境变量

  cd catkin_ws
  source ~/.bashrc
  

至此便完成了工作空间和功能包的创建，之后便可以在功能包中写程序并运行了。

2. 配置仿真环境

（1）下载一个中科院软件所的仿真环境。

github网址：https://github.com/DroidAITech/ROS-Academy-for-Beginners

下载好后，将上述zip文件解压缩后放至/home/你的用户名/catkin_ws/src文件目录下。
（2）安装依赖项

cd ~/catkin_ws
rosdep install --from-paths src --ignore-src --rosdistro=kinetic -y

发现有很多功能包无法通过rosdep来安装系统依赖项。

• 此时我们选择手动安装，提示我们缺什么就安装什么，命令格式如下：

   sudo apt-get install ros-kinetic-功能包名称（软件包中的_注意要替换成-）
   # 例如
   sudo apt-get install ros-kinetic-yocs-cmd-vel-mux
  

• 安装好后，再次执行：
  注意：如果继续报错，则需要继续手动安装功能包，只要将上述代码中的软件包名称不断替换成终端里提示的缺少的功能包即可，请耐心安装。
  最终出现上述提示，则说明已全部安装成功。

（3）检查 gazebo 版本，该仿真环境要求 gazebo 的版本在7.0以上。

gazebo -v

注意：如果 gazebo 版本低于7.0，则需要进行手动升级，依次执行如下命令即可。

 sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
 wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
 sudo apt-get update
 sudo apt-get install gazebo7

（4）编译工作空间。

cd ~/catkin_ws
catkin_make

刷新环境变量。

source ~/catkin_ws/devel/setup.bash （如果在~/.bashrc文件的末尾添加了source ~/catkin_ws/devel/setup.bash，则此步骤可以省略。）

四、ORB-SLAM2 的安装与配置

1.安装cmake，gcc，g++，git

先安装配置ORB-SLAM2所必要的一些工具（已经安装的忽略）。

sudo apt-get update
sudo apt-get install cmake gcc g++ git

2.安装Pangolin0.5(过高版本会导致错误)

Pangolin是一个轻量、便携的管理OpenGI显示、交互和提取的视频输入的快速开发库。其核心是一个简单的OpenGI视口管理器，它能帮助模块化3D可视化，不增加复杂性，提供一个先进且直接的3D导航处理器。Pangolin也提供一个操作程序的机制，这个机制通过配置文件和UI集成，有一个灵活的实时绘图仪，用于可视图形图像数据。
（1）安装依赖项

sudo apt-get install libglew-dev libpython2.7-dev
sudo apt-get install libxkbcommon-x11-dev

（2）下载并安装Pangolin0.5
Pangolin版本过高可能会出问题，经过测试0.5版本可用。
Pangolin0.5版本依赖C++的11版本，如果C++版本过高也会出错。
注意：不能用“git clone https://github.com/stevenlovegrove/Pangolin.git” 这串代码安装Pangolin，否则会默认安装最新版，导致后面运行出现错误。
解压后移动到主目录（其他目录也可），进入Pangolin文件夹进行编译。

先：找到Pangolin工程 CMakeLists.txt 加入如下代码：

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")

再：编译Pangolin

cd Pangolin
mkdir build
cd build
cmake .. 
make
sudo make install

（3）测试Pangolin0.5是否安装成功
先：找到HelloPangolin工程 CMakeLists.txt 加入如下代码：

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")

再：编译HelloPangolin

cd Pangolin
cd examples/HelloPangolin
mkdir build && cd build
cmake ..
make
./HelloPangolin

若出现一个如下的彩色方块，则说明安装成功。

3. 安装Eigen3

Eigen是可以用来进行线性代数、矩阵、向量操作等运算的C++库，它里面包含了很多算法。

sudo apt-get install libeigen3-dev

4. 安装OpenCV3(4会导致错误)

去官网下载opencv3系列，我选择的是3.4.10版本。
网站地址: https://opencv.org/releases
下载完成后，将其解压到主目录，并将该文件夹重命名为opencv3
（1）打开终端，安装依赖项。

sudo add-apt-repository "deb http://security.ubuntu.com/ubuntu xenial-security main"
sudo apt update
sudo apt install libjasper1 libjasper-dev
sudo apt-get install build-essential  
sudo apt-get install cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev 
sudo apt-get install python-dev python-numpy libtbb2 libtbb-dev libjpeg-dev libpng-dev libtiff-dev libjasper-dev libdc1394-22-dev   

（2）打开终端，编译OpenCV3（编译过程较慢，请耐心等待）。

cd opencv3
mkdir build
cd build
cmake ..
make 
sudo make install

（3）配置加载环境，打开一个新终端。

sudo gedit /etc/ld.so.conf.d/opencv.conf

（4）在打开的空白文件中添加如下代码：

/usr/local/lib  

（5）保存后关闭文件夹，打开一个新终端。

sudo ldconfig

（6）配置.bashrc文件

sudo gedit /etc/bash.bashrc

在打开的文件末尾添加以下两行代码：

PKG_CONFIG_PATH=$PKG_CONFIG_PATH:/usr/local/lib/pkgconfig
export PKG_CONFIG_PATH

（7）保存后关闭文件夹，打开一个新终端。

source /etc/bash.bashrc
sudo updatedb

（8）测试OpenCV3是否安装成功

cd opencv3/samples/cpp/example_cmake
cmake .
make
./opencv_example

如果出现"Hello OpenCV"，则说明OpenCV3已安装成功。

5. 安装ORB-SLAM2

下载源码: https://github.com/raulmur/ORB_SLAM2

（1）普通模式
1.编译（注意在src文件夹中打开终端）。

cd ORB_SLAM2
./build.sh

注意：如果编译时报错 c++: internal compiler error: 已杀死 (program cc1plus)，原因是编译需要的内存不够了，解决办法增加虚拟机内存，亲自尝试虚拟机4G不够，8G够。

2.测试。
下载数据集，Computer Vision Group - Dataset Download，下载第一个 fr1/xyz 即可。由于是国外数据集，需要挂梯子，否则下载很慢。
下载好后，进入 ORB_SLAM2 文件夹创建一个文件夹：data，将下载好的数据集压缩包提取到 data 文件夹下面，然后在ORB_SLAM2文件夹下打开一个终端：

./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml ./data/rgbd_dataset_freiburg1_xyz

可以跑通数据集，则证明安装成功。

（2）ROS模式
为了能够在线实时地运行ORB-SLAM2，需要借助ROS。SLAM在ROS下跑和非ROS下跑的区别：可以把ROS理解成在Linux系统下集成了许多好用的第三方和多进程编程的一个库集。在ROS下跑SLAM可以使用ROS的许多成熟的方法库。不在ROS下跑，很多进程间通信、同步、回调之类的就需要自己去设计。

将包含Examples/ROS/ORB_SLAM2的路径添加到ROS_PACKAGE_PATH环境变量中。打开.bashrc文件并在最后添加以下行。

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/用户名/catkin_ws/src/ORB_SLAM2/Examples/ROS

保存后并退出，刷新环境变量。

source ~/.bashrc

如果能正确显示刚刚加入的地址则正常。

echo ${ROS_PACKAGE_PATH}

此仿真环境下，发布的相机图像的话题为 /camera/depth/image_raw 和 /camera/rgb/image_raw，所以我们slam接受话题的数据时名称要一致。打开 /home/用户名/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc 进行修改。位置大概在68行左右，修改成如下所示：

message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "camera/depth/image_raw", 1);

编译ROS下的ORB-SLAM2

./build_ros.sh

注意：如果编译出错
在~/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/CMakeLists.txt里添加一句-lboost_system
解决办法来源：https://github.com/raulmur/ORB_SLAM2/issues/494

set(LIBS
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so
-lboost_system
)
然后再次编译即可！！！

五、运行仿真环境

运行仿真时，建议关闭虚拟机的加速3D图形，否则运行会卡顿。
先关闭虚拟机，打开虚拟机设置，选择显示器栏，关闭 加速3d图形 选项。

（1）roscore是节点和程序的集合，这些节点和程序是基于ROS的系统所必需的。运行roscore，使ROS节点建立通信。
打开一个新终端，输入：

roscore

（2）打开仿真环境。
打开一个新终端，输入：

roslaunch robot_sim_demo robot_spawn.launch

（3）运行键盘控制节点。
打开另一个终端，输入：
注意：将光标定位到此终端上，才能通过按键控制机器人移动！

rosrun robot_sim_demo robot_keyboard_teleop.py

（4）运行orbslam2。
打开一个新终端，输入：

rosrun ORB_SLAM2 RGBD /home/用户名/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/用户名/catkin_ws/src/ORB_SLAM2/Examples/RGB-D/Zdzn.yaml

此时通过按键控制机器人移动，即可进行定位与建图工作！

注意：如果没有 Zdzn.yaml，新建一个即可
Zdzn.yaml

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 277.19135641132203
Camera.fy: 277.19135641132203
Camera.cx: 160.5
Camera.cy: 120.5

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0

Camera.width: 320
Camera.height: 240

# Camera frames per second 
Camera.fps: 50.0

# IR projector baseline times fx (aprox.)
Camera.bf: 40.0

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

# Close/Far threshold. Baseline times.
ThDepth: 40.0

# Deptmap values factor
DepthMapFactor: 1.0

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000

# ORB Extractor: Scale factor between levels in the scale pyramid 	
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid	
ORBextractor.nLevels: 8

# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast			
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500