学习Kalibr工具--Camera与IMU联合标定过程

上一节介绍了，用kalibr工具对camera进行标定的操作流程，在camera标定之好之后，进行camera与IMU进行联合标定的操作的学习，即求取相机和IMU 之间的转换关系(坐标系之间的相对位姿矩阵)；需要先知道相机的内外参数（单目的内外参数和双目的内外参数），用kalibr工具进行单目标定或者双目标定上一节已介绍。
这里以双目+IMU为例进行介绍相机和IMU 的联合标定，也叫多传感器融合；

一、camera - IMU相机标定介绍

参考链接：https://github.com/ethz-asl/kalibr/wiki/camera-imu-calibration

(一)标定的输出文件包括：
1.包含图像和IMU数据的 bag包; （ cam_imu.bag文件）：采集数相机图片和IMU数据
2.相机参数文件；（ camchain.yaml文件）：包含相机的内参和畸变参数的YAML文件（该文件包含相机内在和外在参数的校准以及IMU相对于相机的空间和时间校准参数。）
3.IMU参数文件；（ imu.yaml文件）：包含IMU的基本参数,噪音等
4.标定板参数； ( target.yaml文件)：标定板标准
(二)当上述文件都准备好的时候，执行命令行：

kalibr_calibrate_imu_camera –target xx/target.yaml –cam xx/camchain.yaml –imu xx/IMU.yaml –bag xx/xx.bag –bag-from-to 5 45

由于数据在采集过程中，在开始和结束处存在躁动和激励，所以仅使用 5到45秒之间的数据；

(三)输出结果：
report-imucam-%BAGNAME%.pdf: Report in PDF format. Contains all plots for documentation.
results-imucam-%BAGNAME%.txt: Result summary as a text file.
camchain-imucam-%BAGNAME%.yaml: 这个文件是在输入文件camchain.yaml基础上增加了标定后的cam-imu信息的结果文件。我们想要的T_cam_imu矩阵就在这里。

二、标定数据制作过程

1.数据采集： 数据采集的过程是手持需要标定的设备，同时激活camera和imu器件分别采集图像和imu数据，对着预先制作好的标定板，在激励充分（从不同的角度，旋转和平移，对标定板拍照，并且使得IMU的accel和gyro两个器件的三个轴都被激活）的情况下，同时保存图像和IMU的数据。采集到的数据应该如下所示（双目+IMU）：（假如是单目的话，只需要保留cam0和imu0.csv即可。）

+-- dataset-dir 
    +-- cam0 
    │   +-- 1385030208726607500.png 
    │   +--      ... 
    │   -- 1385030212176607500.png 
    +-- cam1 
    │   +-- 1385030208726607500.png 
    │   +--      ... 
    │   -- 1385030212176607500.png 
    -- imu0.csv

imu0.csv是一个文本文件，是IMU的数据，每一行就是一个gyro和accel在某一时刻的读数，如下所示: 19位时间戳（精确到ns），角速度，含重力的加速度

(timestamps=[ns], omega=[rad/s], alpha=[m/s^2])
timestamp,omega_x,omega_y,omega_z,alpha_x,alpha_y,alpha_z 
1385030208736607488,0.5,-0.2,-0.1,8.1,-1.9,-3.3 
 ... 
1386030208736607488,0.5,-0.1,-0.1,8.1,-1.9,-3.3

2.制作 .bag包： 将在连续时间内通过采集获得的拍摄标定板的图像和IMU 的数据文件（上述中的：cam0、cam1、imu0.csv），将三个数据利用kalibr工具转化为 .bag格式文件，因为kalibr的所有应用程序使用ROS包作为图像和IMU 的数据的源；bag文件制作参考: https://github.com/ethz-asl/kalibr/wiki/bag-format;命令行(创建ROS包)：
kalibr_bagcreater --folder dataset-dir/. --output-bag awsome.bag
dataset-dir/. 是数据输入路径：
其内文件结构应是这样:
dataset-dir/cam0/image_raw
dataset-dir/cam1/image_raw
dataset-dir/imu0
awsome.bag 是制作好的bag文件。输出默认在kalibr_bagcreater同目录下。
注意:a.在包含图片的文件夹的同一目录下需要一个:output空白文件夹,否则 .bag 文件无法生成
b.生成的.bag文件使用:rosbag info XX.bag 判定是否生成成功,如果只有4kb,则生成失败,而一般情况都是路径问题
c.calib可以标定单目+imu，bag内只放单目图像，修改带参运行中的参数即可。

3.相机参数文件： camchain.yaml
camchain.yaml 文件模板：

cam0:
  camera_model: pinhole
  intrinsics: [461.629, 460.152, 362.680, 246.049]
  distortion_model: radtan
  distortion_coeffs: [-0.27695497, 0.06712482, 0.00087538, 0.00011556]
  T_cam_imu:
  - [0.01779318, 0.99967549,-0.01822936, 0.07008565]
  - [-0.9998017, 0.01795239, 0.00860714,-0.01771023]
  - [0.00893160, 0.01807260, 0.99979678, 0.00399246]
  - [0.0, 0.0, 0.0, 1.0]
  timeshift_cam_imu: -8.121e-05
  rostopic: /cam0/image_raw
  resolution: [752, 480]
cam1:
  camera_model: omni
  intrinsics: [0.80065662, 833.006, 830.345, 373.850, 253.749]
  distortion_model: radtan
  distortion_coeffs: [-0.33518750, 0.13211436, 0.00055967, 0.00057686]
  T_cn_cnm1:
  - [ 0.99998854, 0.00216014, 0.00427195,-0.11003785]
  - [-0.00221074, 0.99992702, 0.01187697, 0.00045792]
  - [-0.00424598,-0.01188627, 0.99992034,-0.00064487]
  - [0.0, 0.0, 0.0, 1.0]
  T_cam_imu:
  - [ 0.01567142, 0.99978002,-0.01393948,-0.03997419]
  - [-0.99966203, 0.01595569, 0.02052137,-0.01735854]
  - [ 0.02073927, 0.01361317, 0.99969223, 0.00326019]
  - [0.0, 0.0, 0.0, 1.0]
  timeshift_cam_imu: -8.681e-05
  rostopic: /cam1/image_raw
  resolution: [752, 480]

camchain.yaml文件内容说明：

CAMERA_MODEL
camera_model（pinhole / omni）（针孔、全向）

intrinsics
包含给定投影类型的内部参数的向量。要素如下：
pinhole：[fu fv pu pv]
omni：[xi fu fv pu pv]
ds：[xi alpha fu fv pu pv]
eucm：[alpha beta fu fv pu pv]

distortion_model
distortion_model（radtan /equidistant）

distortion_coeffs
失真模型的参数向量

T_cn_cnm1
相机外在转换，总是相对于链中的最后一个相机
（例如cam1：T_cn_cnm1 = T_c1_c0，将cam0转换为cam1坐标）

T_cam_imu
IMU extrinsics：从IMU到相机坐标的转换（T_c_i）

timeshift_cam_imu
相机和IMU时间戳之间的时间间隔，以秒为单位（t_imu = t_cam + shift）

rostopic
摄像机图像流的主题

resolution
相机分辨率[width,height]

4.IMU参数文件：imu.yaml
imu.yaml 文件模板：(编写这个文件需要imu的手册。)

#Accelerometers
accelerometer_noise_density: 1.86e-03   #Noise density (continuous-time)
accelerometer_random_walk:   4.33e-04   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     1.87e-04   #Noise density (continuous-time)
gyroscope_random_walk:       2.66e-05   #Bias random walk

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

5.标定板文件：target.yaml
Kalibr支持三种标定板，分别是Aprilgrid、Checkerboard和Circlegrid。
参数比较简单：见https://github.com/ethz-asl/kalibr/wiki/calibration-targets
target.yaml文件模板：
1)对于april板.yaml文件格式如：

　　target_type: 'aprilgrid' 　 #gridtype
　　tagCols: 6	　　#number of apriltags
　　tagRows: 6	   #number of apriltags
　　tagSize: 0.088	　#size of apriltag, edge to edge [m]
　　tagSpacing: 0.3	 #ratio of space between tags to tagSize，实际上就是小黑块与大黑块的边长之比 example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25

2)对于checkboard板.yaml文件格式如：

扫描二维码关注公众号，回复： 4887912 查看本文章

　　target_type: 'checkerboard'     #gridtype
　　targetCols: 6	　#number of internal chessboard corners
　　targetRows: 7	　　#number of internal chessboard corners
　 rowSpacingMeters: 0.06	　　  #size of one chessboard square [m]
　　colSpacingMeters: 0.06	　 #size of one chessboard square [m]

3)对于circle板.yaml文件格式如：

　　target_type: 'circlegrid' 　   #gridtype
　　targetCols: 6	　　　　　 #number of circles (cols)
　　targetRows: 7	　　　　　#number of circles (rows)
　　spacingMeters: 0.02	　　#distance between circles [m]
　　asymmetricGrid: False	　　#use asymmetric grid (opencv) [bool]

三、Camera-IMU 联合标定 DEMO:

从官网 Downloads 下载数据集 IMU-camera calibration。
下载下来内容：4个文件
april_6x6.yaml 标定板参数
camchain.yaml 相机标定参数
imu_adis16448.yaml IMU标定参数
dynamic.bag 拍摄内容
cd到bag所在目录下使用kalibr_bagextractor 解码 dynamic.bag
解压命令：

extractor --image-topics /cam0/image_raw /cam1/image_raw --imu-topics /imu0 --output-folder dataset-dir --bag dynamic.bag

得到结果：cam0, cam1, imu0.csv
使用上述三个文件建立 .bag文件：

source /home/wcd/app/kalibr/devel/setup.bash
kalibr_bagcreater --folder dataset-dir/. --output-bag awsome.bag

运行标定程序：

kalibr_calibrate_imu_camera --target april_6x6.yaml --cam camchain.yaml --imu imu_adis16448.yaml --bag dynamic.bag --bag-from-to 5 45

运行后得到：
report-imucam-dynamic.pdf
results-imucam-dynamic.txt
包含：
1.相机的重投影误差，IMU的误差（加速度和陀螺仪）可以作为先验误差来影响数据融合的定权问题
2.相机和IMU各自的标定参数，2个.yaml文件给的
3.IMU与相机之间的相对位姿标定（正反旋转矩阵）cam1 to imu0也有

T_ci:  (imu0 to cam0): 
[[ 0.01680206  0.99985864 -0.00062288  0.06847911]
 [-0.99985871  0.01680236  0.00048881 -0.01472898]
 [ 0.00049921  0.00061458  0.99999969 -0.00376988]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam0 to imu0): 
[[ 0.01680206 -0.99985871  0.00049921 -0.0158756 ]
 [ 0.99985864  0.01680236  0.00061458 -0.06821963]
 [-0.00062288  0.00048881  0.99999969  0.00381973]
 [ 0.          0.          0.          1.        ]]

相机之间的位姿变换标定（基线baseline）：
Baseline (cam0 to cam1): 
[[ 0.99999877  0.00118911 -0.00102243 -0.1101676 ]
 [-0.00118838  0.99999904  0.00071255 -0.00032166]
 [ 0.00102327 -0.00071134  0.99999922  0.00012079]
 [ 0.          0.          0.          1.        ]]
baseline norm:  0.110168134052 [m]

使用标定命令行完成标定演示：

kalibr_calibrate_imu_camera –target xx/target.yaml –cam xx/camchain.yaml –imu xx/IMU.yaml –bag xx/xx.bag –bag-from-to 5 45

参考博客：
https://blog.csdn.net/wwchen61/article/details/78013962
https://blog.csdn.net/heyijia0327/article/details/83583360
https://blog.csdn.net/zhubaohua_bupt/article/details/80222321
https://blog.csdn.net/w492751512/article/details/81218618
https://blog.csdn.net/xinmei4275/article/details/84998506
http://www.mamicode.com/info-detail-1719627.html