MYNTEYE相机标定 + 运行ORB_SLAM3

小觅相机型号：标准入门版S1040-120/Mono
参考：小觅双目摄像头标准系列的标定

【注】使用Aprilgrid标定板标定出来的参数不是opencv格式的（即KDRP矩阵），应用到算法上效果很差，但是imu和外参应该是比较准的，所以我又使用ros中的camera
calibration包对双目相机重新标定了一下，获取的参数格式也对，所以我的标定方法算是走了弯路，所以仅作参考吧～
首先更新相机固件https://mynt-eye-s-sdk.readthedocs.io/zh_CN/latest/src/firmware/contents.html

安装kalibr

mkdir kalibr_ws
mkdir src && cd src
catkin_init_workspace
cd ..
catkin_make
cd src
git clone https://github.com/ethz-asl/kalibr.git
cd ..
catkin_make

标定双目相机

在一张A4纸上打印Aprilgrid标定板，april_6x6_80x80cm.yaml文件如下：

//april_6x6_80x80cm.yaml
target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.033           #size of apriltag, edge to edge [m]
tagSpacing: 0.3          #ratio of space between tags to tagSize

#启动相机
source ~/ORB_SLAM3/MYNT-EYE-S-SDK/wrappers/ros/devel/setup.bash
roslaunch mynt_eye_ros_wrapper display.launch
#降低频率
rosrun topic_tools throttle messages /mynteye/left/image_raw 4.0 /left
rosrun topic_tools throttle messages /mynteye/right/image_raw 4.0 /right
#录制rosbag
cd /home/stancy/桌面/calib_MYNT-S1040/camera
rosbag record -O stereo_calibra.bag /left /right
#标定
source ~/kalibr_ws/devel/setup.bash
kalibr_calibrate_cameras --bag /home/stancy/桌面/calib_MYNT-S1040/camera/stereo_calibra.bag --topics /left /right --models pinhole-radtan pinhole-radtan --target /home/stancy/桌面/calib_MYNT-S1040/camera/april_6x6_80x80cm.yaml

等待20分钟==

取出”camchain-homestancy桌面calib_MYNT-S1040camerastereo_calibra.yaml“

其内容如下：

cam0:

cam_overlaps:

  - 1
    camera_model: pinhole
      distortion_coeffs:
  - -0.14944250369264953
  - -0.05410442520942117
  - -0.00363933615716062
  - -0.026218074835143643
    distortion_model: radtan
      intrinsics:
  - 859.9451842109211
  - 856.3288439473047
  - 463.00538073117633
  - 206.02336727848478
    resolution:
  - 752
  - 480
    rostopic: /left
    cam1:
      T_cn_cnm1:
  - - 0.9997762502564821
    - 0.010970792179046462
    - 0.018085661227983725
    - -0.160273659636843
  - - -0.01114162814889753
    - 0.9998940055936425
    - 0.009372390308428192
    - 0.001007396023440314
  - - -0.0179809217027636
    - -0.009571796950729653
    - 0.9997925120533031
    - 0.0051492032398783106
  - - 0.0
    - 0.0
    - 0.0
    - 1.0
      cam_overlaps:
  - 0
    camera_model: pinhole
      distortion_coeffs:
  - -0.24559651684026967
  - 0.20282224750399167
  - 3.9333749218865324e-05
  - -0.021614731790322934
    distortion_model: radtan
      intrinsics:
  - 862.1916096440964
  - 863.2527279212379
  - 430.0539933130477
  - 176.45715566506482
    resolution:
  - 752
  - 480
    rostopic: /right

标定imu

cd ~/kalibr_ws/src
git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_make
cd src
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make

rosbag record /mynteye/imu/data_raw -O imu.bag

在imu_utils/launch目录下写一个launch文件mynt_imu.launch

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/mynteye/imu/data_raw"/>    #imu topic的名字
        <param name="imu_name" type="string" value= "mynteye"/>   
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "90"/>   #标定的时长
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

cd BAGFILE
roscore
source ~/kalibr_ws/devel/setup.bash
roslaunch imu_utils mynt_imu.launch
rosbag play -r 200 imu.bag

当bag包加速回放完毕后，执行launch的窗口仍然会显示wait for imu data.，等待一段时间计算，计算完毕后会显示计算结果。在~/kalibr_ws/src/imu_utils/data里面找到mynteye_imu_param.yaml

```yaml
%YAML:1.0
---

type: IMU
name: mynteye
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 5.1123174539901288e-05
      gyr_w: 1.3698284234926486e-07
   x-axis:
      gyr_n: 0.
      gyr_w: 0.
   y-axis:
      gyr_n: 1.5336952361970387e-04
      gyr_w: 4.1094852704779459e-07
   z-axis:
      gyr_n: 0.
      gyr_w: 0.
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 1.1587457033827809e-02
      acc_w: 2.4742117983289229e-04
   x-axis:
      acc_n: 1.0941771873337220e-02
      acc_w: 2.2826403342573475e-04
   y-axis:
      acc_n: 1.1057446643815428e-02
      acc_w: 1.6713499514406106e-04
   z-axis:
      acc_n: 1.2763152584330778e-02
      acc_w: 3.4686451092888113e-04

将上述数据带入kalibr模板，建一个新文件imu.yaml

#Accelerometers
accelerometer_noise_density: 1.1587457033827809e-02   #Noise density (continuous-time)
accelerometer_random_walk:   2.4742117983289229e-04   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     5.1123174539901288e-05   #Noise density (continuous-time)
gyroscope_random_walk:       1.3698284234926486e-07   #Bias random walk

rostopic:                    /mynteye/imu/data_raw    #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

双目+imu联合标定

需要4个文件：

• camchain-homestancy桌面calib_MYNT-S1040camerastereo_calibra.yaml（重命名为camerastereo_calibra.yaml）
• imu.yaml
• april_6x6_80x80cm.yaml
• stereo_imu_calibra.bag(重新采集)

source ~/ORB_SLAM3/MYNT-EYE-S-SDK/wrappers/ros/devel/setup.bash
roslaunch mynt_eye_ros_wrapper display.launch
rosrun topic_tools throttle messages /mynteye/left/image_raw 4.0 /left
rosrun topic_tools throttle messages /mynteye/right/image_raw 4.0 /right
rosbag record -O stereo_imu_calibra.bag /left /right /mynteye/imu/data_raw

source ~/kalibr_ws/devel/setup.bash
kalibr_calibrate_imu_camera --target  april_6x6_80x80cm.yaml --cam  camerastereo_calibra.yaml --imu imu.yaml --bag stereo_imu_calibra.bag

生成camchain-imucam-stereo_imu_calibra.yaml

cam0:
  T_cam_imu:

  - - 0.009878019596075815
    - 0.9991226213683129
    - -0.04069904420218814
    - 0.020602331412489114
  - - -0.9998113207973138
    - 0.0105492382071114
    - 0.016310621655285084
    - 0.04503115992241504
  - - 0.01672565497646583
    - 0.04053024849864287
    - 0.9990383132904603
    - -0.13700090065618323
  - - 0.0
    - 0.0
    - 0.0
    - 1.0
      cam_overlaps:
  - 1
    camera_model: pinhole
      distortion_coeffs:
  - -0.14944250369264953
  - -0.05410442520942117
  - -0.00363933615716062
  - -0.026218074835143643
    distortion_model: radtan
      intrinsics:
  - 859.9451842109211
  - 856.3288439473047
  - 463.00538073117633
  - 206.02336727848478
    resolution:
  - 752
  - 480
    rostopic: /left
      timeshift_cam_imu: 0.0005714234146826612
    cam1:
      T_cam_imu:
  - - -0.0007904182972802709
    - 0.9997478177818875
    - -0.022442728873047716
    - -0.1416596623708295
  - - -0.9996586442444786
    - -0.00020386736734068212
    - 0.02612572344237367
    - 0.04452021346563752
  - - 0.026114559659434888
    - 0.02245571816821583
    - 0.9994067092507151
    - -0.1326247494079876
  - - 0.0
    - 0.0
    - 0.0
    - 1.0
      T_cn_cnm1:
  - - 0.9997762502564819
    - 0.010970792179046465
    - 0.018085661227983725
    - -0.160273659636843
  - - -0.011141628148897534
    - 0.9998940055936423
    - 0.009372390308428192
    - 0.001007396023440314
  - - -0.0179809217027636
    - -0.009571796950729653
    - 0.9997925120533029
    - 0.0051492032398783106
  - - 0.0
    - 0.0
    - 0.0
    - 1.0
      cam_overlaps:
  - 0
    camera_model: pinhole
      distortion_coeffs:
  - -0.24559651684026967
  - 0.20282224750399167
  - 3.9333749218865324e-05
  - -0.021614731790322934
    distortion_model: radtan
      intrinsics:
  - 862.1916096440964
  - 863.2527279212379
  - 430.0539933130477
  - 176.45715566506482
    resolution:
  - 752
  - 480
    rostopic: /right
      timeshift_cam_imu: 0.00010450989303887465

使用camera calibration包重新标定双目相机

sudo apt-get install ros-melodic-camera-calibration
#启动相机
source ~/ORB_SLAM3/MYNT-EYE-S-SDK/wrappers/ros/devel/setup.bash
roslaunch mynt_eye_ros_wrapper mynteye.launch 
#棋盘格：6×9
rosrun camera_calibration cameracalibrator.py --size 5x8 --square 0.030 right:=/mynteye/right/image_raw left:=/mynteye/left/image_raw left_camera:=/mynteye/left right_camera:=/mynteye/right --no-service-check

移动棋盘格直到calibration图标变亮，点击calibration，此时界面变暗，计算完毕后终端显示出结果，然后点save，终端中显示出结果保存路径，打开解压得到calibrationdata目录，找到如下两个文件，就是opencv格式的相机内参。

left.yaml：

image_width: 752
image_height: 480
camera_name: narrow_stereo/left
camera_matrix:
  rows: 3
  cols: 3
  data: [1039.343568, 0.000000, 407.735091, 0.000000, 1037.955319, 326.514267, 0.000000, 0.000000, 1.000000]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [-0.222691, -0.158158, 0.003039, -0.005363, 0.000000]
rectification_matrix:
  rows: 3
  cols: 3
  data: [0.999545, 0.005162, -0.029710, -0.005136, 0.999986, 0.000956, 0.029714, -0.000803, 0.999558]
projection_matrix:
  rows: 3
  cols: 4
  data: [1068.746018, 0.000000, 438.915680, 0.000000, 0.000000, 1068.746018, 320.769787, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

right.yaml：

image_width: 752
image_height: 480
camera_name: narrow_stereo/right
camera_matrix:
  rows: 3
  cols: 3
  data: [1045.737076, 0.000000, 411.947833, 0.000000, 1044.236543, 305.736036, 0.000000, 0.000000, 1.000000]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [-0.252067, -0.084802, -0.000398, -0.004209, 0.000000]
rectification_matrix:
  rows: 3
  cols: 3
  data: [0.999795, 0.008692, -0.018293, -0.008708, 0.999962, -0.000800, 0.018286, 0.000959, 0.999832]
projection_matrix:
  rows: 3
  cols: 4
  data: [1068.746018, 0.000000, 438.915680, -132.502739, 0.000000, 1068.746018, 320.769787, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

修改yaml文件

参考：双目相机标定和orbslam2双目参数详解https://blog.csdn.net/weixin_37918890/article/details/95626004
小觅相机标准版参数http://www.myntai.com/mynteye/standard/specs

%YAML:1.0

#--------------------------------------------------------------------------------------------

# Camera Parameters. Adjust them!

#--------------------------------------------------------------------------------------------
Camera.type: "PinHole"

# Camera calibration and distortion parameters (OpenCV) (equal for both cameras after stereo rectification)

# 应该与LEFT.P一致

Camera.fx: 1068.746018
Camera.fy: 1068.746018
Camera.cx: 438.915680
Camera.cy: 320.769787

# 代码中已做畸变纠正，故均为0

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

# 相机的图像大小

Camera.width: 752
Camera.height: 480

# Camera frames per second 

# 相机的帧率，单位：m

Camera.fps: 30.0

# stereo baseline times fx

# 相机的基线（120mm）×fx

Camera.bf: 128.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: 35.0 # 35

# Transformation from camera 0 to body-frame (imu)

# 相机与imu联合标定结果

Tbc: !!opencv-matrix
   rows: 4
   cols: 4
   dt: f
   data: [0.009878019596075815, 0.9991226213683129, -0.04069904420218814, 0.020602331412489114,
         -0.9998113207973138, 0.0105492382071114, 0.016310621655285084, 0.04503115992241504,
         0.01672565497646583, 0.04053024849864287, 0.9990383132904603, -0.13700090065618323,
         0.0, 0.0, 0.0, 1.0]

# IMU noise

# imu标定结果

IMU.NoiseGyro: 5.112317453990129e-05 # 1.6968e-04 
IMU.NoiseAcc: 0.01158745703382781 # 2.0000e-3
IMU.GyroWalk: 1.3698284234926486e-07 
IMU.AccWalk: 0.0002474211798328923 # 3.0000e-3
IMU.Frequency: 200

#--------------------------------------------------------------------------------------------

# 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: 752
LEFT.D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [-0.222691, -0.158158, 0.003039, -0.005363, 0.000000]
LEFT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [1039.343568, 0.000000, 407.735091, 0.000000, 1037.955319, 326.514267, 0.000000, 0.000000, 1.000000]
LEFT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.999545, 0.005162, -0.029710, -0.005136, 0.999986, 0.000956, 0.029714, -0.000803, 0.999558]
LEFT.Rf:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: f
   data: [0.999545, 0.005162, -0.029710, -0.005136, 0.999986, 0.000956, 0.029714, -0.000803, 0.999558]
LEFT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [1068.746018, 0.000000, 438.915680, 0.000000, 0.000000, 1068.746018, 320.769787, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

RIGHT.height: 480
RIGHT.width: 752
RIGHT.D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [-0.252067, -0.084802, -0.000398, -0.004209, 0.000000]
RIGHT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [1045.737076, 0.000000, 411.947833, 0.000000, 1044.236543, 305.736036, 0.000000, 0.000000, 1.000000]
RIGHT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.999795, 0.008692, -0.018293, -0.008708, 0.999962, -0.000800, 0.018286, 0.000959, 0.999832]
RIGHT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [1068.746018, 0.000000, 438.915680, -132.502739, 0.000000, 1068.746018, 320.769787, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

#--------------------------------------------------------------------------------------------

# ORB Parameters

#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image

ORBextractor.nFeatures: 500

# 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