使用Kalibr标定相机和IMU（ZED+px4）外参
提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档
使用Kalibr标定相机和IMU（ZED+px4）外参
- 前言
- 一、Kalibr介绍
- 二、CAM-IMU外参标定
-
- 1.相机内参
- 2.IMU噪声参数
- 3.target(标定板参数)
- 4.bag文件录制
- 5.kalibr: cam-imu联合标定
- 三、运行单目+imu的VINS-Fusion
前言
本文仅使用kalibr中cam-imu的联合标定，关于camera内参默认已经知道了。主要会总结一些标定上的小经验，有不对的地方，希望网友可以指出。文章最后将使用标定的cam-imu外参在VINS-Fusion中进行验证
一、Kalibr介绍
Kalibr是ZTH大佬们开源的一个标定工具箱，可以进行相机内参标定，cam-imu外参标定、多imu、多相机标定等。在很多数据集中，都使用Kalibr来标定cam-imu的外参矩阵，具有很高的可信度，写论文，做实验，都用得上。
本文不涉及Kalibr的安装，各位可以根据官网自己安装，下面开始介绍cam-imu的标定细节和经验。
本文使用的设备是ZED相机和px4里内置的imu，进行左目相机和imu的外参标定。
二、CAM-IMU外参标定
标定一共需要3个yaml文件，分别是：
相机内参的zed_left_raw.yaml，imu噪声参数的px4_imu.yaml，标定板对应的april_7x5.yaml文件
下面会在相机内参、imu噪声、target部分分别介绍yaml文件中的内容。
注意这里的文件名可以根据自己习惯命名，不是固定的。
1.相机内参
关于相机内参的标定，用的比较多的有matlab的标定工具箱，Kalibr本身也支持多相机的内参标定，有需要可以看看Multiple camera calibration 部分。
关于ZED相机的内参，使用出厂标定的数据就好了，如果安装ZED的SDK时使用的是默认的安装路径，可以在/usr/local/zed/settings下面找到一个SN****.conf文件，根据你设置的相机分辨率去找对应的相机内参和畸变参数，我这里使用的是VGA模式，conf文件中
对应的内参和畸变参数如下：
根据我们获得的相机内参和畸变参数，创建一个zed_left_raw.yaml文件(文件名自己定)，内容如下：
cam0:
cam_overlaps: []
camera_model: pinhole #相机模型，kalibr也支持鱼眼模型
distortion_coeffs: [-0.173778, 0.0266126, 0.0010566, -0.000836547] #相机畸参数
distortion_model: radtan #畸变模型
intrinsics: [349.46, 349.46, 346.59, 181.41225]
resolution: [672, 376]
rostopic: /zed/zed_node/left_raw/image_raw_gray
注意一下，distortion_model和camera_model根据自己需要进行选择，Kalibr中Supported models中有介绍。
ZED相机是针孔模型，畸变参数是k1,k2,p1,p2，对应的distortion model是radial-tangential。
2.IMU噪声参数
注意：
我自己之前写过一篇关于IMU内参标定的文章，需要的小伙伴可以参看一下。
但是！！！！！我们其实用不上imu内参标定的数据，理由也在《IMU内参标定》这篇文章中介绍过了，想了解的同学，自己看看，欢迎评论区发表意见，如果想深入了解关于IMU噪声模型的同学，也可以看看我之前的文章。
虽然说imu内参标定的结果不重要，但是imu内参确实对cam-imu外参标定是有影响的，如何取值呢？是一个问题。
有多种选择的方法:
- 一种是参考公开数据集的做法，比如：EuRoC数据集（毕竟是他们自己家采的数据集），Kalibr在download中给了Euroc数据集在使用kalibr标定时用到的imu_adis16448.yaml文件，里面有标定时用的imu的噪声参数（连续噪声模型）。他们用的IMU型号是adls16448，在很多imu内参标定的工具里一般会用这款imu作为示例，比如mintar版本的imu_util中，下面可以对比一下Kalibr中用的imu噪声参数和imu内参标定得到的噪声参数的差别：
可以看到，Kalibr中用的imu噪声参数比imu内参标定得到的噪声参数要大10-15倍左右，理由看我之前的文章。所以我们的一种做法是使用静置imu标定的内参，然后放大10-15倍（倍数看效果，效果不行可以再放大一些）。 - 第二种做法，使用VINS种imu噪声的默认参数，他们给的这个默认参数还是蛮好用的，或者在他们默认参数的基础上做一个微调。本文用的就是在VINS给的默认参数基础上做了点微调，具体的px4_imu.yaml文件的内容如下：
rostopic: /mavros/imu/data
update_rate: 195 #Hz
accelerometer_noise_density: 0.1 #VINS默认 0.2
accelerometer_random_walk: 0.03 #0.05
gyroscope_noise_density: 0.02 #0.02
gyroscope_random_walk: 0.00004 #4e-5
3.target(标定板参数)
标定板的yaml文件没什么好说的，参考Kalibr的download：
我用的是花几百块买的标定板，是一个7x5的板子，yaml文件如下：
#example for aprilgrid
target_type: 'aprilgrid' #gridtype
tagCols: 7 #number of apriltags
tagRows: 5 #number of apriltags
tagSize: 0.03 #size of apriltag, edge to edge [m]
tagSpacing: 0.2 #ratio of space between tags to tagSize a=3cm b=0.6cm
自己用A4纸打印也行，但是要把值量准了，我两种都试过，其实差别不是很大，但是用标定板得到的重投影误差更小
4.bag文件录制
这里用的是px4内置的imu，通过mavros发布imu的数据，但是默认的频率只有30hz，需要执行下面指令修改imu频率到200hz
rosrun mavros mavcmd long 511 31 5000 0 0 0 0 0
这里订阅的消息有两个，图像的topic和imu的topic：
rosbag record /mavros/data/raw zed/zed_node/left_raw/image_raw_gray
录制的时候注意要尽量运动的平滑一些，速度不要太快，如果相机得到的图像质量太差的话可能会提取不到特征点，也不可太慢，要充分激励imu，采集数据的方法在kalibr的github主页上有一个油管上的视频，B站也有人搬过来了，参考视频来录制。bag的时间长短到不是很有影响40-120s左右吧，主要影响一个计算时间，如果不赶时间到没关系。
5.kalibr: cam-imu联合标定
//source 一下kalibr的工作空间
rosrun kalibr kalibr_calibrate_imu_camera --imu /YOUR_FOLDER/px4_imu.yaml --cam /YOUR_FOLDER/zed-left-raw.yaml --target /YOUR_FOLDER/april_7x5.yaml --bag /YOUR_FOLDER/xxx.bag --show-extraction --bag-from-to 5 85
参数说明：
–show-extraction可以显示图像，可以看到提取的特征点。
–bag-from-to 5 85 是指从bag的第5s读到第85s，就是把一头一尾截掉一部分，因为在开始和结束可能有一些剧烈的抖动，会导致错误，你要是整个过程都很平稳，不截掉也行。
标定结果：
Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0): mean 0.615012956118, median 0.536132055556, std: 0.392602996383
Gyroscope error (imu0): mean 0.253306360314, median 0.222449089297, std: 0.165059194168
Accelerometer error (imu0): mean 0.461800885522, median 0.367774639131, std: 0.37771279501
Residuals
----------------------------
Reprojection error (cam0) [px]: mean 0.615012956118, median 0.536132055556, std: 0.392602996383
Gyroscope error (imu0) [rad/s]: mean 0.0176861541002, median 0.0155316624023, std: 0.011524631044
Accelerometer error (imu0) [m/s^2]: mean 0.128973968357, median 0.102713866857, std: 0.105489442742
Transformation (cam0):
-----------------------
T_ci: (imu0 to cam0):
[[-0.01744988 -0.99908438 -0.03906278 0.05493455]
[ 0.01391445 0.03882229 -0.99914925 -0.0962123 ]
[ 0.99975091 -0.01797857 0.01322427 -0.04775604]
[ 0. 0. 0. 1. ]]
T_ic: (cam0 to imu0):
[[-0.01744988 0.01391445 0.99975091 0.05004149]
[-0.99908438 0.03882229 -0.01797857 0.05776085]
[-0.03906278 -0.99914925 0.01322427 -0.09335301]
[ 0. 0. 0. 1. ]]
timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
0.0060200053287647135
Gravity vector in target coords: [m/s^2]
[ 0.08307977 -9.67015417 -1.62777118]
Calibration configuration
=========================
cam0
-----
Camera model: pinhole
Focal length: [349.46, 349.46]
Principal point: [346.59, 181.41225]
Distortion model: radtan
Distortion coefficients: [-0.173778, 0.0266126, 0.0010566, -0.000836547]
Type: aprilgrid
Tags:
Rows: 5
Cols: 7
Size: 0.03 [m]
Spacing 0.006 [m]
IMU configuration
=================
IMU0:
----------------------------
Model: calibrated
Update rate: 195
Accelerometer:
Noise density: 0.02
Noise density (discrete): 0.279284800875
Random walk: 0.0001
Gyroscope:
Noise density: 0.005
Noise density (discrete): 0.0698212002188
Random walk: 1e-06
T_i_b
[[1. 0. 0. 0.]
[0. 1. 0. 0.]
[0. 0. 1. 0.]
[0. 0. 0. 1.]]
time offset with respect to IMU0: 0.0 [s]
结果分析：
指标的话主要看重投影误差，在1个pixle（具体也没有硬指标，别太大了，1个pixle以上误差还挺大的应该）以下应该就还可以吧，这个我们可以参考EuRoC数据集的标定结果（原始数据在download里有），下面是在我自己电脑上用kalibr标定的结果：
Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0): mean 0.418843991251, median 0.356960966203, std: 0.283661622772
Gyroscope error (imu0): mean 0.112183412747, median 0.0980577964024, std: 0.0659293249494
Accelerometer error (imu0): mean 0.348599885514, median 0.313983260978, std: 0.202874498275
Residuals
----------------------------
Reprojection error (cam0) [px]: mean 0.418843991251, median 0.356960966203, std: 0.283661622772
Gyroscope error (imu0) [rad/s]: mean 0.00793256518902, median 0.00693373327843, std: 0.00466190727508
Accelerometer error (imu0) [m/s^2]: mean 0.0492994685936, median 0.0444039386034, std: 0.028690786692
EuRoC数据集里面标定用的是一个比较大的专业标定板，IMU也是比较好的，效果应该要好一些。可以看到他们的重投影误差在0.4左右，我标定的在0.6左右，这个效果还是算不错的，在VINS中也可以直接作为外参使用。
三、运行单目+imu的VINS-Fusion
VINS的安装参考官网的介绍。
这里主要介绍下config里的yaml文件应该怎么修改，下面是我用的yaml文件：
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 1
num_of_cam: 1
imu_topic: "/mavros/imu/data"
image0_topic: "/zed/zed_node/left_raw/image_raw_gray"
output_path: "~/output/"
cam0_calib: "left.yaml"
image_width: 672
image_height: 376
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [-0.01711133, 0.01180156, 0.99978394, 0.03898149,
-0.99917567, 0.03661386, -0.01753311, 0.05820492,
-0.03681287, -0.9992598, 0.01116532, -0.0870125,
0, 0, 0, 1]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 15 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.2 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.05 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.02 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 4.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.805 # gravity magnitude
#unsynchronization parameters
estimate_td: 1 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#loop closure parameters
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "~/output/pose_graph/" # save and load path
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
需要该的点：
1.外参矩阵，对应Kalibr标定结果中的T_ic
2.imu parameters这块，推荐一开始使用vins的默认参数，如果可行，可以做微调，让精度提高些，要是有条件，可以用Vicon采个数据集进行调整。不能直接用imu标定的参数。
3. estimate_td: 1 :这个参数是开启时延迟估计的，开启后精度会提升一些，这个成果是发在了2018 IROS上，Online Temporal Calibration for Monocular Visual-Inertial Systems
4. estimate_extrinsic: 1 :Kalibr标定的外参是可以直接作为真值使用的，设为0是作为真值，设为1是作为初值，在后端中会把外参也作为一个优化变量去优化，最好设为1吧，我设成0，只有在初始化阶段比较好的时候才能运行，设为1鲁棒性好点。
5.在运行mono+imu的模式的时候，初始化阶段不能运动的太快，但是要充分激励IMU，另外初始化阶段尽量在特征点丰富的场景中运行，不然 容易导致初始化的一些变量不准，影响后续的优化结果。
6.相机的参数文件没什么好说的，可以参考vins里的euroc数据集的config文件写。
下面放一个rviz上可视化的轨迹，场景是在走廊和教室，走廊算是场景中特征点比较少的，所以我一开始是在教室里进行初始化的（教室里很多书，特征点比较丰富），我也尝试果在走廊初始化，但是最后飘了。
先是在特征点丰富的教室启动，完成初始化后走到走廊上，然后从3楼走到2楼，最后回到3楼的教室。从终点的位置估计来看（回到原点，理论上位置估计应该是0，0，0），最后VIO是有一定成程度的drift的，这是不可避免的，属于正常现象。
总体来说，Kalibr标定的参数是可信的。
没有允许不许转载！！