相机标定

Posted on | In
相机标定的目标是获取相机内外参、imu参数、几者间的转换矩阵参数，标定工具Kalibr。
一、概述

          相机标定的目标是获取双目相机参数、imu参数、几者之间的转换矩阵参数。标定的主要工作包括：
1.获取双目相机内参+双目转换矩阵；
2.获取imu参数，以测定噪声密度、随机游走；
3.双目+imu联合标定，获取双目到imu的转换矩阵；

二、相机参数说明

          相机参数配置文件camchain.yaml，存储相机内外参的校准、IMU相对于相机的空间/时间校准参数。其所含的参数说明如下：
· CAMERA_MODEL：camera_model(pin_hole/omni)，(针孔/全向)。
· intrinsics：包含给定投影类型的内部参数的向量。要素如下：

  • pinhone: [fu fv pu pv]
  • omni: [xi fu fv pu pv]
  • ds: [xi alpha fu fv pu pv]
  • eucm: [alpha beta fu fv pu pv]

· distortion_model：畸变模型，(radtan/equidistant)。
· distortion_coeffs：畸变参数。
· T_cn_cnm1：相机外在转换，总是相对于链中最后一个相机。（例如：cam1：T_cn_cnm1 = T_c1_c0，将cam0转换为cam1坐标）
· T_cam_imu：imu外参，从imu到相机坐标的转换(T_c_i)。
· timeshift_cam_imu：相机和imu时间戳之间的时间间隔，以秒为单位（t_imu = t_cam + shift）。
· rostopic：摄像机图像流的主题。
· resolution：相机分辨率[width,height]。

示例：

1.相机chain.yaml
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
cam0:
  camera_model: pinhole
  intrinsics: [461.629, 460.152, 362.680, 246.049]
  distortion_model: radtan
  distortion_coeffs: [-0.27695497, 0.06712482, 0.00087538, 0.00011556]
  T_cam_imu:
  - [0.01779318, 0.99967549,-0.01822936, 0.07008565]
  - [-0.9998017, 0.01795239, 0.00860714,-0.01771023]
  - [0.00893160, 0.01807260, 0.99979678, 0.00399246]
  - [0.0, 0.0, 0.0, 1.0]
  timeshift_cam_imu: -8.121e-05
  rostopic: /cam0/image_raw
  resolution: [752, 480]
cam1:
  camera_model: omni
  intrinsics: [0.80065662, 833.006, 830.345, 373.850, 253.749]
  distortion_model: radtan
  distortion_coeffs: [-0.33518750, 0.13211436, 0.00055967, 0.00057686]
  T_cn_cnm1:
  - [ 0.99998854, 0.00216014, 0.00427195,-0.11003785]
  - [-0.00221074, 0.99992702, 0.01187697, 0.00045792]
  - [-0.00424598,-0.01188627, 0.99992034,-0.00064487]
  - [0.0, 0.0, 0.0, 1.0]
  T_cam_imu:
  - [ 0.01567142, 0.99978002,-0.01393948,-0.03997419]
  - [-0.99966203, 0.01595569, 0.02052137,-0.01735854]
  - [ 0.02073927, 0.01361317, 0.99969223, 0.00326019]
  - [0.0, 0.0, 0.0, 1.0]
  timeshift_cam_imu: -8.681e-05
  rostopic: /cam1/image_raw
  resolution: [752, 480]
2.imu配置：imu.yaml
1
2
3
4
5
6
7
8
9
10
#Accelerometers
accelerometer_noise_density: 1.86e-03   #Noise density (continuous-time)
accelerometer_random_walk:   4.33e-04   #Bias random walk
#Gyroscopes
gyroscope_noise_density:     1.87e-04   #Noise density (continuous-time)
gyroscope_random_walk:       2.66e-05   #Bias random walk
rostopic:                    /imu0      #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)
三、标定环境搭建
1.ros安装

注意当前匹配使用的ubuntu系统版本。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
1.[添加软件源=aliyun]  # ubuntu 18.04 & 20.04
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo sh -c '. /etc/lsb-release && echo "deb http://mirrors.ustc.edu.cn/ros/ubuntu/ $DISTRIB_CODENAME main" > /etc/apt/sources.list.d/ros-latest.list'
2.[设置秘钥]
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
3.[安装ROS文件]
sudo apt update
sudo apt install ros-melodic-desktop-full     # ubuntu 18.04
sudo apt install ros-noetic-desktop-full      # ubuntu 20.04 
如果提示找不到rosdep，执行sudo apt install python3-rosdep2，先安装rospack-tools包: sudo apt install rospack-tools
4.[初始化rosdep] # 安装过程中,执行该步骤,参考其他各种科学上网方法
sudo rosdep init
rosdep update
5.[设置环境变量]
echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc      # ubuntu 18.04
echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc       # ubuntu 20.04
source ~/.bashrc
执行上述命令后，可以输入ros并按tab键，正常情况下会输出很多ros开头的对应所有命令，如果环境变量设置有问题，将不会出现。
6.[安装rosinstall]
sudo apt install python-rosinstall python-rosinstall-generator python-wstool build-essential       # ubuntu 18.04
sudo apt install python3-rosinstall python3-rosinstall-generator python3-wstool build-essential    # ubuntu 20.04
7.[安装检查]
ROS的默认安装路径为/opt/ros/noetic/，可进入该路径查看安装包是否存在。
使用基本命令检查ROS是否安装成功。以下三条命令分别打开一个终端进行运行。执行中很多报错依赖包没安装，参考博客原文or百度安装即可。
roscore                               # 启动roscore
rosrun turtlesim turtlesim_node       # 启动小海龟仿真器
rosrun turtlesim turtle_teleop_key    # 启动海龟控制节点

          然后是两个utils的安装，这之前有依赖和环境搭建：

1
2
3
4
5
6
7
8
9
10
1.[安装依赖项]
sudo apt-get install libdw-dev
2.[创建ros工作空间]
mkdir -p ~/Documents/catkin_ws/src
cd ~/Documents/catkin_ws/src
catkin_init_workspace
cd ~/Documents/catkin_ws/
catkin_make
source ~/Documents/catkin_ws/devel/setup.bash

          以下2个包，注意：先编译code_utils，再编译imu_utils，不能放在一起编译，imu_utils依赖code_utils。先把code_utils放在工作空间src下编译。再将imu_utils放到src下编译。

2.core_utils安装
1
2
3
4
cd ~/Documents/catkin_ws/src
git clone https://github.com/gaowenliang/code_utils.git
cd ~/Documents/catkin_ws/
catkin_make

          报错 fatal error: backward.hpp: 没有那个文件或目录，在code_utils下面找到sumpixel_test.cpp，修改#include “backward.hpp”为#include “code_utils/backward.hpp”后再catkin_make。

3.imu_utils安装
1
2
3
4
5
cd ~/Documents/atkin_ws/src
git clone https://github.com/gaowenliang/imu_utils.git
cd ~/Documents/catkin_ws/
catkin_make
source ~/Documents/catkin_ws/devel/setup.bash
4.标定工具Kalibr

官网：https://github.com/ethz-asl/kalibr
安装文档：https://github.com/ethz-asl/kalibr/wiki/installation

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
1.[安装依赖项]
sudo apt-get install -y 
git wget autoconf automake nano 
libeigen3-dev libboost-all-dev libsuitesparse-dev 
doxygen libopencv-dev 
libpoco-dev libtbb-dev libblas-dev liblapack-dev libv4l-dev
2.[安装python包]
sudo apt-get install -y python3-dev python3-pip python3-scipy 
python3-matplotlib ipython3 python3-wxgtk4.0 python3-tk python3-igraph
3.[建立工作空间]
mkdir -p ~/文档/catkin_ws/src
cd ~/文档/catkin_ws/
export ROS1_DISTRO=noetic     #--kinetic=16.04, melodic=18.04, noetic=20.04
source /opt/ros/$ROS1_DISTRO/setup.bash
catkin init
catkin config --extend /opt/ros/$ROS1_DISTRO
catkin config --merge-devel   #--Necessary for catkin_tools>=0.4.
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
4.[源码安装]
cd ~/文档/catkin_ws/src
git clone https://github.com/ethz-asl/kalibr.git
cd ~/文档/catkin_ws/
catkin build -DCMAKE_BUILD_TYPE=Release -j4
5.[运行]
source ~/Documents/catkin_ws/devel/setup.bash
rosrun kalibr <command_you_want_to_run_here>

四、双目相机标定

          利用Kalibr工具进行双目标定。

1.创建新ros空间

          已如上述说明。

2.标定板制作

          下载链接，包含三种类型标定板(Aprilgrid, Checkerboard, Circlegrid)，其中Aprilgrid含序号，能防止计算位姿时跳帧，故用Aprilgrid标定。

3.示例

          下载链接同上，含多相机标定(Multiple camera calibration)、IMU+相机联合标定(IMU-camera calibration)，此处先用前者标定相机，包括：相机参数文件camchain.yaml、标定结果文件target.yaml、录制数据集.bag文件。

4.录制bag数据集

          为了之后imu+相机联合标定，此处采集双目+imu的数据。若已有通过ros发布image和imu消息节点，只需用rosbag record工具将拍摄到的标定板图像制作成.bag文件即可。注意默认设备采集的频率为20~60Hz，会使标定图像过多，计算量太大。采集时最好将ros topic频率降低到4Hz左右。
          ros可更改topic发布频率的节点throttle，分别打开两个终端，运行以下命令：(其中原始话题名称：/mynteye/left/image_raw、/mynteye/right/image_raw，更改频率后话题名称：/left、/right)

1
2
3
rosrun
rosrun topic_tools throttle messages /mynteye/left/image_raw 4.0 /left
rosrun topic_tools throttle messages /mynteye/right/image_raw 4.0 /right

          然后录制bag，参考：Kalibr标定视频教程。

1
rosbag record -O stereo_calibra.bag /left /right /imu

5.进行标定

          进入下载示例Multiple camera calibration文件夹，修改camchain.yaml文件：话题名称、频率、标定板尺寸。（详见视频教程）
          打开终端输入命令：

1
2
source devel/setup.bash
rosrun kalibr kalibr_calibrate_cameras --bag /home/admin/stereo_calibra.bag --topics /left /right --models pinhole-equi pinhole-equi --target /home/admin/april_6x6_80x80cm_A0.yaml

6.标定结果

          标定完成后，生成cam-chain.yaml文件，包含：相机内参、畸变参数、基线等。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
cam0:
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [0.2783471016021346, 0.6964790605940689, -1.7636165117280103, 1.9185066948675424]
  distortion_model: equidistant
  intrinsics: [464.46660755800724, 465.36764628863784, 345.1018538803555, 229.48918054311704]
  resolution: [640, 480]
  rostopic: /left
cam1:
  T_cn_cnm1:
  - [0.9996465213666637, -0.02537787899957037, 0.007924366031799834, -0.11251195025612559]
  - [0.02547649136646326, 0.9995959921960228, -0.012601617884125324, -0.0017042839059835384]
  - [-0.0076013621922192826, 0.012799048524251695, 0.9998891956860474, -0.0005072054140188229]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [0.2682739040565608, 0.7579325851647005, -2.0708675842774156,
    2.3962970091118696]
  distortion_model: equidistant
  intrinsics: [461.8506067496413, 462.2467047389745, 325.2758830849641, 247.10152372631737]
  resolution: [640, 480]
  rostopic: /right

figure-stereo-reprojection-errors

图1 双目标定的重投影误差
五、IMU标定
1.录制imu.bag

          此处采用imu_utils进行标定。保持imu静止不动至少2小时，录制imu数据集.bag。

1
rosbag record /imu -O imu

2.修改launch文件

          自定义可选，修改src/imu_utils-master/launch文件，包括名字、时长之类。(比如: mynt_imu.launch)。

1
2
3
4
5
6
7
8
9
<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value="/imu0"/>
        <param name="imu_name" type="string" value="16448"/>
        <param name="data_save_path" type="string" value="$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value="120"/>
        <param name="max_cluster" type="int" value="100"/>
    </node>
</launch>

3.进行标定
1
2
roscore
rosbag play -r 200　imu_utils/imu.bag    # 播放bag文件:以200速度播放,而非实际等待时长2h
1
2
3
cd imu_ws
source ./devel/setup.bash
roslaunch imu_utils mynt_imu.launch      # 运行launch文件
4.标定结果

          标定结束，生成cam-imu.yaml文件，在data目录: src/imu_utils/data/m210_imu_param.yaml，包含：各方向的随机噪声、随机游走。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
%YAML:1.0
---
type: IMU
name: m210
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 1.1016440992489866e-03
      gyr_w: 5.7968344392887427e-06
   x-axis:
      gyr_n: 1.2601308952358316e-03
      gyr_w: 6.6549144466689008e-06
   y-axis:
      gyr_n: 9.1639862355727669e-04
      gyr_w: 4.5920115983527386e-06
   z-axis:
      gyr_n: 1.1284027789538512e-03
      gyr_w: 6.1435772728445881e-06
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 5.1380013730273288e-02
      acc_w: 1.7544697355051581e-03
   x-axis:
      acc_n: 3.1145378555559291e-02
      acc_w: 2.9961069437766212e-03
   y-axis:
      acc_n: 7.0380818146931270e-02
      acc_w: 1.9377334256450410e-03
   z-axis:
      acc_n: 5.2613844488329294e-02
      acc_w: 3.2956883709381275e-04

六、双目+IMU联合标定

          目标：获取相机内参到imu的相对转换矩阵。
          与kalibr标定双目过程类似，这次在IMU-camera calibration例子基础上，标定imu与camera。首先以上两次标定结果修改.yaml参数，包括：标定结果、话题类型、频率等。

1
kalibr_calibrate_imu_camera --target april_6x6.yaml --cam camchain.yaml --imu imu_adis16448.yaml --bag /home/admin/calib/stereo/imu_stereo.bag --bag-from-to 5 45

标定结果

          .txt文件中的部分结果，包含相机与imu之间的转换矩阵。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0):     mean 0.126389437034, median 0.117608381877, std: 0.0673478862055
Reprojection error (cam1):     mean 0.132576172002, median 0.12130080264, std: 0.0739372479366
Gyroscope error (imu0):        mean 0.000146638755352, median 3.16696236165e-07, std: 0.0014737840839
Accelerometer error (imu0):    mean 1.14000452874e-06, median 1.02723705118e-08, std: 6.0143590628e-06
Residuals
----------------------------
Reprojection error (cam0) [px]:     mean 0.126389437034, median 0.117608381877, std: 0.0673478862055
Reprojection error (cam1) [px]:     mean 0.132576172002, median 0.12130080264, std: 0.0739372479366
Gyroscope error (imu0) [rad/s]:     mean 5.18446291471e-06, median 1.11969028084e-08, std: 5.21061359864e-05
Accelerometer error (imu0) [m/s^2]: mean 8.06104932854e-08, median 7.26366284774e-10, std: 4.2527940778e-07
Transformation (cam0):
-----------------------
T_ci:  (imu0 to cam0): 
[[-0.02093225 -0.99971259  0.01168657 -0.00021865]
 [ 0.13518962 -0.01441203 -0.99071492 -0.00086208]
 [ 0.99059861 -0.01915798  0.13545244  0.00014753]
 [ 0.          0.          0.          1.        ]]
T_ic:  (cam0 to imu0): 
[[-0.02093225  0.13518962  0.99059861 -0.00003417]
 [-0.99971259 -0.01441203 -0.01915798 -0.00022818]
 [ 0.01168657 -0.99071492  0.13545244 -0.00087151]
 [ 0.          0.          0.          1.        ]]
timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
0.000336885304337

figure-stereo-imu-reprojection-errors

图2 双目+imu联合标定的重投影误差
七、参考文献

[1] Kalibr官网：https://github.com/ethz-asl/kalibr/wiki/
[2] Kalibr源码：https://github.com/ethz-asl/kalibr/
[3] Kalibr标定教程：https://www.youtube.com/watch?app=desktop&v=puNXsnrYWTY/