转载自:https://blog.csdn.net/qq_39266065/article/details/108275010
RealSense D435i下运行开源双目SLAM
秃头队长 2020-08-28 15:24:23 604 收藏 6
分类专栏: SLAM
版权
一丶ORB SLAM2
参考README文件
- 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
- 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<sensor_msgs::Image> left_sub(nh, "/camera/left/image_raw", 1);
//message_filters::Subscriber<sensor_msgs::Image> 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