RealSense D435i下运行开源双目SLAM

一丶ORB SLAM2

参考README文件

1. Add the path including Examples/ROS/ORB_SLAM2 to the ROS_PACKAGE_PATH environment variable.
   Open .bashrc file and add at the end the following line. Replace PATH by the folder where you cloned ORB_SLAM2:

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM2/Examples/ROS

关于环境变量问题可以参考：https://www.jianshu.com/p/ee8bde3b84c4
在设置环境变量时，一直设置不成功，我是逐个把ROS和ORB_SLAM2共3个环境变量加进去的，然后就可以运行.sh文件了。

export ROS_PACKAGE_PATH=/home/like/catkin_ws/src:/opt/ros/kinetic/share:
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/like/ORB_SLAM2/Examples/ROS

roscd ORB_SLAM2

3. Execute build_ros.sh script:

  chmod +x build.sh
  ./build.sh

chmod +x build_ros.sh
./build_ros.sh

由于我运行的是双目，因此以双目为例，在运行ORB SLAM2之前，需要讲ORB SLAM2订阅的节点话题与相机发布的节点话题保持一致，可以在相机的.launch文件中修改，也可以修改ROS下的ORB SLAM2中src文件。
由于对.launch的语法不太熟悉，因此修改源代码中的话题名称。

//message_filters::Subscriber left_sub(nh, "/camera/left/image_raw", 1);
//message_filters::Subscriber right_sub(nh, "/camera/right/image_raw", 1);
//add realsense （line：112）
message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/camera/infra1/image_rect_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/camera/infra2/image_rect_raw", 1);

如上，将订阅的双目节点名称改成相机发布的节点名，打开相机的.launch文件后，可以通过rostopic list查看相机的节点名，如果没有的话，需要修改rs_camera.launch文件。
查看以下两个参数是否为false，改为true。

  <arg name="enable_infra1"       default="true"/>
  <arg name="enable_infra2"       default="true"/>

改完之后需要重新执行./build_ros.sh，如果是在.launch文件中修改则不需要。

首先运行相机节点
此时需要先关闭结构光，可参考博客Realsense D435i关闭IR结构光

roslaunch realsense2_camera rs_camera.launch 

然后运行ORB SLAM2

  rosrun ORB_SLAM2 Stereo PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION

以我的为例：

rosrun ORB_SLAM2 Stereo /home/like/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/like/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml false

其中因为只是粗略的尝试一下，并未对相机标定，因此ONLINE_RECTIFICATION设置为false，成功运行！

二丶ORB SLAM3

ORB SLAM3在ORB SLAM2上加了imu，更加适合D435i，编译过程同ORB SLAM2。
依旧以双目IMU为例，修改文件ros_stereo_inertial.cc
修改接受相机发布的topic

// Maximum delay, 5 seconds
//  ros::Subscriber sub_imu = n.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb); 
//  ros::Subscriber sub_img_left = n.subscribe("/camera/left/image_raw", 100, &ImageGrabber::GrabImageLeft,&igb);
//  ros::Subscriber sub_img_right = n.subscribe("/camera/right/image_raw", 100, &ImageGrabber::GrabImageRight,&igb);
  ros::Subscriber sub_imu = n.subscribe("/camera/imu", 1000, &ImuGrabber::GrabImu, &imugb); 
  ros::Subscriber sub_img_left = n.subscribe("/camera/infra1/image_rect_raw", 100, &ImageGrabber::GrabImageLeft,&igb);
  ros::Subscriber sub_img_right = n.subscribe("/camera/infra2/image_rect_raw", 100, &ImageGrabber::GrabImageRight,&igb);

重新编译./build_ros.sh
注意此时的.yaml文件与纯双目并不一样，否则会出现opencv异常。

rosrun ORB_SLAM3 Stereo_Inertial /home/like/ORB_SLAM3/Vocabulary/ORBvoc.txt /home/like/ORB_SLAM3/Examples/Stereo-Inertial/TUM_512.yaml false

成功运行！

三丶VINS-Fusion

下载编译VINS：https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git

修改相机.launch文件，打开IMU话题。

  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>

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

同步时间戳

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

修改VINS订阅的topic话题，在VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml中

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

分别打开节点：

roslaunch realsense2_camera rs_camera_vins.launch 
roslaunch vins vins_rviz.launch 
rosrun vins vins_node /home/like/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml

成功运行，由于未对相机和IMU标定，漂移非常严重！
参考博客：从零开始使用Realsense D435i运行VINS-Mono