realsense双目bag包运行orbslam2

基于ROS

1、更改配置文件

通过rostopic info topic名称查看realsense内置参数，参数如下：
左目：

---
header: 
  seq: 1191
  stamp: 
    secs: 1596012389
    nsecs: 632475827
  frame_id: "camera_infra1_optical_frame"
height: 480
width: 640
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [384.3517150878906, 0.0, 320.7833251953125, 0.0, 384.3517150878906, 239.97474670410156, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [384.3517150878906, 0.0, 320.7833251953125, 0.0, 0.0, 384.3517150878906, 239.97474670410156, 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

右目：

---
header: 
  seq: 181
  stamp: 
    secs: 1596012489
    nsecs: 468810130
  frame_id: "camera_infra2_optical_frame"
height: 480
width: 640
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [384.3517150878906, 0.0, 320.7833251953125, 0.0, 384.3517150878906, 239.97474670410156, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [384.3517150878906, 0.0, 320.7833251953125, 0.0, 0.0, 384.3517150878906, 239.97474670410156, 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
---

然后按照Stereo/EuRoc.yaml的的格式更改相应的内容，更改后如下：

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 384.3517150878906
Camera.fy: 384.3517150878906
Camera.cx: 320.7833251953125
Camera.cy: 239.97474670410156

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

Camera.width: 480
Camera.height: 640

# Camera frames per second 
Camera.fps: 30.0

# stereo baseline times fx
Camera.bf: 47.90639384423901

# 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: 35

#--------------------------------------------------------------------------------------------
# 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: 480
LEFT.width: 640
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: [384.3517150878906, 0.0, 320.7833251953125, 0.0, 384.3517150878906, 239.97474670410156, 0.0, 0.0, 1.0]
LEFT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
LEFT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [384.3517150878906, 0.0, 320.7833251953125, 0.0,  0.0, 384.3517150878906, 239.97474670410156, 0.0,  0.0, 0.0, 1.0, 0.0]

RIGHT.height: 480
RIGHT.width: 640
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: [384.3517150878906, 0.0, 320.7833251953125, 0.0, 384.3517150878906, 239.97474670410156, 0.0, 0.0, 1.0]
RIGHT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
RIGHT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [384.3517150878906, 0.0, 320.7833251953125, 0.0, 0.0, 384.3517150878906, 239.97474670410156, 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

2、运行

通过查看bag包的topic可以得到以下信息，第一个是自己用realsense拍摄的数据，第二个是EuRoc的数据

[email protected]:~$ rostopic list
/camera/imu
/camera/infra1/image_rect_raw
/camera/infra2/image_rect_raw
/camera/left/image_raw
/camera/right/image_raw
/clock
/rosout
/rosout_agg
/tf_static
[email protected]:~$ rostopic list
/cam0/image_raw
/cam1/image_raw
/camera/left/image_raw
/camera/right/image_raw
/clock
/fcu/imu
/fcu/motor_speed
/imu0
/rosout
/rosout_agg
/vicon/firefly_sbx/firefly_sbx

(1)

roscore

(2)

rosrun ORB_SLAM2 Stereo Vocabulary/ORBvoc.txt Examples/Stereo/realsense-d435i-stereo.yaml true

(3)

rosbag play --pause 名称.bag /camera/infra1/image_rect_raw:=/camera/left/image_raw /camera/infra2/image_rect_raw:=/camera/right/image_raw