使用ORB_SLAM3运行Realsense T265_orb-slam3带有t265的yaml文件

关于硬件

官网说明
使用说明
Realsense T265是一款跟踪相机，配有两个FOV为111.7 x 108.6的广角相机，并且带有IMU(BMI055)惯性测量单元。设备内部配有vpu处理器并嵌入了建图和定位算法(VIO)，利用其SDK或UI软件就可以运行对应的算法，实测效果非常好。

获取硬件参数

Realsense系列相机都已经进行了出厂标定并且嵌入到设备中，在安装完SDK并连接上相机之后，即可通过如下指令获取标定参数:

rs-enumerate-devices -c
1

结果(只列出部分需要的)

Intrinsic Parameters: ############  内参 ##############

 Intrinsic of "Fisheye 1" / 848x800 / {Y8}
  Width:      	848
  Height:     	800
  PPX:        	426.648498535156
  PPY:        	396.657501220703
  Fx:         	287.508697509766
  Fy:         	287.584197998047
  Distortion: 	Kannala Brandt4
  Coeffs:     	-0.0108608799055219  	0.0459878295660019  	-0.0432540811598301  	0.00781485624611378  	0  
  FOV (deg):  	111.7 x 108.6

 Intrinsic of "Fisheye 2" / 848x800 / {Y8}
  Width:      	848
  Height:     	800
  PPX:        	423.919891357422
  PPY:        	396.380493164062
  Fx:         	287.574493408203
  Fy:         	287.706298828125
  Distortion: 	Kannala Brandt4
  Coeffs:     	-0.012998660095036  	0.0508498586714268  	-0.0481258817017078  	0.00950818136334419  	0  
  FOV (deg):  	111.7 x 108.5

Motion Intrinsic Parameters: ############  IMU方差 ##############

Motion Intrinsic of "Gyro"	  MOTION_XYZ32F
Bias Variances: 	0.000000499999999  0.000000499999999  0.000000499999999  
Noise Variances: 	0.000005148030141  0.000005148030141  0.000005148030141  
Sensitivity : 
     1.003137       0.000000       0.000000       0.003866  
     0.000000       1.005335       0.000000      -0.000062  
     0.000000       0.000000       0.994192       0.000786  


Motion Intrinsic of "Accel"	  MOTION_XYZ32F
Bias Variances: 	0.000099999997474  0.000099999997474  0.000099999997474  
Noise Variances: 	0.000066952452471  0.000066952452471  0.000066952452471  
Sensitivity : 
     1.011488       0.000000       0.000000      -0.064494  
     0.000000       1.017876       0.000000      -0.002641  
     0.000000       0.000000       1.028140      -0.041595  

Extrinsic Parameters: ############  外参 ##############

Extrinsic from "Fisheye 2"	  To	  "Fisheye 1" : # 相机2到相机1的外参
 Rotation Matrix:
   0.999956         0.00755665       0.0055374     
  -0.00756489       0.99997          0.00146938    
  -0.00552613      -0.0015112        0.999984      

 Translation Vector: 0.0642259269952774  -5.97945181652904e-05  -0.000410528766224161  

Extrinsic from "Gyro"	  To	  "Fisheye 1" : # 陀螺仪到相机1的外参(也就是imu到相机1的外参)
 Rotation Matrix:
  -0.999935        -0.000988599      0.0113603     
   0.00107588      -0.99997          0.00767924    
   0.0113523        0.00769097       0.999906      

 Translation Vector: 0.0106999948620796  -3.63797880709171e-12  7.27595761418343e-11  

Extrinsic from "Accel"	  To	  "Fisheye 1" : # 加速度计到相机1的外参(与陀螺仪一样)
 Rotation Matrix:
  -0.999935        -0.000988599      0.0113603     
   0.00107588      -0.99997          0.00767924    
   0.0113523        0.00769097       0.999906      

 Translation Vector: 0.0106999948620796  -3.63797880709171e-12  7.27595761418343e-11  
 
Extrinsic from "Pose"	  To	  "Fisheye 1" : # tracking坐标系相对于相机1的外参
 Rotation Matrix:
   0.999935        -0.000988599     -0.0113603     
  -0.00107588      -0.99997         -0.00767924    
  -0.0113523        0.00769097      -0.999906      

 Translation Vector: 0.0321129634976387  -2.98972590826452e-05  -0.00020526438311208  
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76

上述参数坐标系定义如下:

修改配置参数

%YAML:1.0

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

# Camera resolution
Camera.width: 848
Camera.height: 800

# Left Camera calibration and distortion parameters (OpenCV)
Camera.fx: 287.508697509766
Camera.fy: 287.584197998047
Camera.cx: 426.648498535156
Camera.cy: 396.657501220703

# Kannala-Brandt distortion parameters
Camera.k1: -0.0108608799055219
Camera.k2: 0.0459878295660019
Camera.k3: -0.0432540811598301
Camera.k4: 0.00781485624611378

# Right Camera calibration and distortion parameters (OpenCV)
Camera2.fx: 287.574493408203
Camera2.fy: 287.706298828125
Camera2.cx: 423.919891357422
Camera2.cy: 396.380493164062

# Kannala-Brandt distortion parameters
Camera2.k1: -0.012998660095036
Camera2.k2: 0.0508498586714268
Camera2.k3: -0.0481258817017078
Camera2.k4: 0.00950818136334419
  
# fisheye2相机相对于fisheye1相机的外参  
# Transformation matrix from right camera to left camera
Tlr: !!opencv-matrix
  rows: 3
  cols: 4
  dt: f
  data: [ 0.999956,   0.00755665,   0.0055374,   0.0642259269952774,
            -0.00756489,   0.99997,   0.00146938,   -0.0000597945181652904,
            -0.00552613,  -0.0015112,   0.999984,   -0.000410528766224161]

# Lapping area between images
Camera.lappingBegin: 0
Camera.lappingEnd: 847

Camera2.lappingBegin: 0
Camera2.lappingEnd: 847

# Camera frames per second 
Camera.fps: 30.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
Camera.bf: 18.465512616769526

# imu相对于fisheye1相机的外参
# Transformation from body-frame (imu) to left camera
Tbc: !!opencv-matrix
   rows: 4
   cols: 4
   dt: f
   data: [-0.999935, -0.000988599, 0.0113603, 0.0106999948620796, 
          0.00107588, -0.99997, 0.00767924, 0.0,
          0.0113523, 0.00769097, 0.999906, 0.0,
          0.0, 0.0, 0.0, 1.0]

# 注意上一步获取的imu方差相关的参数需要开方
# IMU noise (Use those from VINS-mono)
IMU.NoiseGyro: 0.0022689270902785746 # 0.004 (VINS) # 0.00016 (TUM) # 0.00016    # rad/s^0.5 
IMU.NoiseAcc: 0.008182447828797932 # 0.04 (VINS) # 0.0028 (TUM) # 0.0028     # m/s^1.5
IMU.GyroWalk: 0.0007071067804794407 # 0.000022 (VINS and TUM) rad/s^1.5
IMU.AccWalk: 0.0099999998737 # 0.0004 (VINS) # 0.00086 # 0.00086    # m/s^2.5
IMU.Frequency: 200

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

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1500 # 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
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -3.5
Viewer.ViewpointF: 500

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115

编译运行

编译直接参照官方就行: https://github.com/UZ-SLAMLab/ORB_SLAM3
运行

roslaunch realsense2_camera rs_t265.launch #启动相机驱动
rosrun ORB_SLAM3 Stereo_Inertial Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/RST265.yaml false # 运行
1
2

结果