搭建VINS-MONO环境与评估EuRoc定位精度、安装RealSense ZR300、D435驱动

本周顺利搭建了Vins-Mono框架并跑通示例，计算了Vins-Mono在EuRoc数据集上的定位精度，也安装了RealSense ZR300和RealSense D435两款深度摄像头进行具体测试实验。记录一下整体的安装与配置流程，并总结遇到的问题。
参考博客：
https://blog.csdn.net/q1370992706/article/details/79950542

一、VINS-Mono环境搭建

VINS-Mono是单目视觉惯性系统的实时SLAM框架。它使用基于优化的滑动窗口配方来提供高精度的视觉惯性里程计，具有高效的IMU预积分，包括偏差校正，自动估算器初始化，在线外部校准，故障检测和恢复，回环检测，全局位姿图优化，地图合并，位姿图重用，在线时间校准，卷帘快门支持。VINS-Mono主要用于自主无人机的状态估计和反馈控制，但它也能够为AR应用提供精确的定位。
官方代码链接：https://github.com/HKUST-Aerial-Robotics/VINS-Mono
此代码在Linux上运行，并与ROS完全集成。

1）、安装ROS工具包与依赖项ceres-solver

首先确认VINS-Mono安装的环境要求为Ubuntu 16.04与ROS Kinetic，由于之前已经安装了ROS Kinetic，在这里不再进行过多赘述。【详情请参考本人上一篇博客Ubuntu16.04下的ORB SLAM2环境搭建教程中的ROS安装过程】
接下来安装一些ROS的额外工具包，命令行如下：

sudo apt-get install ros-kinetic-cv-bridge ros-kinetic-tf ros-kinetic-message-filters ros-kinetic-image-transport

下面安装依赖项ceres-solver，官网链接为：http://ceres-solver.org/installation.html

Tips.注意，这里一定要留心版本问题，官网明确指出了：Our testing environment: Ubuntu 16.04, ROS Kinetic, OpenCV 3.3.1, Eigen 3.3.3，在我尝试的过程中，发现OpenCV版本不一致没有什么影响，但如果Eigen版本不对是一定会出问题的。
当时我的全部环境都已经安装配置完毕，但运行EuRoC MAV数据集时只能显示特征点，没有定位的轨迹地图，各种重新编译试错都没能解决问题。最后尝试将euroc.launch的ROS节点截断，通过查看源码定位到estimator.cpp的第321行，发现estimator没能正确初始化，怀疑是Eigen版本不对，于是重新手动安装了Eigen3.3.3版本，环境也全部重新编译最终才正确显示。在这里万分感谢某位不愿透露姓名的大神同事帮忙调试源码。

建议直接手动安装Eigen3.3.3版本，安装包下载网址：http://eigen.tuxfamily.org/index.php?title=Main_Page
在官网下载Eigen 3.3.3. : tar.gz 格式压缩文件，之后运行如下命令行：

#将eigen-eigen-5a0156e40feb.tar.g文件解压到/usr/local/include目录下
sudo tar -xzvf eigen-eigen-5a0156e40feb.tar.gz -C /usr/local/include  #-C 建立解压缩档案
#将eigen-eigen-5a0156e40feb文件更名为eigen3
sudo mv /usr/local/include/eigen-eigen-5a0156e40feb /usr/local/include/eigen3 
#将目录Eigen下的所有文件及其子目录复制到目录/usr/local/include中
sudo cp -r /usr/local/include/eigen3/Eigen /usr/local/include  

Tips.注意，因为eigen3 被默认安装到usr/local/include里，是系统默认路径，在很多程序中include时经常使用#include
而不是使用#include
，所以要做上述处理，否则一些程序在编译时会因找不到Eigen/Dense而报错。
这里最好再执行语句：sudo cp usr/local/include/eigen3/ /usr/include/ -R 复制一份到/usr/include/目录，否则后面会遇到一个错误，具体情况后文会进行说明。

OPenCV的安装这里就不多做介绍了，接下来安装ceres-solver ，命令行如下：

# CMake
sudo apt-get install cmake
# google-glog + gflags
sudo apt-get install libgoogle-glog-dev
# BLAS & LAPACK
sudo apt-get install libatlas-base-dev
# Eigen3
sudo apt-get install libeigen3-dev
git clone https://github.com/ceres-solver/ceres-solver
cd ceres-solver
mkdir build
cd build 
cmake ..
make -j4
sudo make install 

2）、安装与编译VINS-Mono

直接按照如下命令行进行VINS-Mono的安装：

mkdir -p vins-mono-catkin_ws/src #新建文件夹，这里我的文件夹命名为vins-mono-catkin_ws
cd vins-mono-catkin_ws/src/
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git
cd ../
catkin_make
source ~/vins-mono-catkin_ws/devel/setup.bash

3）、运行Vins-Mono

下载EuRoC MAV数据集，运行Vins-Mono检验是否安装成功。
数据集链接：https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets
选择Machine Hall 01的ROS bag进行下载，里面包含摄像头拍摄的视频帧图像、imu惯导信息，以及Ground-Truth真值等等。
之后打开四个终端，分别输入如下命令行：

source devel/setup.bash
roslaunch vins_estimator euroc.launch 

source devel/setup.bash
roslaunch vins_estimator vins_rviz.launch

source devel/setup.bash
roslaunch benchmark_publisher publish.launch  sequence_name:=MH_01_easy
#能够在界面上显示Ground-Truth真值，红线为真值，绿线为实时定位结果

source devel/setup.bash
rosbag play YOUR_PATH_TO_DATASET/MH_01_easy.bag

Tips.注意，这里要记得执行命令行source devel/setup.bash，将对应的launch文件添加到路径里，否则会报如下错误：
[catvehicle.launch] is neither a launch file in package [pilotless_automobile] nor is [pilotless_automobile] a launch file name The traceback for the exception was written to the log file

如果无法正常运行，查看vins-mono-catkin_ws/src/VINS-Mono/config/euroc/euroc_config.yaml 该yaml文件中的输出路径是否正确，新建一个自己的输出路径。同时将euroc.launch第三行代码前面的Tab空格去掉。

正确运行结束后的界面如下图所示：

在执行vins_estimator euroc.launch的终端中执行命令s能够对当前轨迹地图进行保存。

4）、评估定位精度

接下来介绍对Vins-Mono定位精度的评估方式。在这里我使用了ORB_SLAM2框架中的评估脚本，通过计算绝对轨迹误差（ATE）测量视觉SLAM系统的性能，其官网链接如下：
https://vision.in.tum.de/data/datasets/rgbd-dataset/tools
下载associate.py脚本，该脚本定义了如何对齐时间戳，同时下载误差的计算脚本evaluate_ate.py。
在你的输出路径中找到文件vins_result_loop.csv ，此文件记录了回环后相机坐标系下的Vins-Mono定位位置坐标数据，在Vins-Mono源码中找到vins-mono-catkin_ws/src/VINS-Mono/benchmark_publisher/config/MH_01_easy/data.csv，此文件中的数据为数据集中的Ground-Truth真值，采用的是世界坐标系。
执行如下命令行获得Vins-Mono的定位均方根误差：

python evaluate_ate.py --save SAVE --save_associations SAVE_ASSOCIATIONS --plot PLOT --verbose data.csv vins_result_loop.csv 

屏幕上会显示计算后的误差结果，如下图所示：

同时会输出轨迹上各个时间戳的定位误差图，如下图所示：

二、安装Intel RealSense ZR300驱动

英特尔RealSense深度鱼眼摄像头ZR300内置惯导IMU，较一般的深度摄像头有着更高的定位精度。
官网链接：https://github.com/IntelRealSense/realsense_sdk_zr300

1）、安装驱动Maplab_Realsense

官网链接：https://github.com/ethz-asl/maplab_realsense
这里使用的ROS上RealSense的驱动是eth-asl的maplab_realsense，这个程序对IMU的陀螺仪、加速度计、图像的时间戳做了对齐处理。通过实验发现，RealSense ZR300的加速度计和陀螺仪的时间戳不是完全相同的，这主要是由于RealSense ZR300的加速度计的频率为250Hz而陀螺仪的频率为200Hz，但是由于RealSense ZR300的时间戳是在硬件上打的，不是操作系统接收到到图像和IMU的时间戳，所以可以通过插值的方式使它们的时间戳对齐。
下面是maplab_realsense的安装与编译过程，命令行如下：

mkdir -p realsense_ws/src  #新建自己的安装路径，我自己的安装文件夹为realsense_ws
cd realsense_ws/src
sudo apt install ros-kinetic-librealsense
git clone https://github.com/catkin/catkin_simple 
git clone https://github.com/ethz-asl/glog_catkin
git clone https://github.com/ethz-asl/gflags_catkin
git clone https://github.com/ethz-asl/eigen_catkin
git clone https://github.com/ethz-asl/maplab_realsense.git
git clone  https://github.com/ethz-asl/cuckoo_time_translator.git
sudo apt-get install autoconf
sudo apt-get install python-catkin-tools  # 安装catkin编译工具
cd ..
catkin build maplab_realsense

在这里，我遇到一个问题，Failed << maplab_realsense ：cmake 如下图所示：

问题的原因是cmake中将Eigen3写为绝对路径user/include/eigen3/，而在上文中我们将Eigen3安装于user/local/include/中，也就是上文提及的安装Eigen3时可能会遇到的问题，执行命令行：sudo cp usr/local/include/eigen3/ /usr/include/ -R 复制一份到/usr/include/目录即可。

编译不报错后，将RealSense ZR300摄像头连接电脑，使用下面的命令运行maplab-realsense：

source devel/setup.bash
roslaunch maplab_realsense maplab_realsense.launch

如不报错则证明驱动已安装成功。

2）、用RealSense ZR300摄像头跑Vins-Mono

需要修改Vins-Mono的config/realsense/realsense_fisheye_config.yaml文件中IMU和Image的topic为/zr300_node/imu和/zr300_node/fisheye/image_raw，标定参数可以不修改（为了更高的精度，也可以使用camodocal进行标定），然后打开两个终端，分别运行Vins-Mono的estimator和rviz，以及启动相机驱动，命令行如下：

source devel/setup.bash
roslaunch maplab_realsense maplab_realsense.launch

source devel/setup.bash
roslaunch vins_estimator realsense_fisheye.launch

source devel/setup.bash
roslaunch vins_estimator vins_rviz.launch

如果无法正常运行，查看vins-mono-catkin_ws/src/VINS-Mono/config/realsense/realsense_fisheye_config.yaml 该yaml文件中的输出路径是否正确，新建一个自己的输出路径。同时将realsense_fisheye.launch第三行代码前面的Tab空格去掉。

正确运行结束后的界面如下图所示：

Tips. 注意，这里IMU的topic为imu_topic: "/zr300_node/imu"，而不是原来的image_raw，否则会出现无法对齐IMU造成只有相机特征点没有相机运动轨迹图像的bug…这里我踩坑了很久，调试起来有点费劲。

三、安装Intel RealSense D435驱动

英特尔RealSense深度摄像头D435捕捉最远距离可以达到10米，而且在户外阳光下也可以使用，支持输出1280x720分辨率的深度画面，视频传输方面可以达到90fps。
英文版官方手册：
https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages

1）、注册用户使用密钥

注册用户密钥，执行命令行如下：

sudo apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE

2）、加入更新源列表

更新资源列表，执行命令行如下：

sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main" -u
sudo rm -f /etc/apt/sources.list.d/realsense-public.list  
sudo apt-get update  #更新

3）、安装驱动

安装驱动与一些需要的工具包，执行命令行如下：

sudo apt-get install librealsense2-dkms)
sudo apt-get install librealsense2-utils #安装演示程序
sudo apt-get install librealsense2-dev
sudo apt-get install librealsense2-dbg #安装RGB-D相机开发需要的工具包

安装完毕后执行realsense-viewer，能正确显示相机拍摄的深度图像界面，如下图所示，则证明驱动已正确安装。

可太难了，各种重装重卸，人心都疯球了…好在结果是好的，继续加油呀。
艰难学习SLAM的乔木同学
记于2019年11月07日