ZED2相机标定--双目、IMU、联合标定

为了获取VINS 配置文件中的相关参数，也为了双目相机及IMU传感器实际输出数据更准确，对ZED2的相机进行标定，包括相机标定、IMU标定及联合标定。

一安装标定工具

1、使用kalibr工具标定ZED2双目相机。
2、用imu_utils标定IMU，依次安装编译code_utils、imu_utils。

这一部分之前已经安装过，具体过程参考博客：https://blog.csdn.net/xiaojinger_123/article/details/120849737?spm=1001.2014.3001.5501

但由于系统从ubuntu18.04变成20.04，注意系统依赖问题，根据相应版本修改。

1 下载编译kalibr

sudo apt update
sudo apt-get install python3-setuptools python3-rosinstall ipython3 libeigen3-dev libboost-all-dev doxygen libopencv-dev ros-noetic-vision-opencv ros-noetic-image-transport-plugins ros-noetic-cmake-modules python3-software-properties software-properties-common libpoco-dev python3-matplotlib python3-scipy python3-git python3-pip libtbb-dev libblas-dev liblapack-dev libv4l-dev python3-catkin-tools python3-igraph libsuitesparse-dev
pip3 install wxPython

sudo pip3 install python-igraph --upgrade

mkdir ~/kalibr_ws/src
cd ~/kalibr_ws/src
git clone --recursive https://github.com/ori-drs/kalibrcd ~/kalibr_ws
source /opt/ros/noetic/setup.bash
catkin init
catkin config --extend /opt/ros/noetic
catkin config --merge-devel # Necessary for catkin_tools >= 0.4.
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Releasecatkin build -DCMAKE_BUILD_TYPE=Release -j4

2 下载编译code_utils、imu_utils
1）报错Could not find a package configuration file provided by “Ceres“
解决：安装cereshttps://blog.csdn.net/weixin_48083022/article/details/118282363

2）ceres 编译报错 error: ‘integer_sequence’ is not a member of ‘std’
在编译一些用到ceres的项目的时候会报错 error: ‘integer_sequence’ is not a member of ‘std’，这是因为较新版本中的ceres对c++版本有要求

在报错的项目的CMakeList里的
set(CMAKE_CXX_FLAGS “-std=c++11”)
改成
set(CMAKE_CXX_STANDARD 14)

其他问题参考：https://github.com/gaowenliang/imu_utils/issues/32

二选择标定板

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

注意：标定过程中，标定板不要离开相机视野范围，开始和结束要平稳进行，尽量使标定板出现在视野所有角落。

checkerboard：
targetCols、targetRows数的是内角点数目

target_type: 'checkerboard' #gridtype
targetCols: 6               #number of internal chessboard corners
targetRows: 8               #number of internal chessboard corners
rowSpacingMeters: 0.17      #size of one chessboard square [m]
colSpacingMeters: 0.17      #size of one chessboard square [m]

aprilgrid：
tagSpacing=小方格边长/大方格边长

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

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

三 ZED2标定数据录制

目前ZED2的分辨率，在ZED2_WS/src/zed-ros-wrapper/zed_wrapper/params文件夹下找到common.yaml，resolution为3，即VGA模式，实际分辨率大小为672*376.

启动ZED2的ROS节点：

roscore
roslaunch zed_wrapper zed2.launch

查看topic，开启可视化图像，确保标定板处于左右目图像中：

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

1）报错：Command ‘rosrun’ not found
解决：sudo apt install ros-noetic-rosbash

2）报错：[rospack] Error: package ‘image_view’ not found
解决：sudo apt-get install ros-noetic-image-view

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

这里有一个降帧的命令

rosrun topic_tools throttle messages old_topic 4.0 new_topic//kalibr推荐的帧率为4HZ

这里还有一点，就是zed相机里有几个话题和原始图像对应，比如：
/zed2/zed_node/left/image_rect_color，这个话题也可以使用：分辨率没有发生变化，但是是对原始图像进行边缘裁剪后拉伸，这可能会导致丢失信息。

降低图像频率，查看当前频率：

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 4.0 /zed2/zed_node/left/image_rect_color2
rosrun topic_tools throttle messages /zed2/zed_node/right/image_rect_color 4.0 /zed2/zed_node/right/image_rect_color2rostopic hz /zed2/zed_node/left/image_rect_color2
rostopic hz /zed2/zed_node/right/image_rect_color2

开始录制标定bag文件,参考Youtube视频:https://youtu.be/puNXsnrYWTY?t=57：
注意：录制过程保证标定板不会超出画面，录制的时候尽量保证图像清晰，不要剧烈移动，同时尽可能的激活IMU各个角度，各个方向。

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

录制结束后会得到一个Kalib_data_vga.bag文件。

查看记录的数据

四开始相机标定

这里使用的是Aprilgrid标定板，对应的april.yaml(april_6x6_80x80cm.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

在kalibr文件夹下，执行标定，其中的april.yaml表示同标定板一起下下来的参数文件。

单目标定：

source ~/kalibr_workspace/devel/setup.bash
rosrun kalibr kalibr_calibrate_cameras --bag Kalib_data_vga.bag --topics /zed2/zed_node/left/image_rect_color2 --models pinhole-radtan --target april.yaml

在执行单目标定、单目+IMU联合表定时会报错如下：

Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance.
Traceback (most recent call last):File "/home/ipsg/tool/kalibr_ws/devel/bin/kalibr_calibrate_cameras", line 15, in <module>exec(compile(fh.read(), python_script, 'exec'), context)File "/home/ipsg/tool/kalibr_ws/src/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras", line 447, in <module>main()File "/home/ipsg/tool/kalibr_ws/src/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras", line 204, in maingraph.plotGraph()File "/home/ipsg/tool/kalibr_ws/src/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py", line 311, in plotGraphedge_label=self.G.es["weight"],
KeyError: 'Attribute does not exist'

解决办法：

找到工作空间下src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras中的如下代码，然后注释掉（201行）

if not graph.isGraphConnected(): obsdb.printTable() print "Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance." graph.plotGraph() sys.exit(-1)

注释掉之后便可以得出标定结果。

双目标定：

source ~/kalibr_workspace/devel/setup.bash
rosrun kalibr kalibr_calibrate_cameras --bag 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 april.yaml

可添加–show-extraction，在标定过程中可以可视化角点检测情况是否良好，发现角点重投影出现严重错误; --approx-sync 0.04，其中0.04可以看情况调整到0.1，作用是使各个摄像头数据同步。
–bag-from-to 参数，如果数据集开始和最后有抬起相机的动作，请加上这个参数，去除这部分的数据。抬起放下的动作会对标定有一定影响。没有可不加。

报错1）：

[FATAL] [1636711641.843028]: No corners could be extracted for camera /zed2/zed_node/left/image_rect_color2! Check the calibration target configuration and dataset.
Traceback (most recent call last):File "/home/sjj/kalibr_workspace/devel/lib/kalibr/kalibr_calibrate_cameras", line 15, in <module>exec(compile(fh.read(), python_script, 'exec'), context)File "/home/sjj/kalibr_workspace/src/kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras", line 447, in <module>main()File "/home/sjj/kalibr_workspace/src/kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras", line 185, in mainif not cam.initGeometryFromObservations(observations):File "/home/sjj/kalibr_workspace/src/kalibr/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py", line 56, in initGeometryFromObservationssuccess = self.geometry.initializeIntrinsics(observations)
RuntimeError: [Exception] /home/sjj/kalibr_workspace/src/kalibr/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp:716: initializeIntrinsics() assert(observations.size() != 0) failed: Need min. one observation

这个问题怀疑是由于标定板太小，相机分辨率不够，更改后可成功检测到角点信息。

报错2）：
标定过程中，如果提示不能得到初始焦距的时候，可以设置：export KALIBR_MANUAL_FOCAL_LENGTH_INIT=1。然后运行程序，当程序运行失败的时候，它会提示要你手动输入一个焦距，Initialization of focal length failed. Provide manual initialization: 这时你手动输入比如 400。给比较大的值，也能收敛。

标定完成后，目录下将会生成3个文件

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

附：.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

五 IMU参数标定

这部分可由以下两种方式获取：
1）通过录制静止数据（越2小时），修改launch文件，使用imu_utils进行标定，得到标定结果文件，从而创建对应的imu-params.yaml（取标定结果Acc及Gyr的平均值填入imu.yaml文件）
2）除了使用自己标定的IMU参数信息，也可以直接使用官网上提供的该参数，经验证，标定过后也是可以在系统中运行的

这里选取第二种方式设置imu.yaml，官方给的参数有一定的可信度，若后期实验数据不好，则可重新手动标定。参考：https://blog.csdn.net/sinat_16643223/article/details/115416277?spm=1001.2014.3001.5506

创建imu-params.yaml

gedit imu-params.yaml

并填写如下内容:

#Accelerometers
accelerometer_noise_density: 1.4e-03   #Noise density (continuous-time)
accelerometer_random_walk:   8.0e-05   #Bias random walk#Gyroscopes
gyroscope_noise_density:     8.6e-05   #Noise density (continuous-time)
gyroscope_random_walk:       2.2e-06   #Bias random walkrostopic:                    /zed2/zed_node/imu/data_raw2      #the IMU ROS topic
update_rate:                 200.0     #Hz (for discretization of the values above)

六相机IMU联合标定

联合标定主要是为了获得相机和IMU轴系的转换关系

rosrun kalibr kalibr_calibrate_imu_camera --bag Kalib_data_vga.bag --cam camchain-Kalib_data_vga.yaml --imu imu-params.yaml --target april.yaml

报错：

[ERROR] [1637115378.140707]: Optimization failed!
Traceback (most recent call last):File "/home/sjj/kalibr_workspace/devel/lib/kalibr/kalibr_calibrate_imu_camera", line 15, in <module>exec(compile(fh.read(), python_script, 'exec'), context)File "/home/sjj/kalibr_workspace/src/kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera", line 246, in <module>main()File "/home/sjj/kalibr_workspace/src/kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera", line 209, in mainiCal.optimize(maxIterations=parsed.max_iter, recoverCov=parsed.recover_cov)File "/home/sjj/kalibr_workspace/src/kalibr/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/icc_calibrator.py", line 179, in optimizeraise RuntimeError("Optimization failed!")
RuntimeError: Optimization failed!

这个报错上面可以成功读取自己对应的imu信息和单目信息。所以说数据大概率也没问题。最终在kalibr的issues里找到了解决问题办法。
如果kalibr运行时遇到问题，均可去官网搜索，一般你遇到的大部分问题都有人帮你埋坑的。

解决：
请打开kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera.py文件，搜索timeOffsetPadding，我先是查看它的值看了一下是0.03，然后增大此变量的值即可，首先我增大到了0.3，不行，增大到3，优化时间太长，导致类似卡死的情况，可再降低，依据自己的情况调试。(PS：不能太大，我一开始改成了100，直接内存给干爆了)。

重新运行，即可得到联合标定结果camchain-imucam-Kalibr_data.yaml文件和完整的PDF报告。