从零开始使用Realsense D435i运行VINS-Mono

从零开始使用Realsense D435i运行VINS-Mono
- （1）安装测试librealsense SDK 2.0
- （2）安装测试realsense
- （3）安装测试VINS-Mono
- （4）在D435i上运行VINS-Mono

从零开始使用Realsense D435i运行VINS-Mono

这次测试D435i过程还是比较顺利的，仅仅花了一个下午就搞定了，比之前测试ZR300和SR300要好多了，下面是我用D435i跑VINS-Mono的全过程（没啥技术含量），首先说下我的电脑配置，
我的电脑的配置：
CPU：Intel® Core™ i5-4590 CPU @ 3.30GHz × 4
内存：11.7 GiB
显卡：GeForce GTX 750 Ti/PCIe/SSE2
系统版本：Ubuntu16.04
系统内核：4.15.0-47-generic（命令：cat /proc/version）
ROS版本：Kinetic

（1）安装测试librealsense SDK 2.0

librealsense SDK相当于相机的驱动，SR300和ZR300的支持驱动是librealsense SDK 1.0，而D435i是librealsense SDK 2.0，貌似一台电脑上不能同时装两个SDK，所以要注意下。SDK的安装方式有两种，一个是从源码编译安装，一个是直接命令行安装，之前librealsense SDK 1.0我采用的是从源码编译安装，感觉比较麻烦，所以这次用尝试下了直接命令行安装，没想到出乎意料地顺利。
官方安装教程Intel® RealSense™ SDK 2.0，这里我把当回搬运工：

注册公匙

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

添加repositories（Ubuntu 16 LTS）

sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main" -u

安装library

sudo apt-get install librealsense2-dkms
sudo apt-get install librealsense2-utils

安装developer和debug packages

sudo apt-get install librealsense2-dev
sudo apt-get install librealsense2-dbg

测试SDK

realsense-viewer

运行完上面测试命令之后会弹出来一个看上去还满酷的SDK界面，打开三个按钮可以看到RGB、双目和IMU的结果就证明你的SDK装成功了。

（2）安装测试realsense

这个realsense指的是对应的realsense相机的ROS包，官方教程如下ROS Wrapper for Intel® RealSense™ Devices
这里就很简单啦，先进入你的ROS工作空间

cd ~/catkin_ws/src
git clone https://github.com/intel-ros/realsense.git
cd ..
catkin_make
rospack profile
source devel/setup.sh

然后运行（不加filters:=pointcloud好像没有点云的节点）

roslaunch realsense2_camera rs_camera.launch filters:=pointcloud

打开rviz，添加PointCloud2的mudule就可以看到点云，如下

（3）安装测试VINS-Mono

这一步先用数据集测试VINS-Mono
同样，进入你的ROS工作空间

cd ~/catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git
cd ..
catkin_make
source ~/catkin_ws/devel/setup.bash

这里编译可能需要安装ceres，安装ceres的步骤自行百度就好，编译源码的同时可以把数据集给下了，测试用的bag文件会比较方便，数据集地址

然后打开三个terminal，分别运行

roslaunch vins_estimator euroc.launch
roslaunch vins_estimator vins_rviz.launch
rosbag play MH_01_easy.bag

结果如下图

有趣的是，数据集里面可以看到测试人员开始的时候拿着摄像头前后左右晃了晃，这个应该是给IMU初始化用的。

（4）在D435i上运行VINS-Mono

前面都测试好之后就可以再D435i上运行VINS-Mone了，这里特地感谢下博客如何用Realsense D435i运行VINS-Mono等VIO算法获取IMU同步数据的作者Manii，让这个过程变得非常简单，如果大家需要对这个过程有一个更加详细的了解可以去参考那个博客，研究出来下面的步骤还是挺费时间的，这里我简单说下流程：

修改realsense包里的rs_camera.launch文件

第一处，修改unite_imu_method如下，这里是让IMU的角速度和加速度作为一个topic输出

 <arg name="unite_imu_method"      default="copy"/>

第二处，修改enable_sync参数为true，这里是开机相机和IMU的同步

  <arg name="enable_sync"           default="true"/>

修改VINS-Mono包里的realsense_color_config.yaml文件

第一处，修改订阅的topic

imu_topic: "/camera/imu"
image_topic: "/camera/color/image_raw"

第二处，修改相机内参，这里先再次打开运行realsesne包，然后可以通过如下命令获取相机内参

rostopic echo /camera/color/camera_info

第三处，IMU到相机的变换矩阵，这里我根据注释的提示修改成2

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 2   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.# 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.# 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
#If you choose 0 or 1, you should write down the following matrix.

第四处，IMU参数，这里我全部修改注释给的参数

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.2          # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.05         # gyroscope measurement noise standard deviation.     #0.05
acc_w: 0.02         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 4.0e-5       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.80       # gravity magnitude

第五处，是否需要在线估计同步时差，根据上述博主的建议这里选择不需要

#unsynchronization parameters
estimate_td: 0                      # online estimate time offset between camera and imu
td: 0.000                           # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

第六处，相机曝光改成全局曝光

#rolling shutter parameters
rolling_shutter: 0                      # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0               # unit: s. rolling shutter read out time per frame (from data sheet).

3. 打开摄像头，运行VINS-Mono

roslaunch realsense2_camera rs_camera.launch
roslaunch vins_estimator realsense_color.launch
roslaunch vins_estimator vins_rviz.launch

结果如下：

步骤到此就结束啦，五一假期闲着没事在实验室测试了下VINS，之后好好学习下原理读下源码，如果可以的话应用到自己项目中来。

此外，对SLAM算法感兴趣的同学可以看考我的博客SLAM算法总结——经典SLAM算法框架总结