VIO测试准备——使用imu_utils和kalibr进行相机与IMU标定

0 前言

近期主要学习VIO。相机使用海康的两个支持外触发的单目相机，完成硬件同步外触发后，固联安装在机器人机体两侧，搭建了一个简易的双目相机。IMU使用的是LPMS-USBAL2（老型号，官方已经不继续出售了），固定安装在机器人机体上。上位机系统为Ubuntu 16.04，已安装ROS-Kinetic。为进一步测试搭建的硬件VIO系统，需要对该系统进行标定，使用到imu_utils和kalibr两个工具包。

1 IMU标定

IMU标定使用imu_utils工具包，开源见https://github.com/gaowenliang/imu_utils[1]，通过该工具包完成标定可以提供IMU的随机误差——noise和random walk。

1.1 安装imu_utils

1.1.1 安装ceres

此项为必要依赖。本机在前续使用中已安装了ceres-1.14.0，此处记录对应版本仅供参考，Github上有最新版本应该是2.0.0。ceres-solver同时还需依赖eigen，本机前续使用中已安装了eigen-3.3.7。

1）安装ceres的一些相关依赖（参考http://www.ceres-solver.org/installation.html）[2]

# CMake
sudo apt-get install cmake
# google-glog + gflags
sudo apt-get install libgoogle-glog-dev libgflags-dev
# BLAS & LAPACK
sudo apt-get install libatlas-base-dev
# SuiteSparse and CXSparse (optional)
sudo apt-get install libsuitesparse-dev

2）build, test, and install Ceres

# 已提前下载好的压缩包，没有下载要先去git
tar zxf ~/Documents/ceres-solver-1.14.0.tar.gz
mkdir ceres-bin
cd ceres-bin
cmake ../ceres-solver-1.14.0
make -j4
make test
sudo make install

1.1.2 安装code_utils

sudo apt-get install libdw-dev
mkdir -p ~/kalibr_ws/src
cd kalibr_ws/src
git clone https://github.com/gaowenliang/code_utils.git

参考博客相机与IMU联合标定[3]，这里需要修改 code_utils 的 CMakeLists.txt 文件,将 CMAKE_CXX_FLAGS “-std=c++11” 改为 CMAKE_CXX_STANDARD 14 并添加 include_directories(include/code_utils)。然后在~/kalibr_ws路径下执行catkin_make。这里也是因为后面下载的imu_utils编译时是需要依赖code_utils的，所以这里先单独编译一下，当然也可以在两个包都下好的情况下使用catkin_make -DCATKIN_WHITELIST_PACKAGES='code_utils'先指定编译，再catkin_make -DCATKIN_WHITELIST_PACKAGES='‘全部编译。

1.1.3 安装imu_utils

cd kalibr_ws/src
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make

同样，在catkin_make前，需要修改imu_utils 的 CMakeLists.txt 文件,将 CMAKE_CXX_FLAGS “-std=c++11” 改为 CMAKE_CXX_STANDARD 14。

1.2 录制IMU数据及标定

1.2.1 录制IMU数据

IMU静置，启动IMU节点，并使用rosbag录制IMU静置数据，两个小时以上。

rosrun lpms_imu lpms_imu_node
rosbag record /imu -O lpms_imu_kalibur.bag

1.2.2 标定及结果

1）配置imu_utils/launch路径下的lpms.launch，相应名称和参数应以自己IMU为准。其中，参数max_time_min 表示从rosbag中加载数据的时间长度。

2）启动.launch文件，播放录制的IMU数据包。

# 启动.launch文件
cd kalibr_ws
source devel/setup.bash
roslaunch imu_utils lpms.launch
# 在对应路径下播放数据包，使用-r 200来倍速播放
rosbag play -r 200 lpms_imu_kalibur.bag

3）完成后可以在imu_utils/data路径下找到与imu_name对应的xxx_imu_param.yaml文件。

lpms_imu_param.yaml如下。其中_n代表noise，_w代表random walk。

%YAML:1.0
---
type: IMU
name: lpms
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 5.4583665041817392e-04
      gyr_w: 6.1968037914410386e-06
   x-axis:
      gyr_n: 5.8354554635613415e-04
      gyr_w: 7.8343325801706735e-06
   y-axis:
      gyr_n: 5.9079026229100088e-04
      gyr_w: 8.0393816671606400e-06
   z-axis:
      gyr_n: 4.6317414260738674e-04
      gyr_w: 2.7166971269918020e-06
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 4.8641695361661035e-03
      acc_w: 2.3944306307068487e-04
   x-axis:
      acc_n: 4.8516094435548350e-03
      acc_w: 2.4772267798533511e-04
   y-axis:
      acc_n: 4.8791457703601451e-03
      acc_w: 2.4455179998123807e-04
   z-axis:
      acc_n: 4.8617533945833312e-03
      acc_w: 2.2605471124548143e-04

2 相机标定

相机标定和第3节的相机-IMU联合标定都使用kalibr工具包，开源见https://github.com/ethz-asl/kalibr[4]。其安装和下载要看Wiki页面右侧栏里的Installation和Downloads。

2.1 安装kalibr

2.1.1 安装依赖项

# 安装依赖项
sudo apt-get install python-setuptools ipython libboost-all-dev doxygen libopencv-dev libeigen3-dev
sudo apt-get install libopencv-dev ros-kinetic-vision-opencv ros-kinetic-image-transport-plugins ros-kinetic-cmake-modules python-software-properties software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev

注意版本的对应，这里都是基于ros-kinetic的命令。

2.1.2 安装kalibr

cd ~/kalibr_ws/src
git clone https://github.com/ethz-asl/Kalibr.git
cd ..
catkin build -DCMAKE_BUILD_TYPE=Release -j4

注意：

1）这里使用了catkin build，如果因为之前同一工作空间下已有imu_utils相关编译，导致catkin build有问题，可以先把kalibr_ws下的build和devel删掉，再使用catkin_build编译。

2）要确保系统联网。

3）编译时间略长，要多等一会。

2.2 相机内外参标定

2.2.1 下载并打印标定板

标定板使用二维码aprilgrid或者棋盘格checkerboard都是可以的，使用时做好相应yaml的修改即可。标定板下载见https://github.com/ethz-asl/kalibr/wiki/downloads，需要有google帐号且要有谷歌云端硬盘的访问权限。如果不可以（比方说我就不可以），可参照https://github.com/ethz-asl/kalibr/wiki/calibration-targets，使用kalibr_create_target_pdf命令自己创建指定标定板然后打印就可以了。

本篇使用了二维码标定板，创建或修改apriltag.yaml

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

其中tagsize和tagspacing是要根据实际打印出来的标定板做修改的，其参数意义可参照下图，该图同样来自https://github.com/ethz-asl/kalibr/wiki/calibration-targets。

2.2.2 录制标定数据

1）启动相机节点，因为kalibr处理时建议图像频率不能太高，此处使用topic_tools/throttle（用法很简单，之前也记录了一下用法[5]）对输出的图像话题降频，4Hz也是官方推荐的频率。

# 启动对应的相机节点
roslaunch test pub.launch
# 对图像数据进行降低频率的操作
rosrun topic_tools throttle messages /camleft/video_image 4.0 /caml
rosrun topic_tools throttle messages /camright/video_image 4.0 /camr

2）录制数据包。这里我是采用保持相机不动，在视野中移动标定板的做法。

rosbag record /caml /camr -O camlr1.bag

建议使用rosrun rviz rviz命令打开rviz，添加图像话题，这样方便自己在录制数据时检查标定板始终出现在视野中。

2.2.3 标定及结果

1）标定

这里是双目相机标定，单目相机也是同理。

rosrun kalibr kalibr_calibrate_cameras --bag /home/luoteng/kalibr_ws/bag/camlr1.bag --topics /caml /camr --models pinhole-radtan pinhole-radtan --target /home/luoteng/kalibr_ws/doc/board/apriltag.yaml --show-extraction --bag-from-to 5 105

--bag后面是录制的数据包的绝对路径；

--topics后面是待标定的话题数据（已经控制频率后的）；

--models后面是相机/畸变模型，有几目相机就要写几个，这里是两个针孔模型相机，所以是两个pinhole-radtan;相应地，其他支持的模型可以查看https://github.com/ethz-asl/kalibr/wiki/supported-models。

--target后面是标定板参数文件的绝对路径；

--show-extraction是在标定过程中的一个显示界面，可以看到图片提取的过程，可以不要；

--bag-from-to后面是想要使用数据时间段的起始时间和结束时间，单位：秒(s)，这个参数可以剔除掉刚开始录制和结束时一些出入视野等画面。

个人遇到的问题记录：

标定运行过程中报错，RuntimeError: Optimization failed!

可以参照github上作者的回答https://github.com/ethz-asl/kalibr/issues/41，提高timeOffsetPadding (https://github.com/ethz-asl/kalibr/blob/master/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera#L171)再次尝试。

2）标定结果

标定完成会生成下面3个文件，其中命名为camchain的.yaml文件是后续联合标定要继续用到的，里面包含了所需的相机的内外参。

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

命名为camchain的.yaml文件内容如下。

cam0:
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [-0.11618573305493797, 0.06422069640456901, -0.00013436975723873972,
    -0.001005182072042385]
  distortion_model: radtan
  intrinsics: [781.3078393884367, 777.6555008477854, 818.0060374225857, 637.1024477055486]
  resolution: [1624, 1240]
  rostopic: /caml
cam1:
  T_cn_cnm1:
  - [-0.997500970365195, 0.038409235650546554, -0.0593004615263394, -0.04013241756555838]
  - [-0.0384584418581223, -0.9992601518503074, -0.000311727361527353, 0.48583888428319955]
  - [-0.05926856139929079, 0.001969655006157624, 0.9982401304740343, -0.007530598232056542]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [-0.12076005804518206, 0.0672304142796621, 0.0002881412854617891,
    0.000123605852697794]
  distortion_model: radtan
  intrinsics: [779.6463647028914, 776.406545014067, 795.0043893364035, 622.8861461860073]
  resolution: [1624, 1240]
  rostopic: /camr

3 相机-IMU联合标定

使用kalibr进行相机-IMU联合标定，kalibr安装已在前续中完成了。相机-IMU联合标定总共需要准备好4个文件，分别是包含标定相机和IMU数据的rosbag，IMU内参yaml，相机内外参yaml，标定板yaml。[6]

3.1 录制数据包

按照kalibr作者的tips，录制数据包时，i).充分激励IMU在三轴平移和三轴旋转上的运动；ii).尽可能减少震动，尤其是在拿起和放下传感器时（关于这点，可以使用参数--bag-from-to指定数据包使用的时间，但在过程中依然要减少传感器的震动）；iii).通过保持较低的快门时间和良好的照明等来尽量减少图片的运动模糊。

关于充分激励IMU各轴，https://github.com/ethz-asl/kalibr/wiki/camera-imu-calibration中作者提供了一个演示视频，b站上也有up搬运了https://www.bilibili.com/video/BV1rC4y1p7ma?share_source=copy_web

录制数据包时同样也要控制图像数据频率。

rosrun topic_tools throttle messages /camleft/video_image 4.0 /caml
rosrun topic_tools throttle messages /camright/video_image 4.0 /camr

rosbag record /caml /camr /imu -O camlr-imu1.bag

3.2 IMU内参yaml、相机内外参yaml、标定板yaml

1）相机内外参yaml就用在前续2.2.3相机标定结果中获得的以camchain开头命名的.yaml文件即可；

2）标定板yaml使用前续2.2.1中创建的标定板yaml文件即可；

3）联合标定中imu内参yaml格式如下，并不能直接拿imu_utils的标定结果yaml文件来用，否则会报错RuntimeError: [ImuConfig Reader]: Could not read configuration from ...

#Accelerometers
accelerometer_noise_density: 4.8641695361661035e-03   #Noise density (continuous-time)
accelerometer_random_walk:   2.3944306307068487e-04   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     5.4583665041817392e-04   #Noise density (continuous-time)
gyroscope_random_walk:       6.1968037914410386e-06   #Bias random walk

rostopic:                    /imu      #the IMU ROS topic
update_rate:                 100.0      #Hz (for discretization of the values above)

其中各项可依照前续1.2.2imu标定结果中填入avg-axes的对应值。

3.3 联合标定及结果

1）准备好上述四项后，可以开始联合标定

rosrun kalibr kalibr_calibrate_imu_camera --target /home/luoteng/kalibr_ws/doc/board/apriltag.yaml --cam /home/luoteng/kalibr_ws/doc/stereo/camchain-homeluotengkalibr_wsbagcamlr1.yaml --imu /home/luoteng/kalibr_ws/doc/imu/kalibr_imu.yaml --bag /home/luoteng/kalibr_ws/bag/camlr-imu1.bag --bag-from-to 20 280 --show-extraction

--target后面是标定板yaml的绝对路径；

--cam后面相机内外参yaml（camchain-xxx.yaml）的绝对路径；

--imu后面是对应格式的imu内参yaml的绝对路径；

--bag后面是录制的数据包的绝对路径；

--bag-from-to后面是想要使用数据时间段的起始时间和结束时间，单位：秒(s)

--show-extraction是在标定过程中的一个显示界面，可以看到图片提取的过程，可以不要。

2）标定完成后，会生成下面4个文件。其中比较关注的IMU和相机变换矩阵和timeshift都在以camchain命名的.yaml文件中，在results的txt文件中也可以看到。