標定雙目相機內參和IMU內參，以及相機到IMU的外參

1 雙目相機標定

相機獲取圖像幀率設置爲4hz，低一點方便計算。然後通過rostopic獲取相機topic，再降低發佈頻率即可。

1.1 開啓相機

Run camera mynteye_wrapper_d

cd MYNT-EYE-D-SDK
source wrappers/ros/devel/setup.bash
roslaunch mynteye_wrapper_d display.launch

rostopic list /topic 查看topic
rostopic hz /topic 查看topic發佈頻率
rostopic info /topic 查看topic信息

1.2 重新發佈一個topic，降低幀率

 rosrun topic_tools throttle messages /mynteye/right/image_color 4.0 /img_right
 rosrun topic_tools throttle messages /mynteye/left/image_color 4.0 /img_left

1.3 將相機拍攝的照片流放在一個bag中

rosbag record -O ~/Documents/tmp/kalibr/calibr_bag/stereo_calibra.bag /img_left /img_right

1.4 進行標定

source ~/kalibr_workspace/devel/setup.bash

kalibr_calibrate_cameras \   
--target  	~/Documents/tmp/kalibr/calibr_board/april_6x6_50x50cmmy.yaml \    #標定板的參數文件，需要根據實際情況修改
--bag 	~/Documents/tmp/kalibr/calibr_bag/stereo_calibra.bag \   #剛纔打包的照片
--models 	pinhole-radtan pinhole-radtan \ #默認相機模型
--topics 	/img_left  /img_right  #包中照片的topic

2. IMU標定

利用港科大的這個方法
https://github.com/gaowenliang/imu_utils

2.1 安裝imu_utils

首先安裝imu_utils和code_utils。按照說明將其放置在一個catkin_space下然後catkin_make一下。

注意：

1. code_utils 依賴 ceres庫， 需要先全局安裝ceres庫，okvis等內部的ceres不行。
2. imu_utils 依賴 code_utils，故先catkin_make code_utils, 再將imu_utils放入編譯。
3. 編譯code_utils 報錯， fatal error: backward.hpp: No such file or directory
   解決辦法： 在code_utils下面找到sumpixel_test.cpp，修改#include “backward.hpp” 爲 #include “code_utils/backward.hpp” 即可編譯成功。

2.2 IMU標定

一種是直接開着IMU，從topic中讀數據，進行標定

1. imu_topic 
cd MYNT-EYE-D-SDK
source wrappers/ros/devel/setup.bash
roslaunch mynteye_wrapper_d display.launch

2. 運行標定程序
roslaunch imu_utils mynt_imu.launch  # 注意這裏的launch文件

一種是先從IMU讀到bag中，之後可以play，然後標定

1. 讀到bag中
rosbag record -O ~/Documents/tmp/kalibr/calibr_bag/imu.bag   /mynteye/imu/data_raw  /mynteye/imu/data_raw

2. play 
rosbag play -r 200 ~/Documents/tmp/kalibr/calibr_bag/imu.bag     #幀率200

3.  運行標定程序
roslaunch imu_utils mynt_imu.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= "30"/>   #標定時長，到時長才會停止
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

2.3 結果轉換

最後結果輸出到imu_util/data/mynteye_imu_param.yaml

%YAML:1.0
---
type: IMU
name: mynteye
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 1.9480242194900136e-03
      gyr_w: 2.2620050177053397e-05
   x-axis:
      gyr_n: 1.4943337644865377e-03
      gyr_w: 1.6979123891120051e-05
   y-axis:
      gyr_n: 2.8556505698921365e-03
      gyr_w: 3.8869840774243172e-05
   z-axis:
      gyr_n: 1.4940883240913672e-03
      gyr_w: 1.2011185865796957e-05
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.1870816125420644e-02
      acc_w: 3.1961985711386886e-04
   x-axis:
      acc_n: 2.1689606568187404e-02
      acc_w: 3.2513933860140864e-04
   y-axis:
      acc_n: 2.3148960398654196e-02
      acc_w: 3.3709366869493846e-04
   z-axis:
      acc_n: 2.0773881409420332e-02
      acc_w: 2.9662656404525953e-04

輸出 imu.yaml, 爲後續相機-IMU標定準備文件

rostopic: /mynteye/imu/data_raw
update_rate: 200.0 #Hz

accelerometer_noise_density: 2.1870816125420644e-02   #continous
accelerometer_random_walk: 3.1961985711386886e-04
gyroscope_noise_density: 1.9480242194900136e-03    #continous
gyroscope_random_walk: 2.2620050177053397e-05

3. 標定 Camera-IMU

3.1 採集 images & imu 數據

在 rosbag record /mynteye/imu/data_raw     \
               /mynteye/right/image_color   \
               /mynteye/left/image_color  \
           -O ~/Documents/tmp/kalibr/calibr_bag/img_imu.bag

3.2 標定camera && imu

source ~/kalibr_workspace/devel/setup.bash

kalibr_calibrate_imu_camera \
--target ~/Documents/tmp/kalibr/calibr_board/april_6x6_50x50cm.yaml \
--bag ~/Documents/tmp/kalibr/calibr_bag/img_imu.bag \
--cam ~/Documents/tmp/kalibr/calibr_bag/camchain-stereo_calibra.yaml \      #這裏需要修改下img的topic名，不是上文中提到的減頻後的topic，而是原來相機的topic， /mynteye/right/image_color
--imu ~/Documents/tmp/kalibr/calibr_bag/mynt_imu.yaml 

3.3 輸出 camchain-imucam.yaml

cam0:
  T_cam_imu:
  - [0.9934428889204068, 0.09176493006449765, 0.0681940178002217, 0.052903481394401354]
  - [0.08653757968000322, -0.9933440680628596, 0.07601848293315787, 0.09115974521093512]
  - [0.07471595382919362, -0.06961867604737666, -0.9947717156156015, -0.014714416225472633]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [-0.32726809165652687, 0.11886776669170422, 8.008858395321615e-05,
    0.00016947151975802357]
  distortion_model: radtan
  intrinsics: [709.0327171920417, 709.3502758333848, 608.5490306087884, 347.7484506944358]
  resolution: [1280, 720]
  rostopic: /mynteye/left/image_color
  timeshift_cam_imu: 0.00807023279474932
cam1:
  T_cam_imu:
  - [0.9935583432182118, 0.09152584793268394, 0.06681944164451824, -0.07539252923588326]
  - [0.08647981867396241, -0.9934367456616839, 0.07486436623147993, 0.0912633943754548]
  - [0.07323291325354019, -0.0686035824817173, -0.994952505844914, -0.014984371397893347]
  - [0.0, 0.0, 0.0, 1.0]
  T_cn_cnm1:
  - [0.9999990200252148, 0.00014298875888003304, 0.0013926603401210713, -0.12828850142112913]
  - [-0.000144590668980564, 0.9999993280445829, 0.0011502190744177136, 0.00012828457178189372]
  - [-0.0013924949359175218, -0.0011504193129222653, 0.9999983687452975, -9.143941399551913e-05]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [-0.3255128311208999, 0.11665971550085348, 0.00020637909503995344,
    0.00020627786588678956]
  distortion_model: radtan
  intrinsics: [707.9559596115516, 708.3682390592019, 622.0228830912382, 352.6979725159225]
  resolution: [1280, 720]
  rostopic: /mynteye/right/image_color
  timeshift_cam_imu: -0.007909975103142588

說明：

1. 推薦爲相機幀率20~40hz, IMU 200HZ.
2. 標定時可以選取rosbag中間的數據，去除首尾的抖動

參靠資料：