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

以双目+IMU为例进行介绍相机和IMU 的联合标定，也叫多传感器融合；

一、相机标定过程

录制含有标定板图像的bag包，注意需要不断移动标定板。
运行Kalibr工具箱中多相机标定节点程序。

kalibr_calibrate_cameras --bag [filename.bag] --topics [TOPIC_0 ... TOPIC_N] --models [MODEL_0 ... MODEL_N] --target [target.yaml]

我们使用的最终命令

使用的bag文件XXX.bag，里面存储了含有标定板的图像
使用标定板规格apriltag_6_6.yaml，可多种选择
相机的畸变模型pinhole-radtan，其他模型还有omni-radtan
bag文件中相机发布图像的topic话题/XXX/image_raw

kalibr_calibrate_cameras --target april_6x6.yml --bag images.bag --bag-from-to 5 20 --models pinhole-radtan --topics /cam0/image_raw

二、camera - IMU相机标定介绍

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

(一) 标定的输入文件包括：

包含图像和IMU数据的bag包(cam_imu.bag文件): 采集数相机图片和IMU数据
相机参数文件(camchain.yaml文件): 包含相机的内参和畸变参数的YAML文件(该文件包含相机内在和外在参数的校准以及IMU相对于相机的空间和时间校准参数)
IMU参数文件(imu.yaml文件): 包含IMU的基本参数,噪音等
标定板参数(target.yaml文件): 标定板标准

(二) 当上述文件都准备好的时候，执行命令行：

rosrun kalibr kalibr_calibrate_imu_camera --target /data/april_6x6_80x80cm.yml --cam /data/cam_april-camchain.yml --imu /data/imu_adis16448.yml --bag /data/imu_april.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矩阵就在这里。

三、标定数据制作过程

(一) 数据采集：

数据采集的过程是手持需要标定的设备，同时激活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

(二) 制作 .bag包：

将在连续时间内通过采集获得的拍摄标定板的图像和IMU的数据文件(上述中的：cam0、cam1、imu0.csv)，将三个数据利用kalibr工具转化为 .bag格式文件，因为kalibr的所有应用程序使用ROS包作为图像和IMU的数据的源；bag文件制作参考: https://github.com/ethz-asl/kalibr/wiki/bag-format;

命令行(创建ROS包)：

rosrun kalibr kalibr_bagcreater --folder /. --output-bag Euroc.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内只放单目图像，修改带参运行中的参数即可。

最后包制作完后验证一下，想要的信息是否都加进包里面了: rosbag info XXX.bag

(三) 相机参数文件：

camchain.yaml
camchain.yaml 文件模板：

cam0:
camera_model: pinhole
intrinsics: [461.629, 460.152, 362.680, 246.049]
distortion_model: 
radtandistortion_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: 
radtandistortion_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]

(四) 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 walkrostopic:                    /imu0      #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

(五) 标定板文件：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 apriltagstag
Rows: 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文件格式如：

target_type: 'checkerboard'     #gridtypetarget
Cols: 6	　#number of internal chessboard cornerstarget
Rows: 7	　　#number of internal chessboard cornersrow
SpacingMeters: 0.06	　　  #size of one chessboard square [m]
colSpacingMeters: 0.06	　 #size of one chessboard square [m]

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

target_type: 'circlegrid' 　   #gridtypetarget
Cols: 6	　　　　　 #number of circles (cols)target
Rows: 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
解压命令：

rosrun kalibr kalibr_bagextractor --image-topics /cam0/image_raw /cam1/image_raw --imu-topic /imu0 --output-folder dataset-dir --bag imu_april.bag

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

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

运行标定程序：

rosrun kalibr kalibr_calibrate_imu_camera --target april_6x6.yml --cam hand_data_3-camchain.yaml --imu imu_adis16448.yml --bag hand_data_3.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]

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

rosrun kalibr kalibr_calibrate_imu_camera --target april_6x6.yml  --cam hand_data-camchain.yaml --imu imu_adis16448.yml --bag hand_data.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

https://www.ngui.cc/el/38435.html