用oak-D双目相机实现ORB-SLAM2双目实例

安装ROS

建议看最好看官方文档安装ROShttp://wiki.ros.org/Distributions更具自己的ubuntu系统版本安装合适的ROS版本我的是16.04+kinetic(不建议使用16.04版本版本太旧需要升级太多东西)

安装depthai

官方文档非常详细我就懒得赘述了https://docs.oakchina.cn/en/latest/pages/HighLevel/ros/ros_test_depthai_cpp_library.html

安装ORB—SLAM2

同理自己看官网文档https://github.com/raulmur/ORB_SLAM2这里建议多锻炼一下直接看官方文档的能力

运行流程

查看双目摄像头发布的话题用于修改orb-slam节点

运行stereo_node.launch实例

gagaga@ubuntu:~$ cd catkin_ws/
gagaga@ubuntu:~/catkin_ws$ source devel/setup.bash
gagaga@ubuntu:~/catkin_ws$ roslaunch depthai_examples stereo_node.launch

查看话题

gagaga@ubuntu:~/catkin_ws$ rostopic list
#我选择的是这两个
/stereo_publisher/left/image
/stereo_publisher/right/image

查看相机内外参

rostopic echo -n 1 /stereo_publisher/left/camera_info
rostopic echo -n 1 /stereo_publisher/right/camera_info

-n 1指消息只打印1次
参数如下
D:相机的畸变系数，在SLAM中一般用到前5位[k1,k2,p1,p2,k3]
K:相机内参矩阵
R：相机旋转矩阵
P:相机投影矩阵，矩阵中第4个数字42.741687850846795对应ORB-SLAM2配置文件中的bf参数

以下是我的相机内外参供大家参考

//左目
header: 
  seq: 0
  stamp: 
    secs: 1681488843
    nsecs: 696887012
  frame_id: "oak_left_camera_optical_frame"
height: 720
width: 1280
distortion_model: "rational_polynomial"
D: [0.8292564749717712, -0.07580796629190445, -0.0001298815623158589, -0.00036975208786316216, -0.011737951077520847, 1.1769423484802246, 0.09024432301521301, -0.045253634452819824]
K: [563.531494140625, 0.0, 639.3436889648438, 0.0, 563.531494140625, 356.0097961425781, 0.0, 0.0, 1.0]
R: [0.9999469518661499, -0.009474759921431541, 0.0040466636419296265, 0.00948441680520773, 0.9999521970748901, -0.002373715629801154, -0.0040239798836410046, 0.002411969704553485, 0.9999889731407166]
P: [566.3302001953125, 0.0, 634.0206909179688, 42.741687850846795, 0.0, 566.3302001953125, 352.3761291503906, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
//右目
header: 
  seq: 0
  stamp: 
    secs: 1681488882
    nsecs: 431191493
  frame_id: "oak_right_camera_optical_frame"
height: 720
width: 1280
distortion_model: "rational_polynomial"
D: [1.209637999534607, -0.026369793340563774, -0.0002514233929105103, -0.0004901287029497325, -0.016096703708171844, 1.5596858263015747, 0.2600933015346527, -0.053905777633190155]
K: [566.3302001953125, 0.0, 634.0206909179688, 0.0, 566.3302001953125, 352.3761291503906, 0.0, 0.0, 1.0]
R: [0.9998436570167542, -0.007231925614178181, 0.016135742887854576, 0.007193292956799269, 0.9999711513519287, 0.0024510070215910673, -0.0161530040204525, -0.0023345546796917915, 0.999866783618927]
P: [566.3302001953125, 0.0, 634.0206909179688, 0.0, 0.0, 566.3302001953125, 352.3761291503906, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

修改配置文件主要修改EuRoC.yaml和ros_stereo.cc这两个文件建议先备份不要直接在源文件上修改

EuRoC.yaml文件位置在**/home/gagaga/catkin_ws/src/ORB_SLAM2/Examples/Stereo**目录下这个文件就是配置相机的内外参将查看到的参数复制指定位置即可

以下是我的供大家参考（但不知道为什么实际运行还是有畸变）

LEFT.height: 720
LEFT.width: 1280
LEFT.D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data:[0.8292564749717712, -0.07580796629190445, -0.0001298815623158589, -0.00036975208786316216, -0.011737951077520847]
LEFT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [563.531494140625, 0.0, 639.3436889648438, 0.0, 563.531494140625, 356.0097961425781, 0.0, 0.0, 1.0]
LEFT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.9999469518661499, -0.009474759921431541, 0.0040466636419296265, 0.00948441680520773, 0.9999521970748901, -0.002373715629801154, -0.0040239798836410046, 0.002411969704553485, 0.9999889731407166]
LEFT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [566.3302001953125, 0.0, 634.0206909179688, 42.741687850846795, 0.0, 566.3302001953125, 352.3761291503906, 0.0, 0.0, 0.0, 1.0, 0.0]

RIGHT.height: 720
RIGHT.width: 1280
RIGHT.D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data:[1.209637999534607, -0.026369793340563774, -0.0002514233929105103, -0.0004901287029497325, -0.016096703708171844]
RIGHT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [566.3302001953125, 0.0, 634.0206909179688, 0.0, 566.3302001953125, 352.3761291503906, 0.0, 0.0, 1.0]
RIGHT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.9998436570167542, -0.007231925614178181, 0.016135742887854576, 0.007193292956799269, 0.9999711513519287, 0.0024510070215910673, -0.0161530040204525, -0.0023345546796917915, 0.999866783618927]
RIGHT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [566.3302001953125, 0.0, 634.0206909179688, 0.0, 0.0, 566.3302001953125, 352.3761291503906, 0.0, 0.0, 0.0, 1.0, 0.0]

ros_stereo.cc文件位置在/home/gagaga/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src 在这里修改订阅话题

    message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/stereo_publisher/left/image", 1);
    message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/stereo_publisher/right/image", 1);

修改完之后重新编译就ok了

gagaga@ubuntu:~/catkin_ws/src/ORB_SLAM2$ ./build_ros.sh

最后开始运行！

我这里写了个脚本在orb-slam根目录下以.sh文件保存直接运行

#!/bin/bash

gnome-terminal --command="bash -c 'roscore; exec bash'"

sleep 3

gnome-terminal --command="bash -c 'roslaunch depthai_examples stereo_node.launch; exec bash'"


gnome-terminal --command="bash -c 'rosrun ORB_SLAM2 Stereo ~/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt ~/catkin_ws/src/ORB_SLAM2/Examples/Stereo/EuRoC.yaml false; exec bash'"

参考文献

https://blog.csdn.net/guanjing_dream/article/details/127859240

https://blog.csdn.net/qq_42221707/article/details/122603455