[论文阅读][SLAM]Targetless Calibration of LiDAR-IMU System Based on Continuous-time Batch Estimation
Targetless Calibration of LiDAR-IMU System Based on Continuous-time Batch Estimation
文章目录
- Targetless Calibration of LiDAR-IMU System Based on Continuous-time Batch Estimation
-
- Abstract
- Introduction
- Related Work
- Trajectory Representation AND Notation
- Methodology
-
- Step1:外参Roation的初始化
- Step2：Surfels Map and Data Association
- Step3：Continuous-time Batch Optimization
- Step4: Refinement
- Experienments
-
- Simulation
- Real-world
原文链接: https://arxiv.org/pdf/2007.14759v1.pdf
代码地址https://github.com/APRIL-ZJU/lidar_IMU_calib
Abstract
这篇文章提出了一种LiDAR-IMU标定方法(称为LI-Calib). LI-Calib 采用了基于B-Spline的连续时间轨迹。相比于基于离散时间的方法，这种方法对于IMU和LiDAR这种高采样率的传感器更合适。此外， LI-Calib decomposes the space into cells and identifies the planar segments for data association，这使得标定问题在没有任何人工target的场景下仍然是well-constrained的（？？？？？这说了个啥）。在仿真和真实数据上做了验证，结果显示该方法精度高，可重复性好。
Introduction
LiDAR这种传感器由于其在精度和鲁棒性方面的优良性质，被广泛应用于定位、建图、跟踪、检测等任务。但其缺点在于，LiDAR对连续的3D点在不同时间点进行采样，因此传感器的运动会引入畸变，比如rolling-shutter effect. IMU可以作为辅助传感器解决这个问题。
对于LiDAR，每个点都是在不同时刻采集的，每个点的精确位置同采集时刻LiDAR的位姿是有关系的; 因此做Pose Estimation对于移除运动畸变是有益的; 基于离散时间的方法[6]使用的是关键帧的位姿插值，牺牲了高动态场景下的精度。Gentil等[7]引入了高斯过程来做IMU的插值，部分的解决了使用low-frequency IMU readings为higher-frequency LiDAR points做插值的矛盾。Rehder等[8]提出了基于continuous-time batch estimation，用于标定camera-IMU-LiDAR system的两阶段方法。先使用棋盘格标定camera-IMU，然后单线激光雷达再同camera-IMU 系统做标定。
受[8]的启发，本文提出了基于continuous-time framework的方法，该法可以看作[9]的工作的延伸。通过使用temporal basis fuctions对轨迹参数化，基于continuous-time的方法可以得到轨迹上任何时间点的精确姿态，这对于高帧率测量系统的标定问题是很有帮助的。同时由于角速度和线加速度是temporal basis fuctions的导数，continuous-time formulation 将IMU的测量以一种自然的方式整合了进来（？？？？？）
总结下本文的贡献有三个：
- 提出了一种精确的、可重复性高的LiDAR-IMU标定系统，无需其他sensor或人工设计的target的参与
- 基于continuous-time trajectory， 提出了LiDAR-IMU标定问题的一种新的formulation，在continuous-time batch optimization的过程中，IMU原始数据的误差和LiDAR的point-to-surfel distances（雷达的某种误差？？？？？）被联合优化，这使得标定问题在没有任何人工target的场景下仍然是well-constrained的
- 在仿真和真实数据上做了验证，结果显示该方法精度高，可重复性好。代码开源（作者声称这是全世界首个开源LiDAR-IMU标定工具包）
Related Work
Motion compensation（运动补偿）和 data association（指点云配准吗？？？？？） 是LiDAR-IMU标定的两个主要难题。
针对运动补偿问题，大多数方法[6,10] 对每个LiDAR 点采用线性插值，其前提假设是在短时间内，角速度和线速度或线加速度是一个常量。这个假设对于低速和平滑运动是成立的。但是对于任意运动（比如手持sensor或将sensor置于四旋翼无人机）则不成立。为解决该问题，Neuhaus[11]使用IMU数据来预测LiDAR的运动。本文和最近的一些工作使用continuous-time representations 来参数化轨迹，好处是无需任何关于运动的先验，即可解决高帧率的运动畸变问题。
LiDAR Points的Data association 是另一个重要问题，[7] [14]分别使用了point-to-point和point-to-plane的方法。[15]则使用了一种point-to-model的策略，考虑了局部邻域的形状信息。本文采用了point-to-surfel的方法，点云被离散化，并用surfels（面元，关于面元与面元地图的代表性工作见SUMA )表示;points和对应的surfels对齐。
基于temporal basis functions的continuous-time batch optimization在标定问题上的应用已被广泛研究。Furgale[18]基于B样条基函数，给出了SLAM问题的证明和实现；并将所提出的框架用于解决Camera-IMU标定问题，该框架对temporal和spatial calibration（？？？？？啥区别）都支持。Rehder在[8]中使用了一个类似的框架来标定camera-IMU和LiDAR的外参，在[20]中，拓展了[18]中的工作以支持多IMU标定。本文提出了一种基于continuous-time batch optimization的LiDAR-IMU标定方法，和[8]的工作在精神上相通，但不需要棋盘格和辅助传感器。
Trajectory Representation AND Notation
符号定义：
- IMU Frame： { I } \{I\} {I}
- LiDAR Frame: { L } \{L\} {L}
- Map Frame: { M } \{M\} {M} located at the first LiDAR scan { L 0 } \{L_0\} {L0} at the beginning of calibration
- L I q , I p L { }_{L}^{I} \boldsymbol{q},{ }^{I} \boldsymbol{p}_{L} LIq,IpL 定义了从LiDAR到IMU的外参， L I R ∈ S O ( 3 ) { }_{L}^{I} \boldsymbol{R} \in S O(3) LIR∈SO(3) 是与 L I q { }_{L}^{I} \boldsymbol{q} LIq对应的 旋转矩阵
使用B-Spline 来做轨迹的参数化，优点是：
- 可计算其封闭形式的导数，这可以简化高采样信息融合（？？？？？怎么简化）
- B-Spline修改一个控制点，仅会影响其附近的曲线段（ 详见 Bezier和B样条曲线)， 这意味着我们所需要的控制点数目是有限的
使用B-Spline 来做轨迹参数化的方式有多种。可以直接在 S E ( 3 ) SE(3) SE(3)中做，也可以分别在 R 3 R_3 R3和 S O ( 3 ) SO(3) SO(3)中做。直接在 S E ( 3 ) SE(3) SE(3)中做，优点是 torque-minimal(???), 但是translation curve的形状很难控制，因为translation和rotation耦合在一起了。而且该形式对于本文的标定方法来说很难求解，因此最终选择了分别参数化的方法。
接下来介绍了B-Spline的矩阵表示形式。本文采用节点均匀分布的三次B-Spline ，并且是cumulative representation of B-spline，用来参数化translation和rotation。（可参考Spline fusion）
translation：
p ( t ) = ∑ j = 0 d u ⊤ M ( j ) ( d + 1 ) p i + j ( 1 ) \boldsymbol{p}(t)=\sum_{j=0}^{d} \boldsymbol{u}^{\top} \boldsymbol{M}_{(j)}^{(d+1)} \boldsymbol{p}_{i+j} \qquad (1) p(t)=j=0∑du⊤M(j)(d+1)pi+j(1)
rotation(详见A General Construction Scheme for Unit Quaternion Curves with Simple High Order Derivatives)：
p ( t ) = ∑ j = 0 d u ⊤ M ( j ) ( d + 1 ) p i + j q ( t ) = q i ⊗ ∏ j = 1 d exp ( u ⊤ M ~ ( j ) ( 4 ) log ( q i + j − 1 − 1 ⊗ q i + j ) ) \boldsymbol{p}(t)=\sum_{j=0}^{d} \boldsymbol{u}^{\top} \boldsymbol{M}_{(j)}^{(d+1)} \boldsymbol{p}_{i+j}\boldsymbol{q}(t)=\boldsymbol{q}_{i} \otimes \prod_{j=1}^{d} \exp \left(\boldsymbol{u}^{\top} \tilde{\boldsymbol{M}}_{(j)}^{(4)} \log \left(\boldsymbol{q}_{i+j-1}^{-1} \otimes \boldsymbol{q}_{i+j}\right)\right) p(t)=j=0∑du⊤M(j)(d+1)pi+jq(t)=qi⊗j=1∏dexp(u⊤M~(j)(4)log(qi+j−1−1⊗qi+j))
轨迹对时间 t t t求导，可得线加速度和角加速度。（？？？？？咋求的要看下）
Methodology
Step1:外参Roation的初始化
感觉就是一个手眼标定的过程，最后求解一个 A X = X B AX=XB AX=XB形式的问题。
-
IMU的姿态不是通过积分得到的（缺点是不准确，而且受到IMU的噪声和漂移影响），而是通过上文提到的B-Spline Fitting得到的，具体是求解下面这个最小二乘问题(要细看???):
q 0 , ⋯ , q N = arg min ∑ k = 0 M ∥ I k ω m − I I 0 R ⊤ ( t k ) I I 0 R ˙ ( t k ) ∥ \boldsymbol{q}_{0}, \cdots, \boldsymbol{q}_{N}=\arg \min \sum_{k=0}^{M}\left\|^{I_{k}} \boldsymbol{\omega}_{m}-{ }_{I}^{I_{0}} \boldsymbol{R}^{\top}\left(t_{k}\right)_{I}^{I_{0}} \dot{\boldsymbol{R}}\left(t_{k}\right)\right\| q0,⋯,qN=argmink=0∑M∥∥∥Ikωm−II0R⊤(tk)II0R˙(tk)∥∥∥ -
[ t k , t k + 1 ] [t_k , t_{k+1} ] [tk,tk+1]时间段内的IMU rotation可通过 上步的B-Spline得到，LiDAD的rotation可以通过NDT得到。任意 k k k时刻两个sensor之间的rotation需满足以下关系：
q ⊗ L I q = L I q ⊗ L k + 1 L k q \boldsymbol{q} \otimes_{L}^{I} \boldsymbol{q}={ }_{L}^{I} \boldsymbol{q} \otimes{ }_{L_{k+1}}^{L_{k}} \boldsymbol{q} q⊗LIq=LIq⊗Lk+1Lkq
将不同时刻的方程Stack起来求解即可。（这里方程加了个权重要细看？？？？？） -
作者专门解释了下为什么没有做translation的初始化：
- 加速度与重力耦合，并与方向相关。因此，rotation B-Spline 会影响translation B-Spline的精度
- B-Spline的二阶导数在式 ( 1 ) (1) (1)的 u u u向量中引入了两个零元素，使得控制点无法求解 (???以上两点没看懂)
- 使用IMU的raw data 来计算B-Spline是不准确的
虽然没有做translation的初始化，从实验结果来看这对Calibration的结果影响很小。
Step2：Surfels Map and Data Association
使用Step1估计出的Rotation，可以为Lidar帧间配准提供Rotation的初值，还可以移除Rotation引起的畸变，因此可以提升lidar odometry和lidar point cloud map的质量。将lidar point cloud map的点云离散化在3D cells中，每个cells内的点计算一阶和二阶矩（？？？？？就是重心和协方差矩阵吧）。然后使用了一个指标来找Surfels[32]：
P = 2 λ 1 − λ 0 λ 0 + λ 1 + λ 2 , P=2 \frac {\lambda _ {1}-\lambda _ {0}}{\lambda _ {0}+\lambda _ {1}+\lambda _ {2}} , P=2λ0+λ1+λ2λ1−λ0,
λ 0 , λ 1 , λ 2 \lambda _ {0},\lambda _ {1},\lambda _ {2} λ0,λ1,λ2是协方差矩阵的特征值，从小到大排列。当cell中的点接近一个平面的时候， P P P接近1（ λ 0 \lambda _ {0} λ0接近0, 另两个接近相等）。如果一个cell中的 P P P值大于某个阈值，则使用RANSAC拟合平面，并用该平面到原点的距离和法向表示该平面 π = [ n ⊤ , d ] ⊤ \pi=\left[\boldsymbol{n}^{\top}, d\right]^{\top} π=[n⊤,d]⊤。
所有的平面和Lidar point cloud map都是在 L 0 L_0 L0 系的.
使用tiny planes， 而不是环境中的大平面，好处是可以使用环境中所有的平面状区域提供的信息，为batch optimization 提供了更可靠的约束（？？？？？）。为了鲁棒性，距离超出阈值的point-to-plane correspondences 被拒绝（？？？？？有提到这些correspondences的作用吗）。
点云使用了random sample作降采样。
Step3：Continuous-time Batch Optimization
总结下，需要估计的系统状态为：
x = [ L I q ⊤ , I p L ⊤ , x q ⊤ , x p ⊤ , b a ⊤ , b a ⊤ ] ⊤ \boldsymbol{x}=\left[{ }_{L}^{I} \boldsymbol{q}^{\top},{ }^{I} \boldsymbol{p}_{L}^{\top}, \boldsymbol{x}_{q}^{\top}, \boldsymbol{x}_{p}^{\top}, \boldsymbol{b}_{a}^{\top}, \boldsymbol{b}_{a}^{\top}\right]^{\top} x=[LIq⊤,IpL⊤,xq⊤,xp⊤,ba⊤,ba⊤]⊤
其中 x q = [ q 0 ⊤ , ⋯ , q N ⊤ ] ⊤ \boldsymbol{x}_{q}=\left[\boldsymbol{q}_{0}^{\top}, \cdots, \boldsymbol{q}_{N}^{\top}\right]^{\top} xq=[q0⊤,⋯,qN⊤]⊤ and x p = [ p 0 ⊤ , ⋯ , p N ⊤ ] ⊤ \boldsymbol{x}_{p}=\left[\boldsymbol{p}_{0}^{\top}, \cdots, \boldsymbol{p}_{N}^{\top}\right]^{\top} xp=[p0⊤,⋯,pN⊤]⊤为Trajectory Representation 一节中提到的continuous-time trajectory的rotation和translation的控制点。
问题可以定义为：
有以下测量：
sequence of
- associated LiDAR points L \mathcal{L} L
- linear acceleration measurements A \mathcal{A} A
- angular velocity measurements W \mathcal{W} W
以上测量的噪声项都是独立的高斯白噪声。
求极大似然估计 p ( x ∣ L , A , W ) p(\boldsymbol{x} \mid \mathcal{L}, \mathcal{A}, \mathcal{W}) p(x∣L,A,W)
可被以下最小二乘问题近似：
x ^ = arg min { ∑ k ∈ A ∥ r a k ∥ Σ a 2 + ∑ k ∈ W ∥ r ω k ∥ Σ ω 2 + ∑ j ∈ L ∥ r L j ∥ Σ L 2 } \hat{\boldsymbol{x}}=\arg \min \left\{\sum_{k \in \mathcal{A}}\left\|\boldsymbol{r}_{a}^{k}\right\|_{\Sigma_{a}}^{2}+\sum_{k \in \mathcal{W}}\left\|\boldsymbol{r}_{\omega}^{k}\right\|_{\Sigma_{\omega}}^{2}+\sum_{j \in \mathcal{L}}\left\|\boldsymbol{r}_{\mathcal{L}}^{j}\right\|_{\Sigma_{\mathcal{L}}}^{2}\right\} x^=argmin⎩⎨⎧k∈A∑∥∥rak∥∥Σa2+k∈W∑∥∥rωk∥∥Σω2+j∈L∑∥∥∥rLj∥∥∥ΣL2⎭⎬⎫
其中，IMU残差定义：
r a k = I k a m − I a ( t k ) − b a r ω k = I k ω m − I ω ( t k ) − b g \boldsymbol{r}_{a}^{k}={ }^{I_{k}} \boldsymbol{a}_{m}-{ }^{I} \boldsymbol{a}\left(t_{k}\right)-\boldsymbol{b}_{a}\quad\boldsymbol{r}_{\omega}^{k}={ }^{I_{k}} \boldsymbol{\omega}_{m}-{ }^{I} \boldsymbol{\omega}\left(t_{k}\right)-\boldsymbol{b}_{g} rak=Ikam−Ia(tk)−barωk=Ikωm−Iω(tk)−bg
激光雷达的残差计算方式，大概就是把每个点转到 L 0 L_0 L0 系，然后计算和Step2中的面元的距离。类似一个point2plane error。
Step4: Refinement
Batch optimization之后，使用 x \boldsymbol{x} x的最佳估计再做一遍去畸变，rebuild lidar sufels map和point-to-surfel data associations。注意：NDT只在第一次迭代中做。实验中大概4次迭代可以得到一个精度较高的标定结果。
Experienments
simulated和real world data 都被拿来做实验。Continuous-time batch optimization 是使用自己写的Kontiki 库来做的。
Simulation
模拟生成10组数据，（均为约10s的数据），加入高斯噪声（特性与真实传感器噪声特性相同）。结果是：translational errors： 0.0043 ± 0.0006m， orientational errors ： 0.0224 ± 0.0026度。
另外，在vicon room 中测量了trajectory的精确度。
Real-world
室内和室外场景，每个场景5组数据（25-30s，角速度47.39度 /s，加速度0.628 m / s 2 m/s^2 m/s2）。外参真值由CAD中的测量得到。
对数据的要求：所有的场景要保证足够的线加速度和角加速度， knot spacing
of the spline 是0.02s。
作者使用图表展示了以下内容：
- 针对不同数据，结果的一致性
- BSpline trajectory error
- 多次迭代结果的收敛性
一些结论：
- 在室内场景，平面区域更多，结果收敛性的更好；一致性也更好（误差在mm，零点几度这个量级）。
- 方法当前的局限在于初次迭代时使用了NDT，NDT结果如果很差，会无法得到enough reliable point-to-surfel correspondences,标定结果也会很差。希望未来可以有更鲁棒的lidar 前端。