ZED-Mini 标定完全指南（应该是最详细的吧）

标定 ZED-Mini 相机主要为了跑 VINS-Fusion 以及后期的联合标定相关事宜

双目相机标定

出厂标定数据

关于ZED相机的内参，使用出厂标定的数据就好了，如果安装ZED的SDK时使用的是默认的安装路径，可以在/usr/local/zed/settings下面找到一个SN****.conf文件，根据设置的相机分辨率去找对应的相机内参和畸变参数，这里使用的是VGA模式

[LEFT_CAM_2K]
fx=1400.91
fy=1400.91
cx=1136.24
cy=666.392
k1=-0.172408
k2=0.0261692
p1=0.000139195
p2=-0.00020522
k3=0

[RIGHT_CAM_2K]
fx=1399.79
fy=1399.79
cx=1078.94
cy=581.798
k1=-0.17124
k2=0.0254432
p1=0.000633285
p2=-0.000151551
k3=0

[LEFT_CAM_FHD]
fx=1400.91
fy=1400.91
cx=992.24
cy=585.392
k1=-0.172408
k2=0.0261692
p1=0.000139195
p2=-0.00020522
k3=0

[RIGHT_CAM_FHD]
fx=1399.79
fy=1399.79
cx=934.94
cy=500.798
k1=-0.17124
k2=0.0254432
p1=0.000633285
p2=-0.000151551
k3=0

[LEFT_CAM_HD]
fx=700.455
fy=700.455
cx=654.62
cy=381.196
k1=-0.172408
k2=0.0261692
p1=0.000139195
p2=-0.00020522
k3=0

[RIGHT_CAM_HD]
fx=699.895
fy=699.895
cx=625.97
cy=338.899
k1=-0.17124
k2=0.0254432
p1=0.000633285
p2=-0.000151551
k3=0

[LEFT_CAM_VGA]
fx=350.2275
fy=350.2275
cx=342.81
cy=198.098
k1=-0.172408
k2=0.0261692
p1=0.000139195
p2=-0.00020522
k3=0

[RIGHT_CAM_VGA]
fx=349.9475
fy=349.9475
cx=328.485
cy=176.9495
k1=-0.17124
k2=0.0254432
p1=0.000633285
p2=-0.000151551
k3=0

[STEREO]
Baseline=62.9989
TY=-0.000663536
TZ=0.00110222
CV_2K=0.00866806
CV_FHD=0.00866806
CV_HD=0.00866806
CV_VGA=0.00866806
RX_2K=0.000842401
RX_FHD=0.000842401
RX_HD=0.000842401
RX_VGA=0.000842401
RZ_2K=0.00115049
RZ_FHD=0.00115049
RZ_HD=0.00115049
RZ_VGA=0.00115049

可以根据出厂标定数据编写 VINS-Fusion 的相机配置文件

标定板选择

关于标定板的选择，checkerboard和aprilgrid这两种是比较常用的。（相机需要距离标定板1-2m，标定板占据视野60%以上）。由于Aprilgrid能提供序号信息，能够防止姿态计算时出现跳跃的情况。所以建议采用Aprilgrid进行标定

注意：标定过程中，标定板不要离开相机视野范围，开始和结束要平稳进行，尽量使标定板出现在视野所有角落
下载Aprilgrid 6x6 0.8x0.8 m (A0 page)两个文件

在这里插入图片描述

用A4纸打印pdf文件，量好大正方形和小正方形的尺寸

在这里插入图片描述

修改apriltag.yaml

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.021           #size of apriltag, edge to edge [m]
tagSpacing: 0.285714          #ratio of space between tags to tagSize

标定过程

目前ZED-Mini的分辨率，在~/catkin_ws/src/zed-ros-wrapper/zed_wrapper/params文件夹下找到common.yaml，resolution为3，即VGA模式，实际分辨率大小为640*360

启动ZED-Mini的ROS节点，查看topic，开启可视化图像，确保标定板处于左右目图像中

roscore
roslaunch zed_wrapper zedm.launch

rostopic list
rosrun image_view image_view image:=/zed2/zed_node/left/image_rect_color
rosrun image_view image_view image:=/zed2/zed_node/right/image_rect_color

用zed相机采集ros包，可参考下面的视频
https://www.youtube.com/watch?app=desktop&v=puNXsnrYWTY
（1）俯仰角摆动3次
（2）偏航角摆动3次
（3）翻滚角摆动3次
（4）上下移动3次
（5）左右移动3次
（6）前后移动3次
（7）自由移动，摆动幅度大一些，但要移动缓慢些，使标定目标尽可能出现在相机所有视野范围内

rosbag record -O Kalib_data_vga.bag /zedm/zed_node/imu/data_raw /zedm/zed_node/left/image_rect_color /zedm/zed_node/right/image_rect_color

💡 录制过程保证标定板不会超出画面，录制的时候尽量保证图像清晰，不要剧烈移动，同时尽可能的激活IMU各个角度，各个方向

在这里插入图片描述

将两个文件单独放置一个文件夹（这里放置在了kalibr的文件夹下）
在这里插入图片描述

利用kalibr运行标定命令（修改自己的目录）

# 相机标定
source /home/jia/Desktop/package/kalibr_workspace/devel/setup.bash

#双目
rosrun kalibr kalibr_calibrate_cameras --bag /home/jia/Desktop/package/kalibr_workspace/zed/zed.bag --topics /zed2/zed_node/left/image_rect_color2 /zed2/zed_node/right/image_rect_color2 --models pinhole-radtan  pinhole-radtan --target /home/jia/Desktop/package/kalibr_workspace/zed/april_6x6_80x80cm.yaml --bag-from-to 5 150 --show-extraction --approx-sync 0.04

xxx/stereo_calibra.bag stpe3 录制的bag路径
/left_img /right_img 话题名
omni-radtan omni-radtan 后面是相机/畸变模型，有几目相机就要写几个，这里是两个omni-radtan模型相机，所以是两个omni-radtan;相应地，其他支持的模型可以查看https://github.com/ethz-asl/kalibr/wiki/supported-models
xxx/april_6x6_80x80cm_A0.yaml 标定板配置文件（使用标准的标定板，从官网下载的yaml文件参数不用修改，笔记本中有这个文件可以直接用）
–show-extraction　是在标定过程中的一个显示界面，可以看到图片提取的过程，可以不要；
–approx-sync 0.05　时间戳不对齐问题
–bag-from-to后面是想要使用数据时间段的起始时间和结束时间，单位：秒(s)，这个参数可以剔除掉刚开始录制和结束时一些出入视野等画面

一段时间等待后，出现三个文件，标定结束

在这里插入图片描述

.yaml  主要作用为了后期IMU+相机联合标定
.pdf   以图的方式显示效果
.txt   含有相机的内参以及重投影误差

可以查看report的pdf，重投影误差（reprojection errors）在1个像素以内标定就是比较好的

在这里插入图片描述

可以查看一下生成的txt文件中的reprojection error（重投影误差）是多少，理想范围是0.1-0.2

在这里插入图片描述

生成的yaml文件就是两个相机的内参及两个相机之间的变换矩阵

在这里插入图片描述

camera_model//相机模型
T_cam_imu //IMU extrinsics：从IMU到相机坐标的转换（T_c_i）
camera projection type (pinhole / omni)
intrinsics//相机内参
vector containing the intrinsic parameters for the given projection type. elements are as follows:
pinhole: [fu fv pu pv]
omni: [xi fu fv pu pv]
distortion_model//畸变模型
lens distortion type (radtan / equidistant)
distortion_coeffs//畸变参数
parameter vector for the distortion model
T_cn_cnm1//左右摄像头的相对位姿
camera extrinsic transformation, always with respect to the last camera in the chain
(e.g. cam1: T_cn_cnm1 = T_c1_c0, takes cam0 to cam1 coordinates)
timeshift_cam_imu//在捕捉数据时，imu数据和图像时间偏移，相机和IMU时间戳之间的时间间隔，以秒为单位（t_imu = t_cam + shift）
timeshift between camera and IMU timestamps in seconds (t_imu = t_cam + shift)
rostopic //摄像机图像流的主题
topic of the camera's image stream
resolution //相机分辨率[width,height]
camera resolution [width,height]

标定结果检测：

The distortion_coeffs values should be small, even if not equal to zero
The last value of the first row of the T_cn_cnm1 matrix should be very near to -0.06
The last value of the second row of the T_cn_cnm1 matrix should be near to zero
The last value of the third row of the T_cn_cnm1 matrix should be near to zero
The values on the diagonal of the T_cn_cnm1 matrix should be very near to 1.0
The remaining values of the T_cn_cnm1 matrix should be near to zero

一些细节问题

因为zed相机通常以60Hz左右的频率进行录制，而kalibr标定又要求频率不能过高，推荐为4HZ，因此要降低原话题发布频率（标定的图像过多，导致计算量太大），参考降低图像数据到4Hz，（部分博客推荐20HZ，均可，处理时间较长），IMU数据至200Hz，ros的做法是先订阅，然后重新发布

3.关于相机频率设置问题，20Hz or 4Hz？

看了很多博客，大多数中文博客对此没有过多解释，为了方便用20hz的居多。而外文博客用4hz的居多（官方推荐）。

我的理解是差别并不大，考虑到我们并没有专业设备（特制标定板）和合适的标定环境（其实最好是在阴暗环境中），频率反而成为了次要的问题，尽量使用rect话题录制和熟练稳定的手法倒是比较重要的。

当然，如果你用20hz来录制，尽量还是要慢一些，否则后续kalibr很容易无法detect角点进而失败，出现问题1

💡 通过rostopic hz命令查看相关话题的发布频率，相机为15Hz，IMU为200Hz，不需要调整

关于相机图像是采用 raw 还是 rect 也有些讨论

在这里插入图片描述

使用rect话题就可以了，虽然有的博客说直接用出厂参数，但是后续涉及到联合标定，并不符合kalibr的文件格式，也是不怎么方便的。

经过n多次的相机标定，发现只要手法正确，采集环境合理，标定出来的内参和出厂内参其实差的并不多（尽管kalibr给出的误差看起来有点大…）

关于使用出厂标定数据还是使用自标定数据，需要经过实际测试才知道

在这里插入图片描述

imu 标定

先把结论放在这里，关于imu噪声参数的标定对于vio（比如vins、orb3）其实并没有多重要，虽然说他们的配置文件中都要求给一个imu的噪声参数，理由会在正文中说明。如果做imu的标定只是为了给vio的配置文件中imu的噪声参数一个数值的话，不必花太多时间追求一个精确的imu噪声参数，耗时且意义不大

有几个要注意的地方：

imu的噪声分为两种，“White Noise”和“Bias”，都有连续时间模型和离散时间模型两种形式，两种模式可以相互转换，这里强调一下有的标定工具输出的是连续型（imu_utils），有的是离散型（kalibr_allan）。另外有的VIO算法需要的是连续性的imu噪声参数，有的是连续型的噪声参数，具体根据代码或单位辨识
为了方便后续的imu-cam外参标定，有必要说一下Kalibr需要的imu噪声参数的格式。Kalibr需要的噪声参数都是连续时间模型的，单位如下图：

在这里插入图片描述

有博客使用仿真的imu数据对kalibr_allan和imu_utils标定的结果进行了比较（链接），结果是kalibr_allan的高斯白噪声和随机游走都比较准，而imu_util（gaowenliang的版本）的随机游走误差的标定比较差，所以推荐使用kalibr_allan。但是这位博主用的是仿真的数据，和实际情况或许有一些偏差，我自己使用kalibr_allan的过程中并没有取得较为理想的效果，并不推荐kalibr_allan(虽然是uzh大神们写的)，更推荐mintar修改后的imu_utils

gaowenliang的imu_utils貌似有单位的问题，然而大神已经很久没出现了，在github的issue有关于单位问题的讨论，然后有个叫mintar的大佬把单位问题修改了，把输出的值都改成了连续时间的单位，也开源了，并且使用realsense相机同allan_variance_ros的标定结果做了对比（这个开源算法也是Kalibr推荐的，对比结果在imu_utils的issue里），并且结果很相近，所以算法应该是比较合理正确的

标定过程

## 数据采集
source devel/setup.bash

roslaunch zed_wrapper zedm.launch

## 单独录制imu
rosbag record -O zed-imu-calibrate.bag /zedm/zed_node/imu/data_raw

静止zed相机，采集数据，一般为2h左右

在这里插入图片描述

编写launch文件，启动标定工具。根据自己情况进行修改，将该文件放置imu_utils的launch文件夹下

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <!-- 数据集的话题 -->
        <param name="imu_topic" type="string" value= "/zed2/zed_node/imu/data_raw"/>
        <!-- IMU的名字，后面生成的标定文件会附带这个名字作为标记 -->
        <param name="imu_name" type="string" value= "zed_imu"/>
        <!-- 标定结果输出路径 -->
        <param name="data_save_path" type="string" value= "/home/jia/Desktop/package/imu_utils/zed_imu/result"/>
        <!-- 数据集的长度，单位：分钟 -->
        <param name="max_time_min" type="int" value= "120"/>
        <!-- Allan方差的cluster，一般设置100即可 -->
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

运行命令：

source devel/setup.bash

roslaunch imu_utils zed_imu.launch

# 新打开一个终端 播放imu
rosbag play -r 200 /home/jia/Desktop/package/imu_utils/zed_imu/zed-imu-calibrate.bag

这一步是关键，在执行上上步roslaunch imu_utils xsens.launch之后，程序会进入等待话题的状态

得到如下输出结果：

在这里插入图片描述

尽快执行rosbag play -r 200 自己的.bag，程序进入bag读取，并计算allan方差。当bag包加速回放完毕后，执行launch的窗口仍然会显示wait for imu data.，等待一段时间计算，计算完毕后会显示计算结果

在这里插入图片描述

catkin_ws/src/imu_utils/data这个文件夹下会出现一系列的data文件，打开xsens_imu_param.yaml这个文件，会看到计算出来的噪声和随机游走的系数值

一些细节问题

在这里插入图片描述

标定时录制的数据集都是在imu静止的状态下录制的，但是我们在做imu-cam标定或者是vio的时候，imu都是处于动态运动的状态，所以直接使用我们标定的参数是会出问题的，大概率会导致系统的奔溃，这个问题在这个**github issue**中有讨论，mintar给了几点假设：

The allan variance estimation methods (mintar/imu_utils and ori-drs/allan_variance_ros) estimate a model based on Q, N, B, K, R or at least N (“rate/acceleration white noise”), B (“bias instability”), K (“rate/acceleration random walk”). But all VIO packages and Kalibr just use N and K. The calibration is done under close to ideal circumstances in a static setup. In a dynamic setting, with other factors like temperature changes etc., the noise will be higher. The calibration packages take the average of the axes, but some IMUs have different gyros/accelerometers for different axes, so one should probably use the maximum and not the average to be on the safe side. In my setup, there’s no hardware synchronization between IMU and camera. Maybe using a higher standard deviation for the IMU prevents VINS from trusting the IMU too much and deviating when the real error is from a wrong time synchronization.

另外，在Kalibr的**imu noise model**的介绍的最后也做了一个说明：

It is important to note that the IMU measurement error model used here is derived from a sensor which does not undergo motion, and at constant temperature. Hence scale factor errors and bias variation caused by temperature changes, for example, are not accounted for. So clearly, the model is optimistic. Particularly when using low-cost MEMS IMUs with Kalibr, you may have to increase the noise model parameters to “capture” these errors as well. In other words, if you use directly the “sigmas” obtained from static sensor data, Kalibr will tend to trust your IMU measurements too much, and its solution will not be optimal. From our experience, for lowest-cost sensors, increasing the noise model parameters by a factor of 10 or more may be necessary. If you use Kalibr with such a device, please give us feedback, such that we can develop specific guidelines, device-specific parameter suggestions, or more advanced methods to determine these parameters.

那么从这个讨论的内容来看，我们标定imu的结果其实不能直接用在vio算法中，最多只能做一个参考吧，在这个基础上做一些放大。也就是说其实我们不做imu的标定也无所谓，对于vio而言，当vio效果不理想的时候可以直接放大一下imu的噪声参数进行比较。包括在imu-cam的标定过程中，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

相机-IMU联合标定

准备好之前的相机标定的yaml文件
准备好imu的标定文件，将数据中的avg-axis数据按下面的格式写入（注意Acc与Gyr的顺序与imu的话题）

#Accelerometers
accelerometer_noise_density: 1.9033710113349051e-02  #Noise density (continuous-time)
accelerometer_random_walk:   5.5854263338777580e-04  #Bias random walk

#Gyroscopes
gyroscope_noise_density:     1.6947414041138789e-03   #Noise density (continuous-time)
gyroscope_random_walk:       3.9106036886980679e-06  #Bias random walk

rostopic:                    /zed2/zed_node/imu/data_raw2      #the IMU ROS topic
update_rate:          200.0      #Hz (for discretization of the values above)

准备好之前的april_6x6_A4.yaml
准备好之前双目标定的bag文件

source /home/jia/Desktop/package/kalibr_workspace/devel/setup.bash

# imu+双目
rosrun kalibr kalibr_calibrate_imu_camera --bag /home/jia/Desktop/package/kalibr_workspace/imu_zed/zed.bag --target /home/jia/Desktop/package/kalibr_workspace/imu_zed/april_6x6_80x80cm.yaml --cam /home/jia/Desktop/package/kalibr_workspace/imu_zed/zed-camchain.yaml --imu /home/jia/Desktop/package/kalibr_workspace/imu_zed/resultzed_imu_imu_param.yaml

对命令解释说明

１、--target src/data/april_6x6_80x80cm_A0.yaml ：标定板的参数文件；

2、--bag images_imu.bag指定数据包

3、–bag-from-to 5 50设定bag包开始时间和结束时间，避开拿起和放下IMU的时间段内的数据

２、--cam src/data/camera.yaml：camera的参数文件，格式参考【文档１:camera.yaml】。使用标定好的相机参数替换对应的内容；
３、--imu src/data/ium.yaml ：imu的配置文件，格式参考【文档２：imu.yaml】，用标定好的imu参数替换相应的参数；
４、–imu-models scale-misalignment IMU的参数模型

在这里插入图片描述