【无人机自主导航5 SLAM】Intel Realsense T265C双目相机实现ORB-SLAM3

发布于 2021-03-21  1296 次阅读


深度相机的SDK驱动安装、ROS功能包编译等内容在本站文章:
【无人机自主导航4-1 视觉传感器】通过ROS发布、订阅话题连接视觉相机已有说明,未配置环境的先进行驱动配置。

一、ORB-SLAM3依赖

(一)Pangolin

#安装依赖
sudo apt install libglew-dev
sudo apt install cmake
sudo apt install libpython2.7-dev
#可选依赖见github,我没装所以就不贴出来啦
#下载并编译
git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
mkdir build
cd build
cmake ..
cmake --build .
#最后不要忘了安装
sudo make install

(二)其他

根据Github仓库自行安装其他缺少的库。

二、ORB-SLAM3配置编译

cd ~/catkin_ws/src
git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git ORB_SLAM3
#编译
cd ORB_SLAM3
chmod +x build.sh
./build.sh

cd ..
catkin_make
source devel/setup.bash

三、编译ORB-SLAM3的ROS接口

chmod +x build_ros.sh
./build_ros.sh

3.1 修正相机 launch

根据下方路径自行修改launch文件,包括相机内外参数、启动的镜头、fps等,启动相机时,启动更改后相应的文件即可:

ganahe@ganahe-Nitro-AN515-51:~/catkin_ws/src/realsense-ros/realsense2_camera/launchpwd
/home/ganahe/catkin_ws/src/realsense-ros/realsense2_camera/launch
ganahe@ganahe-Nitro-AN515-51:~/catkin_ws/src/realsense-ros/realsense2_camera/launch tree
.
├── demo_pointcloud.launch
├── demo_t265.launch
├── includes
│   └── nodelet.launch.xml
├── opensource_tracking.launch
├── rs_aligned_depth.launch
├── rs_camera.launch
├── rs_d400_and_t265.launch
├── rs_d435_camera_with_model.launch
├── rs_from_file.launch
├── rs_multiple_devices.launch
├── rs_rgbd.launch
├── rs_rtabmap.launch
└── rs_t265.launch

本机的参数文件如下:

<!--
Important Notice: For wheeled robots, odometer input is a requirement for robust
and accurate tracking. The relevant APIs will be added to librealsense and
ROS/realsense in upcoming releases. Currently, the API is available in the
https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/libtm/include/TrackingDevice.h#L508-L515.
-->
<launch>
  <arg name="serial_no"           default=""/>
  <arg name="usb_port_id"         default=""/>
  <arg name="device_type"         default="t265"/>
  <arg name="json_file_path"      default=""/>
  <arg name="camera"              default="camera"/>
  <arg name="tf_prefix"           default="(arg camera)"/>

  <arg name="fisheye_width"       default="848"/>  <arg name="fisheye_height"      default="800"/>
  <arg name="enable_fisheye1"     default="true"/>
  <arg name="enable_fisheye2"     default="true"/>

  <arg name="fisheye_fps"         default="30"/>

  <arg name="gyro_fps"            default="200"/>
  <arg name="accel_fps"           default="62"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>
  <arg name="enable_pose"         default="true"/>

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

  <arg name="linear_accel_cov"      default="0.01"/>
  <arg name="initial_reset"         default="false"/>
  <arg name="unite_imu_method"      default=""/>

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

  <group ns="(arg camera)">
    <include file="(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="tf_prefix"                value="(arg tf_prefix)"/>
      <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_sync"              value="(arg enable_sync)"/>

      <arg name="fisheye_width"            value="(arg fisheye_width)"/>
      <arg name="fisheye_height"           value="(arg fisheye_height)"/>
      <arg name="enable_fisheye1"          value="(arg enable_fisheye1)"/>
      <arg name="enable_fisheye2"          value="(arg enable_fisheye2)"/>

      <arg name="fisheye_fps"              value="(arg fisheye_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="enable_pose"              value="(arg enable_pose)"/>

      <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="publish_odom_tf"          value="$(arg publish_odom_tf)"/>
    </include>
  </group>
</launch>

启动指令如下:

roscore

打开新的终端,启动T265C:

# 注意在此之前必须在 rs_t265.launch中设定相机的参数,比如是否开启相机
roslaunch realsense2_camera rs_t265.launch

3.2 修改Topic

打开相机节点实现通讯:

# 注意在此之前必须在 rs_t265.launch中设定相机的参数,比如是否开启相机
roslaunch realsense2_camera rs_t265.launch

查看相机镜头对应的话题信息:

rostopic list

根据显示的信息修改ros_stereo.cc文件,比如本文中的数据修正为:

    message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/camera/fisheye1/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/camera/fisheye2/image_raw", 1);

再次编译./build_ros.sh

3.3 再次编译功能包

随后执行:

cd ~/catkin_ws
catkin_make
source devel/setup.bash

四、运行ORB-SLAM3

三个终端

  • 首先执行:
    roscore
  • 运行相机:
roslaunch realsense2_camera rs_t265.launch
  • 运行SLAM:
rosrun ORB_SLAM3 Stereo ~/catkin_ws/src/ORB_SLAM3/Vocabulary/ORBvoc.txt

~/catkin_ws/src/ORB_SLAM3/Examples/ROS/ORB_SLAM3/T265.yaml  false

file

file

补充

根据大家的反馈,下面补充本文实验所用的T265.yaml文件,基本采用官方的改动较小:

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
Camera.type: "PinHole"

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 286.419189453125
Camera.fy: 286.384307861328
Camera.cx: 101.355010986328
Camera.cy: 102.183197021484

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0

Camera.width: 848
Camera.height: 800

# Camera frames per second 
Camera.fps: 30.0

# IR projector baseline times fx (aprox.)
Camera.bf: 40.0

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

# Close/Far threshold. Baseline times.
ThDepth: 40.0

# Deptmap values factor
DepthMapFactor: 1.0

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000

# ORB Extractor: Scale factor between levels in the scale pyramid   
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid  
ORBextractor.nLevels: 8

# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast           
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500