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 #include
    #include #include

    到此为止已经完成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
    

    VINS on RealSense D435i_第1张图片

    单目+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 on RealSense D435i_第2张图片

但拿出去走廊走一圈回到原来的地方(但未检测到回环)效果如下:

VINS on RealSense D435i_第3张图片
完整版视频

VINS-Fusion with D435i

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


总结

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

你可能感兴趣的:(计算机视觉,自动驾驶)