ZED2相机IMU联合标定&&运行vins-mono

------------------------------------------------------------------------------------------------------------------------------

一、相机介绍

ZED2相机是由STEREOLABS公司提供的惯性双目相机，这里是关于ZED2功能以及性能较为详细的的图文介绍。ZED的SDK在官网中可以找到（https://stereolabs.com）

二、 标定工具包的选择

经过调研，我们选择kalibr作为双目相机内参与畸变，以及相机与imu之间的外参的标定。（同时也可以使用matlab进行标定，但根据最后的结果显示还是kalibr更准一些，imu的bias、随意游走的标定选用imu_util
Kalibr是一个解决以下校准问题的工具箱:
1.多相机校准:具有非全局共享重叠视场的相机系统的内在和外在校准，支持广泛的相机模型。
2.视觉惯性校准(CAM-IMU): IMU的空间和时间校准，包括相机系统和IMU固有参数。
3.多惯性校准(IMU-IMU):对一个IMU进行空间和时间的校准，而不是一个基本惯性传感器以及IMU的固有参数(需要一个辅助相机传感器)。
4.卷帘式相机校准(Rolling Shutter Camera Calibration):卷帘式相机的全部内部校准(投影、失真和快门参数)。

可以通过下列命令从github进行安装。

git clone https://github.com/ethz-asl/kalib.git

这里顺便介绍一个很好用的仓库gitee，我想很多像我一样的小白都深受github网速慢下载缓慢的困扰，网上有很多ubuntu上提速github的方法，但效果都并不理想（甚至可以说没什么用），希望能帮到你。在gitee上注册一个账号，可以将github的链接导入到仓库中。导入后这个样子

你可以通过箭头上的网址在git clone时代替github网址下载，记得设置一个简单的用户名和密码，git clone下载的时候后要用。

回到正题，接下来是imu_utils的安装，参考github上的安装要求即可，且依赖于code_utils，需要先安装code_utils，然后创建catkin工作空间，必须先把code_utils放进去catkin_make,然后再把imu_utils文件放入工作空间中catkin_make,否则会报错找不到code_utils。
im[u_utils和code_utils的github在这里
https://github.com/gaowenliang/imu_utils
https://github.com/gaowenliang/code_utils

三、标定方法理论介绍

一般碰到公式有用纸笔记录的习惯，笔记不整请见谅。

1.相机内参标定

张友正内参标定法：
张友正教授1988年提出，理论上通过最少6张图片，就可以求出相机内参。
标定所用的传统标定板大概长这样。一般用的时候打印出来贴在板子上或者墙上，用相机从不同角度拍摄即可。

三张笔记解释清楚，首先看第一张图，第一个公式的含义是由外部世界物体的坐标投影到相机图像平面的过程，物体坐标先通过相机与标定板之间的变换矩阵转移到相机本身。相机内参由四个或五个参数组成，这里写作五个，分别为fx，fy，cx，cy，图中有所不同分别记为fa，f，xo，yo，本质和是一样的。这四个数量分别为图像平面与真实世界的缩放（fx，fy）以及图像平面坐标原点和真实世界的偏移（cx，cy）。在标定过程中，我们把世界坐标系设置在标定板的角点上，这样物体坐标的Z方向为0，因此就有了中间的式子。我们把内参乘外参得到的矩阵记为H，$$H=[h1,h2,h3{]}^T$$
由旋转矩阵的正交性，各轴之间相互垂直，可以的到最后的式子。

上面的圈1式中的A-1AT记为B，得到一个33的对陈矩阵，所以实际参数为6个。将B矩阵转换为b列向量，这样我们将二次型转化为两个向量相乘。取多个图像进行n次操作，这样就构成了经典的Ab=0的问题，可用SVD，直接求解等方法求出b，通过b得到B之后对B矩阵进行cholesky分解得到了最后的结果。

由内参根据第一张图中的公式可以反推外参。

由于每一幅照片的拍摄角度不同，所以每一幅照片外参也不同。

畸变标定：
相机畸变模型分为四参数和五参数模型，标定原理一致，但kalibr中运用的为4参数，实际使用时四参数够用。这里拿五参数举例
在不考虑畸变系数的情况下优化完内外参数后，可以计算畸变系数。需要注明，畸变变换是在归一化平面进行的，因此计算时可以直接将坐标变换到归一化坐标系方便进行计算，也方便进行理解。
设（u,v）为标定板根据外参投影到归一化平面上的点坐标， ( u d i s , v d i s ) (u_{dis},v_{dis}) (udis​,vdis​)为畸变后的坐标，也即实际图像坐标根据内参反向映射到归一化平面。则可得

其中$r^2=u^2+v^2$
将其转化为方程，则是

可以写成$Ab=C$

则可直接计算出畸变参数。

2.IMU艾伦方差标定

这里借用课程中的ppt描述，原理讲解的简洁明了。

3.相机与IMU外参标定

kalibr外参标定原理参阅kalibr系列论文。
论文：Continuous-Time Batch Estimation using Temporal Basis Functions
论文： Unified Temporal and Spatial Calibration for Multi-Sensor Systems
论文： Extending kalibr Calibrating the Extrinsics of Multiple IMUs and of Individual Axes

VINS-mono在线标定原理：
实际上是利用不同帧之间的角点追踪，先通过重投影求解出相机之间的位姿变换，imu 之间的位姿变换可以通过自身解算得到，求解量为相机和imu位姿变换，从示意图以及公式可以看出，通过从bo imu 出发到c1相机共有两条路径，我们通过这两条路径构成等式，多帧采集数据便组成A*b=0问题，可通过SVD求解。

四、标定实操

1.安装ZED2 wapper

该程序为官方提供的节点调用程序，以供后续开发使用。

cd ~/catkin_ws/src
git clone --recursive https://github.com/stereolabs/zed-ros-wrapper.git
git clone https://github.com/stereolabs/zed-ros-interfaces.git
git clone https://github.com/stereolabs/zed-ros-examples.git
cd ../
rosdep install --from-paths src --ignore-src -r -y
catkin_make -DCMAKE_BUILD_TYPE=Release

进行测试

source ./devel/setup.bash
roslaunch zed_display_rviz display_zed.launch

2.棋盘格

这里提醒，更新的kalibr中似乎不支持checkboard.yaml的标定板的。我用常规的（上文提到的样式）在kalibr中尝试很多次，并不能检测到角点。使用
rosrun kalibr kalibr_create_target_pdf --type apriltag --nx [4] --ny [6] --tsize 0.05 --tspace 0.2
生成如下棋盘格

![在这里插入图片描述](https://img-blog.csdnimg.cn/c056201911f141dab658e899de8fc56e.png上述指令规则如下

3.标定数据录制

根据需求修改了ZED2的分辨率，在ZED2_WS/src/zed-ros-wrapper/zed_wrapper/params文件夹下找到common.yaml，设置resolution为3，即HD720模式，实际分辨率大小为640*360.

然后打开ZED2相机开启数据录制:

roslaunch zed_wrapper zed2.launch

启用左摄像头可视化功能，以确保将标定板保持在相机范围内。

rosrun image_view image_view image:=/zed2/zed_node/left/image_rect_color

kalibr在处理标定数据的时候要求图像的频率不可过高，降低图像数据到20HZ，IMU数据至200HZ

rosrun topic_tools throttle messages /zed2/zed_node/imu/data_raw  200 /zed2/zed_node/imu/data_raw2
rosrun topic_tools throttle messages /zed2/zed_node/left/image_rect_color 20 /zed2/zed_node/left/image_rect_color2
rosrun topic_tools throttle messages /zed2/zed_node/right/image_rect_color 20 /zed2/zed_node/right/image_rect_color2

录制数据

rosbag record -O Kalib_data_vga.bag /zed2/zed_node/imu/data_raw2 /zed2/zed_node/left/image_rect_color2 /zed2/zed_node/right/image_rect_color2

4.开始相机标定

单目：

kalibr_calibrate_cameras --bag /home/ipsg/ZED2_WS/Kalib_data_vga.bag --topics /zed2/zed_node/left/image_rect_color2 --models pinhole-radtan  --target /home/ipsg/dataset/checkboard.yaml --bag-from-to 5 140 --show-extraction --approx-sync 0.04

双目：

kalibr_calibrate_cameras --bag /home/ipsg/ZED2_WS/Kalib_data_vga.bag --topics /zed2/zed_node/left/image_rect_color2 /zed2/zed_node/right/image_rect_color2 --models pinhole-radtan  pinhole-radtan --target /home/ipsg/dataset/checkboard.yaml --bag-from-to 5 140 --show-extraction --approx-sync 0.04

标定结果

5.IMU参数标定

根据imu_utils文件夹里面的 tum.launch 改写ZED2标定启动文件：ZED2_calibration.launch注意，max_time_min对应的参数，默认是75，如果不足，这个时间值要改为略小于真实时间。我录制了一小时20分钟的数据（80分钟）：

<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"/>
        <param name="imu_name" type="string" value= "ZED2"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "75"/>
        <param name="max_cluster" type="int" value= "200"/>
    </node>
</launch>

启动标定：

roslaunch imu_utils ZED2_calibration.launch

回访数据包，以200Hz的速率回放：

rosbag play -r 200 imu_calibration.bag  

最后可以得到标定结果文件：

%YAML:1.0
---
type: IMU
name: ZED2
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 1.5613081804663456e-03
      gyr_w: 5.2136979291365317e-06
   x-axis:
      gyr_n: 1.5056347381046632e-03
      gyr_w: 3.2366868691766584e-06
   y-axis:
      gyr_n: 1.6077683842094395e-03
      gyr_w: 3.2713230322843935e-06
   z-axis:
      gyr_n: 1.5705214190849337e-03
      gyr_w: 9.1330838859485453e-06
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.1490988052722659e-02
      acc_w: 8.2474418857263112e-04
   x-axis:
      acc_n: 2.2077193703268706e-02
      acc_w: 9.7275149533692404e-04
   y-axis:
      acc_n: 2.2608717423832660e-02
      acc_w: 8.6956480632981506e-04
   z-axis:
      acc_n: 1.9787053031066613e-02
      acc_w: 6.3191626405115426e-04

6.相机+IMU联合标定

利用步骤2中录制的数据包，执行标定，需要准备cam.yaml及imu.yaml文件，cam.yaml为单双目输出的标定文件，本次标定仅对左目标定，使用如下：
imu.yaml信息由步骤4中的IMU标定结果得出，取标定结果Acc及Gyr的平均值填入imu.yaml文件。
执行联合标定：

kalibr_calibrate_imu_camera \
    --target /home/ipsg/dataset/checkboard.yaml \
    --bag /home/ipsg/ZED2_WS/Kalib_data_vga.bag \
    --bag-from-to 10 130 \
    --cam /home/ipsg/tool/kalibr_ws/cam.yaml \
    --imu /home/ipsg/tool/kalibr_ws/imu.yaml \
    --imu-models scale-misalignment \
    --timeoffset-padding 0.1

加载时间很长，请耐心等待。（1w*2w的雅可比高斯牛顿。。。，算了两三个小时）
标定结果为Kalib_data_HD720-camchain-imucam.yaml：

cam0:
  T_cam_imu:
  - [-0.007082706299509217, -0.9999468425977298, 0.007493146889307467, 0.013454669543264547]
  - [-0.006699477744578297, -0.007445716378999684, -0.9999498378946577, 0.0034949106123359714]
  - [0.9999524750053993, -0.007132551186871505, -0.006646385795442938, -0.03274712148436802]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [0.0021938842120549805, -0.011235060140414982, -0.0014426372193363359, -0.0007936056060251419]
  distortion_model: radtan
  intrinsics: [261.21331635773214, 262.92087129926944, 323.3191286446446, 180.28287450914445]
  resolution: [640, 360]
  rostopic: /zed2/zed_node/left/image_rect_color2
  timeshift_cam_imu: 0.0005616084301743977

五、 使用ZED2运行VINS-mono

修改realsense_color_config.yaml文件
1、订阅topics修改

imu_topic: "/zed2/zed_node/imu/data_raw"
image_topic: "/zed2/zed_node/left/image_rect_color"

2、左目相机内参修改

model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 360
distortion_parameters:
   k1: 0
   k2: 0
   p1: 0
   p2: 0
projection_parameters:
   fx: 261.21331635773214
   fy: 262.92087129926944
   cx: 323.3191286446446
   cy: 180.28287450914445

这里使用的是校正后的图像，故设置畸变系数均为0；

3、IMU至cam的变换矩阵，参数修改为2，使用在线标定（设置为0，使用已有的标定参数也是可以运行的）：

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 2   # 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.
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.                        
#If you choose 0 or 1, you should write down the following matrix.

4、IMU参数，使用VINS-mono中给定的参数

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.2          # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.05         # gyroscope measurement noise standard deviation.     #0.05
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.80       # gravity magnitude

5、不需要在线估计同步时差

#unsynchronization parameters
estimate_td: 0                      # online estimate time offset between camera and imu
td: 0.000                           # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

6、相机改为全局曝光

#rolling shutter parameters
rolling_shutter: 0                      # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0               # unit: s. rolling shutter read out time per frame (from data sheet). 

7、运行

roslaunch zed_wrapper zed2.launch
roslaunch vins_estimator realsense_live.launch 
roslaunch vins_estimator vins_rviz.launch

最终结果，终于打通标定这一步了。