VINS on RealSense D435i

关于Realsense D435i运行VINS系列

前言

在SLAM中，主要是以激光SLAM和视觉SLAM为主，激光雷达直接可以获取三维点云坐标信息，所以激光SLAM会比视觉SLAM稳定许多，但是由于激光雷达挺贵的，而相机成本低廉许多，所以视觉SLAM在工业应用中起到很多作用，视觉SLAM的性能也逐渐发展起来，有时精度甚至可以和激光SLAM可媲美。而在视觉SLAM中相机的FOV有限，不像三位机械旋转式激光雷达那样有水平范围为360度，所以当相机旋转比较大或运动较快或曝光度突然变化的时候（总结来说就是场景变化过大而导致特征匹配不上）很容易跟丢而IMU作为廉价的传感器测量线加速度和角速度（当然九轴IMU可以直接测量姿态角），噪声较大，短时间内可以提供比较准确的测量，而相机可以长时间运行，噪声较小，两种传感器原理上基本是一种互补的关系，且两种传感器的成本都可以做到比较低廉且重量很轻，不像激光雷达那么高昂且较重。所以现在的无人机定位方案基本首选视觉惯性里程计方案（Visual Inertial Odometry，VIO），而自动驾驶L4级别中定位方案基本采用激光惯性里程计（Lidar Inertial Odometry，LIO）。

在SLAM中，VIO具有完整理论紧耦合优化的理论方案为港科大的VINS，也是为数不多具有开创性将VO与IMU的优点结合的工作，精华同样在于VO与IMU紧耦合工作上。所以市面上很多商业方案如VR、科研无人机都是采用VINS方案，包括浙大FAST实验室的高飞博士进行运动规划全是采用VINS提供无人机位置姿态信息（师出同门）。在2017年开源VINS-Mono，支持单目相机，可以在线估计camera与IMU的外參，同样可以在iphone手机上运行（他们提供了VINS-Mobile）。后一年开源VINS-Fusion，支持双目相机+IMU。在2021年开源GVINS，将GNSS、VIO都融合到一个系统，即便在比较极端情况下，GVINS稳定性与精度也都由于前两者。

关于VINS-Mono的代码注释与详解：https://blog.csdn.net/qq_41839222/article/details/85793998

（该博文讲解非常详细），当然深蓝学院也有高翔和贺一家关于从零开始手写VIO的课程。

现在用同一个Realsense D435i分别设备演示一下运行VINS-Mono和VINS-Fusion，其中D435i包含有一个RGB成像模块、两个红外成像模块、一个深度成像模块和一个IMU。

D435i运行VINS-Mono

该程序需要在Ubuntu ROS下运行，参见ROS安装，原作者使用的系统是Ubuntu16.04，而我这里的电脑是Ubuntu20.04,有部分的包可能不一样。安装流程如下：

1. 安装ROS相关包

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

2. 安装ceres，ceres安装教程，其中现在版本是最新的2.1,VINS-Mono貌似不支持这个版本，换成1.14

3. 克隆编译

   mkdir -p VINS_ws/src
   cd VINS_ws/src
   git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git
   catkin build
   source devel/setup.bash
   

   其中源码测试都是基于OpenCV3完成的，但是Ubuntu20.04 ROS都是OpenCV4,所以需要改成对应的函数变量名：

   原函数名（OpenCV3）	新函数名(OpenCV4)
   CV_GRAY2RGB	cv::COLOR_GRAY2RGB
   CV_BGR2GRAY	cv::COLOR_BGR2GRAY
   CV_LOAD_IMAGE_GRAYSCALE	cv::IMREAD_GRAYSCALE
   CV_AA	cv::LINE_AA
   CV_CALIB_CB_ADAPTIVE_THRESH	cv::CALIB_CB_ADAPTIVE_THRESH
   CV_CALIB_CB_NORMALIZE_IMAGE	cv::CALIB_CB_NORMALIZE_IMAGE
   CV_CALIB_CB_FILTER_QUADS	cv::CALIB_CB_FILTER_QUADS
   CV_CALIB_CB_FAST_CHECK	cv::CALIB_CB_FAST_CHECK
   CV_RETR_CCOMP	cv::RETR_CCOMP
   CV_CHAIN_APPROX_SIMPLE	cv::CHAIN_APPROX_SIMPLE
   CV_CALIB_CB_FILTER_QUADS	cv::CALIB_CB_FILTER_QUADS
   CV_GRAY2BGR	cv::COLOR_GRAY2BGR
   CV_CALIB_CB_NORMALIZE_IMAGE	cv::CALIB_CB_NORMALIZE_IMAGE
   CV_TERMCRIT_EPS	cv::TermCriteria::EPS
   CV_TERMCRIT_ITER	cv::TermCriteria::COUNT
   CV_THRESH_BINARY_INV	cv::THRESH_BINARY_INV
   CV_CALIB_CB_FAST_CHECK	cv::CALIB_CB_FAST_CHECK
   CV_CALIB_CB_ADAPTIVE_THRESH	cv::CALIB_CB_ADAPTIVE_THRESH
   CV_THRESH_BINARY	cv::THRESH_BINARY
   CV_SHAPE_CROSS	cv::MORPH_CROSS
   CV_SHAPE_RECT	cv::MORPH_RECT
   CV_ADAPTIVE_THRESH_MEAN_C	cv::ADAPTIVE_THRESH_MEAN_C
   CV_FONT_HERSHEY_SIMPLEX	cv::FONT_HERSHEY_SIMPLEX
   #include <opencv/cv.h>	#include <opencv2/opencv.hpp>
   #include <opencv/highgui.h>	#include <opencv2/highgui/highgui.hpp>

   到此为止已经完成VINS-Mono的安装

4. 安装Realsense驱动

   首先到Intel RealSense SDK 2.0官网下载，如果是Linux Ubuntu，找到https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md直接命令行安装

   $ sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
   $ sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u
   $ sudo apt-get install librealsense2-dkms
   $ sudo apt-get install librealsense2-utils
   $ sudo apt-get install librealsense2-dev
   $ sudo apt-get install librealsense2-dbg
   

   如果相机通过usb连接电脑，采用命令realsense-viewer可以查看图像

   安装ROS驱动

   sudo apt-get install ros-noetic-realsense2-camera
   sudo apt-get install ros-noetic-realsense2-description
   mkdir -p ~/catkin_ws/src
   cd ~/catkin_ws/src/
   git clone https://github.com/IntelRealSense/realsense-ros.git
   cd ..
   catkin_make
   source devel/setup.bash
   roslaunch realsense2_camera rs_camera.launch  # 该命令正常打开相机
   

   此时需要修改一下rs_camera.launch，IMU开启，相机分辨率640×480，重命名为rs_camera_d435i.launch

   <launch>
     <arg name="serial_no"           default=""/>
     <arg name="usb_port_id"         default=""/>
     <arg name="device_type"         default=""/>
     <arg name="json_file_path"      default=""/>
     <arg name="camera"              default="camera"/>
     <arg name="tf_prefix"           default="$(arg camera)"/>
     <arg name="external_manager"    default="false"/>
     <arg name="manager"             default="realsense2_camera_manager"/>
   
     <arg name="fisheye_width"       default="640"/>
     <arg name="fisheye_height"      default="480"/>
     <arg name="enable_fisheye"      default="false"/>
   
     <arg name="depth_width"         default="640"/>
     <arg name="depth_height"        default="480"/>
     <arg name="enable_depth"        default="false"/>
   
     <arg name="infra_width"        default="640"/>
     <arg name="infra_height"       default="480"/>
     <arg name="enable_infra1"       default="true"/>
     <arg name="enable_infra2"       default="true"/>
   
     <arg name="color_width"         default="640"/>
     <arg name="color_height"        default="480"/>
     <arg name="enable_color"        default="true"/>
   
     <arg name="fisheye_fps"         default="30"/>
     <arg name="depth_fps"           default="30"/>
     <arg name="infra_fps"           default="30"/>
     <arg name="color_fps"           default="30"/>
     <arg name="gyro_fps"            default="200"/>
     <arg name="accel_fps"           default="250"/>
     <arg name="enable_gyro"         default="true"/>
     <arg name="enable_accel"        default="true"/>
   
     <arg name="enable_pointcloud"         default="false"/>
     <arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
     <arg name="pointcloud_texture_index"  default="0"/>
   
     <arg name="enable_sync"               default="true"/>
     <arg name="align_depth"               default="true"/>
   
     <arg name="publish_tf"                default="true"/>
     <arg name="tf_publish_rate"           default="0"/>
   
     <arg name="filters"                   default=""/>
     <arg name="clip_distance"             default="-2"/>
     <arg name="linear_accel_cov"          default="0.01"/>
     <arg name="initial_reset"             default="false"/>
     <arg name="unite_imu_method"          default="linear_interpolation"/>
     <arg name="topic_odom_in"             default="odom_in"/>
     <arg name="calib_odom_file"           default=""/>
     <arg name="publish_odom_tf"           default="true"/>
     <arg name="allow_no_texture_points"   default="false"/>
     <arg name="emitter_enable"   		default="false"/>
   
   
   
     <group ns="$(arg camera)">
       <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
         <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
         <arg name="external_manager"         value="$(arg external_manager)"/>
         <arg name="manager"                  value="$(arg manager)"/>
         <arg name="serial_no"                value="$(arg serial_no)"/>
         <arg name="usb_port_id"              value="$(arg usb_port_id)"/>
         <arg name="device_type"              value="$(arg device_type)"/>
         <arg name="json_file_path"           value="$(arg json_file_path)"/>
   
         <arg name="enable_pointcloud"        value="$(arg enable_pointcloud)"/>
         <arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/>
         <arg name="pointcloud_texture_index"  value="$(arg pointcloud_texture_index)"/>
         <arg name="enable_sync"              value="$(arg enable_sync)"/>
         <arg name="align_depth"              value="$(arg align_depth)"/>
   
         <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
         <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
         <arg name="enable_fisheye"           value="$(arg enable_fisheye)"/>
   
         <arg name="depth_width"              value="$(arg depth_width)"/>
         <arg name="depth_height"             value="$(arg depth_height)"/>
         <arg name="enable_depth"             value="$(arg enable_depth)"/>
   
         <arg name="color_width"              value="$(arg color_width)"/>
         <arg name="color_height"             value="$(arg color_height)"/>
         <arg name="enable_color"             value="$(arg enable_color)"/>
   
         <arg name="infra_width"              value="$(arg infra_width)"/>
         <arg name="infra_height"             value="$(arg infra_height)"/>
         <arg name="enable_infra1"            value="$(arg enable_infra1)"/>
         <arg name="enable_infra2"            value="$(arg enable_infra2)"/>
   
         <arg name="fisheye_fps"              value="$(arg fisheye_fps)"/>
         <arg name="depth_fps"                value="$(arg depth_fps)"/>
         <arg name="infra_fps"                value="$(arg infra_fps)"/>
         <arg name="color_fps"                value="$(arg color_fps)"/>
         <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
         <arg name="accel_fps"                value="$(arg accel_fps)"/>
         <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
         <arg name="enable_accel"             value="$(arg enable_accel)"/>
   
         <arg name="publish_tf"               value="$(arg publish_tf)"/>
         <arg name="tf_publish_rate"          value="$(arg tf_publish_rate)"/>
   
         <arg name="filters"                  value="$(arg filters)"/>
         <arg name="clip_distance"            value="$(arg clip_distance)"/>
         <arg name="linear_accel_cov"         value="$(arg linear_accel_cov)"/>
         <arg name="initial_reset"            value="$(arg initial_reset)"/>
         <arg name="unite_imu_method"         value="$(arg unite_imu_method)"/>
         <arg name="topic_odom_in"            value="$(arg topic_odom_in)"/>
         <arg name="calib_odom_file"          value="$(arg calib_odom_file)"/>
         <arg name="publish_odom_tf"          value="$(arg publish_odom_tf)"/>
         <arg name="allow_no_texture_points"  value="$(arg allow_no_texture_points)"/>
       </include>
     </group>
   </launch>
   

   确认图像和IMU话题数据都有

   rostopic echo /camera/imu 
   rostopic echo /camera/color/image_raw
   

5. 修改VINS-Mono的配置文件

   在VINS-Mono/config/realsense/realsense_color_config.yaml中修改对应的imu和camera topic，输出路径，其中相机内参数，厂家出厂的d435i内参有少许不一致，需要修改一下。不想标定就直接用rostopic echo /camera/color/camera_info，查看相机内参，不过最好自己标定一下，采用ROS提供的张正友标定法。imu与camera外參可以不管，imu白噪声和随机游走可以采用原始值，但最好也自己标定一下，参考网站。

6. 运行

   roslaunch realsense2_camera rs_camera_d435i.launch 
   roslaunch vins_estimator vins_rviz.launch
   roslaunch vins_estimator realsense_color.launch
   

   

   单目+IMU需要运动初始化，有时因IMU积分的原因而导致漂移严重。

---

D435i运行VINS-Fusion

这里采用D435i输出的红外双目图像，VINS-Fusion采用的是双目+IMU会进行估计，该方案不需要运动初始化，而VINS-Mono需要相机初始的时候运动初始化。安装基本和前面VINS-Mono的一致

cd ~/VINS-Fusion_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
cd ../
catkin build
source ~/VINS-Fusion_ws/devel/setup.bash

运行D435i时需要查看红外图像，同样是上述的rs_camera_d435i.launch

修改VINS-Fusionconfig/realsense_d435i/realsense_stereo_imu_config.yaml

其中estimate_extrinsic最好都设置成1,这样就会优化初值。

%YAML:1.0

#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; 
imu: 1         
num_of_cam: 2  

imu_topic: "/camera/imu"
image0_topic: "/camera/infra1/image_rect_raw"
image1_topic: "/camera/infra2/image_rect_raw"
output_path: "~/output/"

cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 480
   

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1   # 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.

body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 1, 0, 0, -0.00552,
           0, 1, 0, 0.0051,
           0, 0, 1, 0.01174,
           0, 0, 0, 1 ]

body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 1, 0, 0, 0.0446571,
           0, 1, 0, 0.0051,
           0, 0, 1, 0.01174,
           0, 0, 0, 1 ]

#Multiple thread support
multiple_thread: 1

#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 30            # min distance between two features 
freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
F_threshold: 1.0        # ransac threshold (pixel)
show_track: 1           # publish tracking image as topic
flow_back: 1            # perform forward and backward optical flow to improve feature tracking accuracy

#optimization parameters
max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
max_num_iterations: 8   # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.1          # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 0.01         # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 0.001         # accelerometer bias random work noise standard deviation.  #0.002
gyr_w: 0.0001       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.805         # gravity magnitude

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

#loop closure parameters
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "~/output/pose_graph/" # save and load path
save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0 

出行效果如下：

但拿出去走廊走一圈回到原来的地方（但未检测到回环）效果如下：

完整版视频

VINS-Fusion with D435i

效果貌似还可以，如果后面检测到回环效果会更好。

---

总结

两种方案，个人认为双目+IMU（VINS-Fusion运行D435i的红外双目图像+IMU）比单目+IMU（VINS-Mono运行D435的RGB图像+IMU）更好，双目图像本身两张图像就可以三角化深度估计了，而单目图像需要运动时候三角化进行深度估计（有尺度漂移问题），所以会更好一些。