使用OAK-D相机跑ORB-SLAM3算法遇到的问题总结

问题1：相机内外参数的获取

1. 在启动相机驱动后使用命令行查看

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

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

header: 
  seq: 0
  stamp: 
    secs: 1668563459
    nsecs: 222877835
  frame_id: "oak_left_camera_optical_frame"
height: 720
width: 1280
distortion_model: "rational_polynomial"
D: [6.860053539276123, 8.262731552124023, -0.0022835570853203535, -0.0021555486600846052, -33.7902717590332, 6.535381317138672, 9.392735481262207, -34.76184844970703]
K: [800.8717041015625, 0.0, 630.91015625, 0.0, 800.8717041015625, 313.4556884765625, 0.0, 0.0, 1.0]
R: [0.9999741911888123, 8.463647827738896e-05, 0.00718728918582201, -7.144125265767798e-05, 0.9999983310699463, -0.0018361477414146066, -0.007187432609498501, 0.0018355868523940444, 0.9999724626541138]
P: [799.4793090820312, 0.0, 629.921875, 59.60097983474698, 0.0, 799.4793090820312, 289.2037658691406, 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
---
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

问题2:在启动相机并运行ORB-SLAM3程序后无法成功初始化

每帧图像的特征点检测正常，但是在pangolin的图像帧Frame窗口特征点很不稳定，而且一直无法初始化成功。
解决：
ORB-SLAM3针对双目和双单目组合两种情况提供了两种配置文件，因为之前调试的原因选择了双单目组合对应的配置文件格式，因此出现错误

1. 首先要选择双目对应的配置文件格式，即带有Stereo Rectification参数的配置文件，将问题1获得的参数填到对应位置即可
2. 将Stereo Rectification中左右目相机的畸变系数全部写成0.0，因为相机发布出来的图像已经是去畸变的，不需要再去畸变
   最后附上完整的参数

%YAML:1.0

#--------------------------------------------------------------------------------------------
# System config
#--------------------------------------------------------------------------------------------

# When the variables are commented, the system doesn't load a previous session or not store the current one

# If the LoadFile doesn't exist, the system give a message and create a new Atlas from scratch
#System.LoadAtlasFromFile: "Session_MH01_MH02_MH03_Stereo60_Pseudo"

# The store file is created from the current session, if a file with the same name exists it is deleted
#System.SaveAtlasToFile: "Session_MH01_MH02_MH03_Stereo60_Pseudo"

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

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 799.4793090820312
Camera.fy: 799.4793090820312
Camera.cx: 629.921875
Camera.cy: 289.2037658691406

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

Camera.bFishEye: 0

Camera.width: 1280
Camera.height: 720

# Camera frames per second 
Camera.fps: 30.0

# stereo baseline times fx
Camera.bf: 59.60097983474698

# 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: 60.0 #35.0 #60.0

#--------------------------------------------------------------------------------------------
# Stereo Rectification. Only if you need to pre-rectify the images.
# Camera.fx, .fy, etc must be the same as in LEFT.P
#--------------------------------------------------------------------------------------------
LEFT.height: 720
LEFT.width: 1280
LEFT.D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [0.0, 0.0, 0.0, 0.0, 0.0]
LEFT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [800.8717041015625, 0.0, 630.91015625, 0.0, 800.8717041015625, 313.4556884765625, 0.0, 0.0, 1.0]
LEFT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.9999741911888123, 8.463647827738896e-05, 0.00718728918582201, -7.144125265767798e-05, 0.9999983310699463, -0.0018361477414146066, -0.007187432609498501, 0.0018355868523940444, 0.9999724626541138]
LEFT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [799.4793090820312, 0.0, 629.921875, 59.60097983474698, 0.0, 799.4793090820312, 289.2037658691406, 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: [0.0, 0.0, 0.0, 0.0, 0.0]
RIGHT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [799.4793090820312, 0.0, 629.921875, 0.0, 799.4793090820312, 289.2037658691406, 0.0, 0.0, 1.0]
RIGHT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.9999744892120361, 0.00568584306165576, 0.004325373098254204, -0.005693774204701185, 0.9999821186065674, 0.0018235768657177687, -0.004314926918596029, -0.0018481580773368478, 0.9999889731407166]
RIGHT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [799.4793090820312, 0.0, 629.921875, 0.0, 0.0, 799.4793090820312, 289.2037658691406, 0.0, 0.0, 0.0, 1.0, 0.0]

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

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

# 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
Viewer.imageViewScale: 0.5
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
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131