提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档
使用Kalibr标定相机和IMU（ZED+px4）外参
前言
本文仅使用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#Hzaccelerometer_noise_density:0.1#VINS默认 0.2accelerometer_random_walk:0.03#0.05gyroscope_noise_density:0.02#0.02gyroscope_random_walk:0.00004#4e-5
3.target(标定板参数)
标定板的yaml文件没什么好说的，参考Kalibr的download：
我用的是花几百块买的标定板，是一个7x5的板子，yaml文件如下：
#example for aprilgridtarget_type:'aprilgrid'#gridtypetagCols:7#number of apriltagstagRows:5#number of apriltagstagSize: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 long51131500000000
这里订阅的消息有两个，图像的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 585
参数说明：
–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.039062780.05493455][0.013914450.03882229-0.99914925-0.0962123][0.99975091-0.017978570.01322427 -0.04775604][0. 0. 0. 1. ]]
T_ic: (cam0 to imu0):
[[-0.01744988 0.013914450.999750910.05004149][-0.99908438 0.03882229-0.017978570.05776085][-0.03906278 -0.999149250.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:1num_of_cam:1imu_topic:"/mavros/imu/data"image0_topic:"/zed/zed_node/left_raw/image_raw_gray"output_path:"~/output/"cam0_calib:"left.yaml"image_width:672image_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-matrixrows:4cols:4dt: 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 supportmultiple_thread:1#feature traker paprametersmax_cnt:150# max feature number in feature trackingmin_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 topicflow_back:1# perform forward and backward optical flow to improve feature tracking accuracy#optimization parametersmax_solver_time:0.04# max solver itration time (ms), to guarantee real timemax_num_iterations:8# max solver itrations, to guarantee real timekeyframe_parallax:10.0# keyframe selection threshold (pixel)#imu parameters The more accurate parameters you provide, the better performanceacc_n:0.2# accelerometer measurement noise standard deviation. #0.2 0.04gyr_n:0.05# gyroscope measurement noise standard deviation. #0.05 0.004acc_w:0.02# accelerometer bias random work noise standard deviation. #0.02gyr_w:4.0e-5# gyroscope bias random work noise standard deviation. #4.0e-5g_norm:9.805# gravity magnitude#unsynchronization parametersestimate_td:1# online estimate time offset between camera and imutd:0.0# initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)#loop closure parametersload_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 pathsave_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标定的参数是可信的。
没有允许不许转载！！
版权归原作者 猫星星 所有， 如有侵权，请联系我们删除。