orb-slam3编译运行指南-基于ROS和realsense在线

引言

• 测试范围：双目 + IMU；跑ROS节点

编译

• Pangolin: 直接下载v0.6版本 mkdir build && cd build && cmake .. && make && sudo make install
• ROS在Example_OLD 目录下，按照README有些目录需要修改，比如Section 7 export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM3/Examples/ROS
• 编译ROS版本需要rosdep
• AR相关编译会报错，可以网上搜下解决办法
• ubuntu20.04 sudo apt-get install python-is-python3

数据集准备

采集双目和IMU数据，打包成rosbag，topic如下（acc和gro不分开，与Kalibr一致）：

1. /cam0/image_raw
2. /cam1/image_raw
3. /imu0

配置文件

因为数据集是鱼眼相机，可以直接根据Example目录下的新版配置文件(ORB_SLAM3/Examples/Stereo-Inertial/TUM-VI.yaml)进行修改。填入相应的内外参即可。以下是参考配置文件。

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
File.version: "1.0"


Camera.type: "KannalaBrandt8"

# Left Camera calibration and distortion parameters (OpenCV)
Camera1.fx: 216.4328723494598
Camera1.fy: 216.4026510413344
Camera1.cx: 320.671183849444
Camera1.cy: 238.3502667084163

# Kannala-Brandt distortion parameters
Camera1.k1: 0.4519467752398101
Camera1.k2: -0.3706124232389851
Camera1.k3: 0.1332661721150505
Camera1.k4: -0.01808348569384299

# Right Camera calibration and distortion parameters (OpenCV)
Camera2.fx: 216.2815251082965
Camera2.fy: 216.3747998080645
Camera2.cx: 318.357478762681
Camera2.cy: 242.5734006983998

# Kannala-Brandt distortion parameters
Camera2.k1: 0.4526410908419655
Camera2.k2: -0.3683184275546367
Camera2.k3: 0.130528646291109
Camera2.k4: -0.01725595894881075
    
# Transformation matrix from right camera to left camera
Stereo.T_c1_c2: !!opencv-matrix
  rows: 4
  cols: 4
  dt: f
  data: [ -0.9999296290538163,-0.003745826792878408,0.01125636361952495,0.0006159035495833772,
            0.0074172142223413555,-0.9379195953607303,0.3467734383881556,0.11513974676823271,
            0.009258610774685204,0.34683252647351537,0.9378813766711231,-0.020766136698781194,
          0.0,0.0,0.0,1.0]

# Lapping area between images (We must calculate)
# Camera.lappingBegin: 0
# Camera.lappingEnd: 640 

# Camera2.lappingBegin: 0
# Camera2.lappingEnd: 640

Camera1.overlappingBegin: 0
Camera1.overlappingEnd: 640

Camera2.overlappingBegin: 0
Camera2.overlappingEnd: 640


# Camera resolution
Camera.width: 640
Camera.height: 480

# Camera frames per second 
Camera.fps: 30

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

# Image scale, it changes the image size to be processed (<1.0: reduce, >1.0: increase)
# Camera.imageScale: 1.0 # 0.7071 # 1/sqrt(2)

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

# stereo baseline times fx
# Camera.bf: 25.315641726801807


# Transformation from body-frame (imu) to left camera
IMU.T_b_c1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: f
   data: [0.005909396060214012, 0.9838846099385019, -0.178706892324738, -0.08579151809890839, 
          0.7609799475085763, -0.1203644912299456, -0.6375122812470367, -0.01303537873896517,
         -0.6487484863396971, -0.1322250489775607, -0.7494277402757612, -0.003535961294329092,
          0.0, 0.0, 0.0, 1.0]

# Do not insert KFs when recently lost
# InsertKFsWhenLost: 0

# IMU noise
IMU.NoiseGyro: 0.001313973415952309 # 0.000005148030141 # rad/s^0.5 
IMU.NoiseAcc: 0.04400312244363475 # 0.000066952452471 # m/s^1.5
IMU.GyroWalk: 2.062752092874584e-05 # rad/s^1.5
IMU.AccWalk: 1.544775027472382e-05 # m/s^2.5
IMU.Frequency: 1024.0

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

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

# 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 # 20
ORBextractor.minThFAST: 7 # 7

#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1.0
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2.0
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3.0
Viewer.ViewpointX: 0.0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -3.5
Viewer.ViewpointF: 500.0
# Viewer.imageViewScale: 2

ROS运行

1. 播放bag即可，如有必要remap一下camera相关的topic：

   rosbag play --pause dynamic.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw /imu0:=/imu

2. 运行 orb 视觉惯性节点：

   rosrun ORB_SLAM3 Stereo_Inertial ORB_SLAM3/Vocabulary/ORBvoc.txt ORB_SLAM3/Examples_old/Stereo-Inertial/config.yaml false

   如果找不到 ORB_SLAM3 节点，则可能需要 source 以下 ORB_SLAM3/build/devel/setup.bash

realsense D435i在线跑的问题

会出现 rs2::invalid_value_error‘ hwmon command 0x7d failed 的问题，应该是realsense sdk版本和ORB-SLAM中调用不兼容（realsensesdk 比较新）导致曝光配置报错。

注释掉Examples/Stereo/stereo_realsense_D435i.cc中的相关D435i配置代码即可：

   sensor.set_option(RS2_OPTION_AUTO_EXPOSURE_LIMIT,5000);
   get_sensor_option(sensor);
   // 不用RGB的话，把rgb的配置也注释掉
   sensor.set_option(RS2_OPTION_EXPOSURE,100.f);

存疑：
如果是跑stereo还会遇到yaml文件配置的问题报错，可能是因为stereo在ORB-SLAM2里面就有了，所以作者直接拉过来，没有作相应修改。只需要将运行指令中的yaml配置文件指向 Examples_old 文件夹里的D435i配置文件即可：

{
    "version": "0.2.0",
    "configurations": [
        {
            "name": "stereo_realsense_D435i",
            "type": "cppdbg",
            "request": "launch",
            "program": "${workspaceFolder}/Examples/Stereo/stereo_realsense_D435i",
            "args": ["${workspaceFolder}/Vocabulary/ORBvoc.txt",
                     "${workspaceFolder}/Examples_old/Stereo/RealSense_D435i.yaml"],
            "stopAtEntry": false,
            "cwd": "${workspaceFolder}",
            "environment": [],
            "externalConsole": false,
            "MIMode": "gdb",
            "miDebuggerPath": ""
        }
    ]
}