LR: ICRA 2020: Spatiotemporal Camera-LiDAR Calibration: A Targetless and Structureless Approach

Abstract

我们提出了一个targetless和structureless的时空 相机-lidar 标定方法.

我们的方法结合了一个闭式解和modified structureless BA, 用了coarse-to-fine的方法, 且不需要一个在时空参数上的猜测.

因为3D特征(structure)是只从三角化计算而来, 不需要有一个标定目标, 或者是用2D特征和3D点云匹配.

1. Introduction

时间同步 经常被忽视, time lag / delay的影响都是很大的在手持中, 因为角速度一般都很高.

第一阶段: 提供了闭式解对于外参和大致的时间延迟.

第二阶段: 时空参数被refine, 通过structureless 持续时间的 SfM 模型.

2. Related Work

[1] 利用了投影3D线段到2D图在室内环境.

有很多使用互信息的. 互信息算力cost太大.

Temporal Calibration 比外参标定需要考虑不同步.

3. Overview

每个轨迹的时间戳通过检测sensory开始动先大致同步一下. 第二部就是减少3D-2D投影误差来refine.

4. Proposed Method

A. Continuous-Time Trajectory Representation

\[\mathbf{T}(\tau):=\left[\begin{array}{cc} \mathbf{R}(\tau) & \mathbf{t}(\tau) \\ \mathbf{0} & 1 \end{array}\right] \]

最简单的model 连续时间轨迹的办法就是线性插值 on the manifold. 给定两个pose \(T_i, T_j\) 在时间点 \(\tau_i, \tau_j\). 在时间\(\tau \in [\tau_i, \tau_j]\) 可以插值为:

\[\mathbf{T}(\tau)=\mathbf{T}_{i} e^{\alpha[\xi] \times} \]

这里 \([\boldsymbol{\xi}]_{\times}=\log \left(\mathbf{T}_{i}^{-1} \mathbf{T}_{j}\right)\), 然后插值的ratio是 \(\alpha = (\tau - \tau_i) / (\tau_j - \tau_i)\).

B. Coarse camera-LiDAR Extrinsic Parameter Estimation

利用每个传感器的移动来寻找粗略的外参, 有LiDAR odometry 和 视觉odometry.

在coarse的阶段, 我们假设两个轨迹是大致同步的.

给定update to scale的相机pose, 计算相对pose:

\[^{C_{i}} \mathbf{T}_{C_{j}}:= \left[ \begin{array}{cc} \mathbf{R}_{C_{i}} & \lambda \mathbf{t}_{C_{i}} \\ \mathbf{0} & 1 \end{array}\right]. \]

这里 \(\lambda > 0\) 是给单目尺度不确定的.

对于LiDAR来说, 相对pose是:

\[^{L_{i}} \mathbf{T}_{L_{j}}=\left(^{w} \mathbf{T}_{L}\left(\tau_{i}\right)\right)^{-1} w_{L}\left(\tau_{j}\right):=\left[\begin{array}{cc} \mathbf{R}_{L_{i}} & \mathbf{t}_{L_{i}} \\ \mathbf{0} & 1 \end{array}\right] \]

这样的话, 相对位姿约束就成了: 手眼标定 \(AX=XB\)

\[^{L_{i}} \mathbf{T}_{L_{j}}^{L} \mathbf{T}_{C}=^{L} \mathbf{T}_{C}^{C_{i}} \mathbf{T}_{C_{j}} \]

把旋转和平移分开结算:

\[\begin{array}{c} \mathbf{R}_{L} \mathbf{R}=\mathbf{R} \mathbf{R}_{C} \\ \left(\mathbf{I}-\mathbf{R}_{L}\right) \mathbf{t}+\lambda \mathbf{R} \mathbf{t}_{C}=\mathbf{t}_{L} \end{array} \]

旋转的解可以用aligning correspondenecs in the manifold. 让 \(\mathbf{R}_{L_{k}}=e^{\left[\mathbf{r}_{L}\right]}, \mathbf{R}_{C_{k}}=e^{\left[\mathbf{r}_{C}\right]_{\times}}\).

它的协方差是:

\[\mathbf{M}=\sum_{i}^{k} \mathbf{r}_{L_{i}} \mathbf{r}_{C_{i}}^{\top} \]

通过SVD, 旋转矩阵可以用分解协方差矩阵得到:

\[\mathbf{R}=\left(\mathbf{M}^{\top} \mathbf{M}\right)^{-1 / 2} \mathbf{M}^{\top} \]

有了旋转之后, 平移和尺度因子可以用解线性方程得到:

\[\left[\begin{array}{cc} \left(\mathbf{I}-\mathbf{R}_{L_{1}}\right) & \mathbf{R} \mathbf{t}_{C_{1}} \\ \vdots & \vdots \\ \left(\mathbf{I}-\mathbf{R}_{L_{k}}\right) & \mathbf{R} \mathbf{t}_{C_{k}} \end{array}\right]\left[\begin{array}{l} \mathbf{t} \\ \lambda \end{array}\right]=\left[\begin{array}{c} \mathbf{t}_{L_{1}} \\ \vdots \\ \mathbf{t}_{L_{k}} \end{array}\right] \]

注意至少需要3组匹配来找到唯一解. 而且有足够的roll, pitch, yaw移动也是重要的.

C. Refined Extrinsic and Time Lag Estimation

闭式解, 粗糙的外参估计减少了 algebraic distance 而不是 几何误差. 所以我们会用time lag 估计, 使用非线性优化来refine.

视觉特征 $ ^wp_j$ 的3D位置是用2D图像特征三角花得到的. 那么第 j 个特征的位置在第 i 个图像 \(^{I_i} p_j\) 和它的重投影误差就是:

\[\mathbf{e}_{i j}=^{I_{i}} \mathbf{p}_{j}-\pi\left(^{C} \mathbf{T}_{L}^{L} \mathbf{T}_{W}\left(\tau_{i}+\tau\right)^{w} \mathbf{p}_{j}\right) \]

这里 \(^CT_L := e^{[\xi]_\times}\) 是外参, \(\tau\) 表示LiDAR的时间延迟. \(\pi (.)\) 是相机投影函数.

我们找到最优参数 \(\mathbf{x}=\left(\boldsymbol{\xi}, \tau,^{w} \mathbf{p}\right)\) 最小化了下述对象函数:

\[\mathbf{f}(\mathbf{x})=\frac{1}{2} \sum_{i} \sum_{j} \mathbf{e}_{i j}^{\top} \mathbf{\Sigma}_{i j}^{-1} \mathbf{e}_{i j} \]

这里 \(\Sigma_{ij}^{-1}\) 表示M-estimator的权重矩阵.

用了高斯牛顿:

\[\mathbf{H}=\mathbf{J}^{\top} \mathbf{\Sigma}^{-1} \mathbf{J}, \quad \mathbf{g}=\mathbf{J}^{\top} \mathbf{\Sigma}^{-1} \mathbf{b} \]

增量是:

\[\delta x = -H ^{-1}g \]

D. Structureless Optimization Update

为了估计的高效, 我们引入了structureless approach, 我们从状态估计中边缘化了3D点 \(^wp_j\).

\[\delta \mathbf{x}=\left[\begin{array}{l} \delta \mathbf{x}_{c} \\ \delta \mathbf{x}_{s} \end{array}\right], \quad \delta \mathbf{x}_{c}=\left[\begin{array}{l} \delta \boldsymbol{\xi} \\ \delta \tau \end{array}\right], \quad \delta \mathbf{x}_{s}=\left[\begin{array}{c} \delta^{w} \mathbf{p}_{1} \\ \cdots \\ \delta^{w} \mathbf{p}_{J} \end{array}\right] \]

这里 \(\delta x_c\) 是外参和时间延迟, \(\delta x_s\) 是3D点集, 要被边缘化的.

高斯消元:

\[\left[\begin{array}{ll} \mathbf{H}_{c c} & \mathbf{H}_{c s} \\ \mathbf{H}_{s c} & \mathbf{H}_{s s} \end{array}\right]\left[\begin{array}{l} \delta \mathbf{x}_{c} \\ \delta \mathbf{x}_{s} \end{array}\right]=-\left[\begin{array}{l} \mathbf{g}_{c} \\ \mathbf{g}_{s} \end{array}\right] \]

5. Experiments

A. Experiment Setup

B. Closed-From Solution in the Coarse Stage

上图实验表示最统治的成分是相对位姿的motion, 但是在大于30°之后也不会提升了. 在大于15个samples之后也不会提升旋转参数估计的精度.

我们假设LiDAR移动distortion已经在里程计估计中被补偿了[2].

上表展示了同步误差如何影响.

C. Extrinsic and Time Lag Estimation in the Refinement Stage

....

6. Discussion

平移估计误差在短基线的时候还是相对高. 可以假设在平移估计里有0.5 到 4cm的误差.

在coarse和refinement阶段的外参估计比较还是体现了传统的 motion-based only 方法产生相对高的不确定性在旋转估计上. 这是因为即使是很小的旋转移动在refinement阶段会被放大, 通过投影到图像上, 所以在优化阶段旋转的观测性更好.

A. Limitation and Future works

还是有一些实际的限制.

**时间延迟估计: ** 当平台是静止的时候时间延迟是不可观的, 在静止的时候时延的不确定性就提升了.

Dedicated motion: 对于legged robot, 标定更加容易. 但是对于车辆来说就比较困难了.

7. Conclusion

没啥.