AR/VR硬件的多传感器快速标定方案
摘要：本文属于AR/VR技术的系列文章，AR/VR硬件通常包含多传感器，比如相机、激光雷达、IMU等，本文主要介绍一种高效率、低成本、易部署的面向多传感器的标定方案，解决AR/VR硬件的硬件标定问题。
一、技术背景
阿里云-人工智能实验室致力于提供高精度的空间数字化解决方案，以及数字化内容在AR/VR的应用，目前已经不仅落地了云上会展、全息店铺、酒店服务机器人等业务，还在构建大规模室内外AR/VR地图等方面持续探索。 以上业务和研究均由我们自主研发的硬件支持，我们的自研硬件配备激光雷达、相机、IMU等传感器，融合多个传感器的感知能力来提升三维重建算法的精度。
为了在AR/VR应用中提供更高精度的地图，各个传感器需要在时间和空间上精确的标定，时间标定为了解决各传感器数据时间不同步问题，空间标定指的是将各个传感器数据注册到一个统一的世界坐标系。兼顾标定精度和量产可行性这两方面因素，我们设计并实现了一套多传感器快速标定方案。本文只介绍多传感器系统的空间标定即外参标定的方案。
二、问题定义
我们这里选取背包设备为例，介绍多传感器标定的定义和多传感器的坐标系统。我们自研的背包扫描设备的传感器模块由两个激光雷达、四个相机、一个惯性导航传感器(IMU)组成。每一个传感器都是以自己为中心来感知世界的，也就是说每一个传感器都会由一个属于自己的坐标系(局部坐标系): IMU坐标系为$I$, 相机坐标系为$\{C\}=\{C_0, C_1, C_2, C_3\}$和激光雷达坐标系$\{L\}=\{L_0, L_1\}$. 有如此多的坐标系是很复杂的，我们需要把这些传感器的局部坐标系统一起来，让他们都隶属于一个载体坐标系, 这里我们选取激光雷达$L_0$为载体坐标系。
坐标系统一之后，每一个传感器都应该把他们对外界环境的感知测量数据转换到载体坐标系下, 传感器之间的外参标定指的是各个传感器坐标系之间的变换矩阵。在三维世界中，这个外参变换矩阵包括一个3x3的旋转矩阵$R \in \mathbb{R}^{3x3}$和一个三维平移向量$t \in \mathbb{R}^3$.
三、多相机标定
3.1 相机内参标定
在标定多相机系统之间的外参之前，我们需要标定每个相机的内参. 相机的内参是什么？这还要从相机模型和成像原理说起。现在大多数的相机都属于针孔相机模型，成像过程可以用如图3.1-(a)的小孔成像的原理表示。
然而我们想要知道三维世界中的一个空间点如何映射到图像上的那个像素点，就需要将这个成像过程用一种数学模型来表示，这就是相机投影模型，也叫相机坐标系到图像坐标系的投影。如图3.1-(b)所示，假设已知相机坐标系下一个空间点$X=(X,Y,Z)$, 它在图像上对应的像素$p=(x,y)$, 则投影过程可以用如下公式表示:
$$\begin{bmatrix} x\\ y\\ 1 \end{bmatrix} = \begin{bmatrix}
f_x & 0 & c_x & 0\
0 & f_y & c_y & 0\
0 & 0 & 1 & 0 \
0 & 0 & 0 & 1
\end{bmatrix} \cdot \begin{bmatrix} X\\ Y\\ Z\\ 1 \end{bmatrix}$$
理想状态下空间中一点应该沿着上述成像模型映射到图像中的某个像素。但是我们忽略了相机中的一个重要零部件-镜头。镜头的一些工艺误差、安装误差是会让成像产生畸变的。比如图3.2中的建筑物轮廓被明显弯曲了。那么我们就需要对镜头建立一种合适的畸变模型，才能使物理世界中的直线也在图像上还原成直线。畸变一般分为两大类：径向畸变(radial distortion)和切向畸变(tangential distortion)。径向畸变主要由镜头在光轴上偏移导致，切向畸变主要由镜头与成像平面不平行导致。
因此，相机内参标定就是确定内参和畸变参数 (equidistqant畸变模型)或者(radial tangential模型)的过程. 详细的相机成像模型和镜头畸变模型有兴趣的同学可以参考UIUC Computer VIsion和Classical Models for the various classical lens projections.
相机内参标定已经存在很成熟的开源方案和工具，像Matlab和Opencv、Kalibr。基本思想还是沿袭张正友2000年工作[1]: 假设标定板为世界坐标系, 所有角点的空间坐标已知且Z=0, 则可以通过三张不同的图像计算出相机的内参, 进而计算出每张图像的位姿。标定板可以使用棋盘格或者Apriltag标定板，借助opencv或者kalibr工具可以很方便的完成相机内参标定工作。 
3.2 多相机外参标定
这里以双目相机为例，简介多相机外参标定。假设双目相机是完全同步的，并且每个相机的内参已知，传统的双目外参标定方案需要左右相机之间存在一定的共视区域，也就是说左右相机要能够同时观测到一个公共目标(Target object), 这个Target可以是标定板或者其他pattern的物体。因为标定板方便为每个特征角点建立世界坐标系下的空间坐标$\{X_i\}$, 我们这里还以标定板为Target。已知世界坐标系，左目和右目相机分别观测到标定板上的角点$\{x_i\}$，因此可以求解PnP问题完成两个相机在标定板坐标系中的定位(localization)。根据两个相机的定位结果，我们可以得到左目与右目之间的一个初始外参$_{left}^{right}\textrm{T}$, 然后将外参作为优化变量，通过最小化每个相机的重投影误差来得到更为精确的外参。
四、相机-IMU标定
相机-IMU标定具体指求解相机坐标系和IMU坐标系之间的外参$_{I}^{C}\textrm{T}$和时间偏移$t_d$. 外参$_{I}^{C}\textrm{T}$指的是两个坐标系之间的变换矩阵。而时间偏移$t_d$指的是我们在同一时刻采样得到的图像和IMU数据其实并非真的是同一时刻的传感器测量数据，这是因为相机和IMU器件时钟源不同步、电信号延迟等问题导致的，因此我们需要将这个时间偏移$t_d$估算出来，补偿我们实际测量得到的数据。
在标定IMU与相机之前，我们需要得到IMU的内参和双目相机的外参(如果是单目相机则只需要内参文件). 原因是IMU内参标定结果会给出IMU器件的噪声，双目(单目)相机的外参(内参)标定结果能够提供一个较好的相机模型初值，这就能够让相机-IMU标定算法能够更精确的估计外参$_{I}^{C}\textrm{T}$和时间偏移$t_d$.
4.1 IMU内参标定
所谓IMU内参标定，其实是为了分析惯性器件的误差，这包括加速剂的误差和陀螺仪的误差。以陀螺仪的误差分析为例, 主要有以下几项:
量化噪声;
角度随机游走;
角速度随机游走;
零偏不稳定性噪声;
速率斜坡;
可以看出，这些误差多为噪声，分析这些误差最直接的办法就是通过统计的方法求方差。目前最常用的误差分析方法是Allan方差分析法，我们得到的数据都是各种误差混合在一起的，这种方法可以很好的将各种误差隔离分析，使之不会互相影响。具体推导过程可以参考西工大严恭敏老师的《惯性仪器测试与数据分析》。如下图所示的是三种IMU器件的Allan方差曲线，这里的每一条曲线都是由五条误差项拟合得到的。值得庆幸的是，网上有开源的IMU误差分析工具imu_utils，这个工具会输出陀螺仪和加速计每个轴的角度/加速度噪声标准差和bias噪声方差, 这些结果在接下来的相机-IMU标定过程会被使用到。
4.2 相机-IMU标定
相机和IMU之间的标定不仅要求解两个坐标系之间的外参，还要求解两种传感器时钟系统的偏移。理想情况下，每一帧图像的时间戳$t_{cam}$应该与该时间戳下采样的imu数据$t_{imu}$在时间轴上完全对齐的，但由于电子信号传输时延、算法处理时延以及图像曝光时间等延迟，实际上$t_{cam}=t_{imu} + t_d$。这个时间偏移是固定的，我们就可以把$t_d$作为一个可估计的量添加到系统的状态变量中。
kalibr提供了一个标定相机-IMU[2]的工具，如下图所示，棋盘格标定板世界坐标系$F_w$, 相机坐标系$F_c$, IMU坐标系$F_i$。基本思想是基于CT—SLAM, 把任意时刻的相机位姿参数化为时间t的函数，这种方法有利于同时估计外参$_{C}^{I}\textrm{T}$和时间偏移$t_d$。具体的，由于标定板确定了世界坐标系，我们可以得到每一帧图像的位姿$_W^{C}\textrm{T}_{t}$，用图像位姿乘上一个初始估计外参可以得到一个IMU的位姿$_W^{I}\textrm{T}_{t}=_W^C\textrm{T}_t \cdot _C^I\textrm{T}$. 多个时刻的IMU位姿可以拟合一个B—Spline, 这就是我们要拟合的位姿关于时间的函数曲线。最终的优化是拿图像的重投影误差去优化外参$_{C}^{I}\textrm{T}$、时间偏移$t_d$、IMU器件的bias向量，这就是我们要标定的结果。
kalibr工具包在标定结束后会生成一份标定报告，报告里有陀螺仪、加速计的bias估计值的曲线，以及相机的重投影误差图表。
五、多激光雷达标定
激光雷达的标定也包括内参标定和外参标定两部分。但是现在市场上的多线激光雷达在出厂时都已经最内参做好了严格标定，用户拿到的数据就是经过标定校准后的数据，所以我们在这里不对内参标定做展开。多个激光雷达之间的外参标定方法可以分为两大类: 一类是采用标定板或其他标定媒介的标定方法，我们所使用的多激光雷达标定方案也属于此类，如下左图, 蓝色和红色的点云分别代表两个16线激光雷达a和b，它们被放置在一个提前完成建图的环境w中，每个雷达点云在这个地图中完成定位求解位姿$_w^a\textrm{T}$和$_w^b\textrm{T}$，两个雷达之间外参$T_b^a=_w^b\textrm{T}^{-1} \cdot _w^a\textrm{T}$； 一类是基于手眼标定的方法不需要特定的标定媒介，比如下右图, 雷达a和b各自运行LOAM[3], 然后通过手眼标定得到一个的闭式解$T_b^a$ 。
六、激光雷达-相机标定
激光雷达与相机之间的外参标定最近几年涌现出一大批工作，比较有代表性的包括基于标定板的[4]、[5]，基于自然场景的[6]和[7]. 值得一提的是[6]中同时估计了外参$_L^C\textrm{T}$和时间偏移$t_{d_2}$.
下面简单介绍下[4]的工作原理. 标定板放在激光雷达和相机的公式区域，图像上提取标定板的四条边，并进一步在相机坐标系中拟合标定板所在平面的法向量。在激光雷达坐标系中，我们提取出标定板轮廓并进一步拟合它的四条边和平面法向量。因此，我们可以在相机坐标系C和激光雷达坐标系L之间建立四对边对应关系$\{l_i \leftrightarrow L_i \}$和一对法向量对应关系$^C\textrm{n}_i \leftrightarrow ^L\textrm{n}_i$, 这些约束可以用来求解6Dof的$_L^C\textrm{T}$。
七、多相机-激光雷达的标定
上述方法都是两两传感器之间的外参标定，我们称之为pairwise sensor calibration. 在实际的批量生产中，需要串联这些方法来完成一个复杂的多传感器系统的标定工作，尤其是激光雷达和相机的标定工作，需要很多的人工干预工作，这无疑降低了产线的能效。所以，我们提出一种基于全景基础设施的多传感器标定方法，无需要求相机-激光雷达之间存在共视区域，也不需要拍摄多个图像-点云对，只需要一次拍摄即可完成多个相机与多个激光雷达之间的两两外参标定，极大地提高了多相机、多雷达系统的标定效率。
我们采用很多个CCTAG标志点设计并重建了一种全景基础设施。这种标志点在空间中随机分布，提供高精度、没有特征描述子的中心点，从而保证这个全景基础设施地图的精度。
我们的整体方案分为两部分：基础设施重建过程和多相机-激光雷达传感器标定过程。
在基础设施重建过程，我们采用stereo-sfm方法完成基于“特征点”的稀疏重建。与传统视觉重建不同的是，由于“特征”没有描述子，我们巧妙地利用了标志点在空间中随机分布的特性，完成了Tracking和LoopClosure Detection。通过与第三方高精度扫描仪对这个全景基础设施的扫描结果做对比，验证了我们方法的精度和可行性。
在多传感器标定过程，相机与激光雷达同时在同一个基础设施坐标系中做定位，根据定位结果可以推导两两传感器之间的外参。在相机定位中，我们首先提取每个相机图像上的所有标志点中心坐标，然后求Triangle based PnP问题得到相机在基础设施坐标系中的位姿$_W^{C_i}\textrm{T}$；不同于视觉定位过程使用的是基础设施的稀疏点云模型，我们在雷达定位中使用的是稠密的上采样后的点云模型。首先从雷达点云中提取墙角点和线，这些约束足够求解雷达在基础设施点云模型中的位姿, 然后再采用GICP进一步优化雷达位姿$_W^{L_i}\textrm{T}$。这样，每个传感器只需要一帧图像或者点云数据就完成了在全景基础设施中的定位，我们进而可以求得两两传感器之间的外参$_{L_i}^{C_i}\textrm{T} = _W^{L_i}\textrm{T}^{-1} \cdot _W^{C_i}\textrm{T}$。
上述传感器标定过程中，虽然有分别对相机/雷达的定位结果做优化，但是并没有联合优化两种传感器的位姿，原因是基于本方案很难去寻找相机与雷达之间的共同观测，这是本文工作需要提升的一个方向。
上述流程的详细算法步骤见[8], 论文在投2021 IROS with RAL, 欢迎提出您的宝贵意见！
结语
由于相机、激光雷达、IMU等传感器能够以互补的方式进行环境感知，多传感器融合技术已经成为阿里云-人工智能实验室的AR/VR业务的主流技术方案。多传感器精确标定是多传感器融合不可或缺的前提条件，却很少有同时兼顾精度与效率的相关工作。本文提出一种高效的多相机-激光雷达传感器系统的标定，可以在保证精度的前提下，只需要一帧传感器数据完成标定，满足产线上部署要求。但是本文的方案还需要提前标定相机内参、IMU内参、相机-IMU外参，雷达-相机之间的外参没有进一步的紧耦合优化，也还没有一个成熟稳定的方案标定雷达-IMU外参，这都是我们下一步工作需要主要解决的方向。
参考文献
- A Flexible New Technique for Camera Calibration.
- Unified temporal and spatial calibration for multi-sensor systems.
- Low-drift and real-time lidar odometry and mapping.
- Automatic Extrinsic Calibration of a Camera and a 3D LiDAR Using Line and Plane Correspondences.
- Automatic extrinsic calibration between a camera and a 3D Lidar using 3D point and plane correspondences.
- Spatiotemporal Camera-LiDAR Calibration: A Targetless and Structureless Approach.
- Automatic targetless extrinsic calibration of a 3D lidar and camera by maximizing mutual information.
- __Single-Shot is Enough: Panoramic Infrastructure Based Calibration of Multiple Cameras and 3D LiDARs__.
