ZED 2i 双目-IMU标定

环境：ubuntu 18.04

ROS-melodic

1、标定IMU的白噪声和bias游走时，有两个常用的开源工具，一个是imu_utils，一个是kalibr_allan，都是计算IMU随机误差的Allan方差的
2、Allan方差是将误差序列在某个指定的时间尺度上的波动情况进行了精确提取，其具体计算步骤如下：
   a、将整段误差序列按设定的时间尺度的长度（例如1分钟）进行分块；

   b、通过分块确定所要考察的时间块长度；

   c、每块求平均值；

   d、利用块内求平均的办法把短于块长度的那些快速变化成份（细节）都抹掉；

   e、相邻块的平均值求差；

   f、利用相邻块求差的办法把长于两块长度的那些缓慢变化成份（宏观）都抹掉

   g、将所有差值进行统计，得到其均方值，并乘以1/2。

   h、最后对差值序列统计其均方值（这是处理任何随机样本的标准操作），这样统计出来的就是介于1倍块长度和2倍块长度这样一个很窄的时间尺度范围内的误差波动情况。

标定过程：

IMU标定：

1、编译标定工具，使用imu_utils进行标定，依赖ceres-solver库，要先安装ceres库
   imu_utils是依赖于code_utils的，所以先编译code_utils后再下载编译imu_utils
$   cd  ~/calibration_ws/src
$   git clone https://github.com/gaowenliang/code_utils
$   cd ..
$   catkin_make
编译需要安装 $ sudo apt install libdw-dev #安装libdw库
将sumpixel_test.cpp中的：
#include “backward.hpp"
修改为
#include “code_utils/backward.hpp”

编译成功code_utils之后，下载编译imu_utils
$  cd  ~/calibration_ws/src
$  git clone https://github.com/gaowenliang/imu_utils
$  cd ..
$  catkin_make

编译完成

2、准备数据集

打开ZED相机

$ roslaunch zed_wrapper zed2i.launch

静止IMU，录制一段两个小时左右的 /imu_topic 的数据，用于之后回放标定

$  rosbag record -O zed-imu-calibrate.bag /zed2i/zed_node/imu/data_raw

3、标定IMU
    编写一个launch文件，启动标定工具。
这里以标定ZED 2i内部的IMU为例，如下：
 <launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <!-- 数据集的话题 -->
        <param name="imu_topic" type="string" value= "/zed2i/zed_node/imu/data_raw"/>
        <!-- IMU的名字，后面生成的标定文件会附带这个名字作为标记 -->
        <param name="imu_name" type="string" value= "zed_imu"/>
        <!-- 标定结果输出路径 -->
        <param name="data_save_path" type="string" value= "/home/lyy/ZED_calib/imu_calib"/>
        <!-- 数据集的长度，单位：分钟 -->
        <param name="max_time_min" type="int" value= "170"/>
        <!-- Allan方差的cluster，一般设置100即可 -->
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

之后就是播放录制的数据集，以及运行上面的launch文件：
 roslaunch imu_utils zed_imu_calibrate.launch
播放数据集的时候可以加速播放，我这里是加速了200倍
  rosbag play zed-imu-calibrate.bag -r 200

～～～～～～～～～生成IMU的白噪声参数文件～～～～～～～～～～～～～～

Camera--IMU标定：

选择使用Kaibr

Kailbr 使用手册 https://slightech.github.io/MYNT-EYE-S-SDK-Guide/src/slam/how_to_use_kalibr.html

1、安装依赖 
$ sudo apt install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev
$ sudo apt install ros-noetic-vision-opencv ros-noetic-image-transport-plugins ros-noetic-cmake-modules
$ sudo apt install python-software-properties software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython 
$ sudo apt install libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev 
$ sudo apt install build-essential python-dev libxml2 libxml2-dev zlib1g-dev bison flex libigraph0-dev texlive-binaries
$ sudo pip install -i https://pypi.tuna.tsinghua.edu.cn/simple python-igraph
$ sudo pip install python-igraph --upgrade

2、编译Kaibr
$ cd ~/kalibr_ws/src
$ git clone https://github.com/ethz-asl/Kalibr.git
$ cd .. && catkin build -DCMAKE_BUILD_TYPE=Release -j2

注：编译完成会显示成功数 警告数 最好多编译几次 全部成功再进行下一步

使用方式
例如：查看标定版生成的指令的参数
$ rosrun kalibr kalibr_create_target_pdf -h

3、制作标定版
下载标定版
三种类型的标定板(Aprilgrid, Checkerboard, Circlegrid)。由于Aprilgrid能提供序号信息, 能够防止姿态计算时出现跳跃的情况, 所以建议采用Aprilgrid进行标定。

标定板下载：https://github.com/ethz-asl/kalibr/wiki/downloads

生成标定版
$ rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.08 --tspace 0.3
这时文件夹中会生成一个标定板 target.pdf
注意：这个是A0纸尺寸的，后面我们编写target.yaml文件的时候要以实际的标定板的尺寸进行参数填写
target.yaml文件

在标定之间我们需要一个描述这个标定板的文件，这个文件是描述标定板的真实尺度的，比如：如果用A4纸打印的就写成如下即可
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.285714285714   #ratio of space between tags to tagSize
codeOffset: 0            #code offset for the first tag in the aprilboard
 

4、数据采集

把标定纸放在桌面上，让相机从各个角度对标定纸进行拍摄即可，或者相机不动，把标定纸在相机前面晃，让相机能在各个角度对标定纸拍摄

1.启动相机
$ roslaunch zed_wrapper zed2i.launch

2.启动左右摄像头可视化功能
$ rosrun image_view image_view image:=/zed2i/zed_node/left/image_rect_color & rosrun image_view image_view image:=/zed2i/zed_node/right/image_rect_color 
或者分别
$ rosrun image_view image_view image:=/zed2i/zed_node/left/image_rect_color __name:=left_image_view &
$ rosrun image_view image_view image:=/zed2i/zed_node/right/image_rect_color __name:=right_image_view &

3.
通常设备采集的频率为 20-60 hz, 这会使得标定的图像过多, 而导致计算量太大
通过topic_tools将ros topic的频率降低到4hz左右进行采集
设置IMU的频率为200HZ
调整频率，查看
$ rosrun topic_tools throttle messages /zed2i/zed_node/imu/data_raw  200 /zed2i/zed_node/imu/data_raw2
$ rosrun topic_tools throttle messages /zed2i/zed_node/left/image_rect_color 4.0 /zed2i/zed_node/left/image_rect_color2
$ rosrun topic_tools throttle messages /zed2i/zed_node/right/image_rect_color 4.0 /zed2i/zed_node/right/image_rect_color2

$ rostopic hz /zed2i/zed_node/left/image_rect_color2
$ rostopic hz /zed2i/zed_node/right/image_rect_color2

录制IMU+Camera
$ rosbag record -O Kalib_data_vga.bag /zed2i/zed_node/imu/data_raw2 /zed2i/zed_node/left/image_rect_color2 /zed2i/zed_node/right/image_rect_color2

查看录制的话题

$rosbag info Kalib_data_vga.bag

单目标定：
$ rosrun kalibr kalibr_calibrate_cameras --bag kalib_data_vga.bag --target target.yaml --topic /zed2i/zed_node/left/image_rect_color --models pinhole-radtan

单目+IMU标定：
$ rosrun kalibr_calibrate_imu_camera --target [target.yaml] --bag [Kalib_data_vga.bag] --cam [camchain.yaml] --imu [imu.yaml] 

其中imu.yaml编写为:

rostopic: /zed2i/zed_node/imu/data_raw2
update_rate: 200.0 #Hz

accelerometer_noise_density: 2.1263614567188106e-02
accelerometer_random_walk: 5.5681010088760180e-04
gyroscope_noise_density: 1.6691322134314045e-03
gyroscope_random_walk: 5.3600289047510118e-06

双目标定：
$  source ~/kalibr_workspace/devel/setup.bash
$  rosrun kalibr kalibr_calibrate_cameras --bag Kalib_data_vga.bag --topics /zed2i/zed_node/left/image_rect_color2 /zed2i/zed_node/right/image_rect_color2 --models pinhole-radtan pinhole-radtan --target target.yaml 

pdf中有一些误差分析结果
从给出的结果来看，重投影误差不大，基本控制在1-2个像素之内。