使用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外参标定是有影响的，如何取值呢？是一个问题。
  有多种选择的方法:
  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]
```
  结果分析：
  指标的话主要看重投影误差，在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标定的参数是可信的。
  
  没有允许不许转载！！