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

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

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

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个数字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
---

问题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