使用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文件中
对应的内参和畸变参数如下:
使用Kalibr标定相机和IMU(ZED+px4)外参_第1张图片
根据我们获得的相机内参和畸变参数,创建一个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_modelcamera_model根据自己需要进行选择,Kalibr中Supported models中有介绍。
ZED相机是针孔模型,畸变参数是k1,k2,p1,p2,对应的distortion model是radial-tangential。

2.IMU噪声参数


注意:
我自己之前写过一篇关于IMU内参标定的文章,需要的小伙伴可以参看一下。
但是!!!!!我们其实用不上imu内参标定的数据,理由也在《IMU内参标定》这篇文章中介绍过了,想了解的同学,自己看看,欢迎评论区发表意见,如果想深入了解关于IMU噪声模型的同学,也可以看看我之前的文章。


虽然说imu内参标定的结果不重要,但是imu内参确实对cam-imu外参标定是有影响的,如何取值呢?是一个问题。
有多种选择的方法:

  1. 一种是参考公开数据集的做法,比如: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倍(倍数看效果,效果不行可以再放大一些)。
  2. 第二种做法,使用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]

使用Kalibr标定相机和IMU(ZED+px4)外参_第2张图片
结果分析:
指标的话主要看重投影误差,在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

使用Kalibr标定相机和IMU(ZED+px4)外参_第3张图片
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上可视化的轨迹,场景是在走廊和教室,走廊算是场景中特征点比较少的,所以我一开始是在教室里进行初始化的(教室里很多书,特征点比较丰富),我也尝试果在走廊初始化,但是最后飘了。使用Kalibr标定相机和IMU(ZED+px4)外参_第4张图片
先是在特征点丰富的教室启动,完成初始化后走到走廊上,然后从3楼走到2楼,最后回到3楼的教室。从终点的位置估计来看(回到原点,理论上位置估计应该是0,0,0),最后VIO是有一定成程度的drift的,这是不可避免的,属于正常现象。
总体来说,Kalibr标定的参数是可信的。

没有允许不许转载!!

你可能感兴趣的:(标定,自动驾驶,人工智能,算法)