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}( au):=left[egin{array}{cc} mathbf{R}( au) & mathbf{t}( au) \ mathbf{0} & 1 end{array} ight] ]

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

[mathbf{T}( au)=mathbf{T}_{i} e^{alpha[xi] imes} ]

这里 ([oldsymbol{xi}]_{ imes}=log left(mathbf{T}_{i}^{-1} mathbf{T}_{j} ight)), 然后插值的ratio是 (alpha = ( au - au_i) / ( au_j - au_i)).

B. Coarse camera-LiDAR Extrinsic Parameter Estimation

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

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

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

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

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

对于LiDAR来说, 相对pose是:

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

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

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

把旋转和平移分开结算:

[egin{array}{c} mathbf{R}_{L} mathbf{R}=mathbf{R} mathbf{R}_{C} \ left(mathbf{I}-mathbf{R}_{L} ight) 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} ight]}, mathbf{R}_{C_{k}}=e^{left[mathbf{r}_{C} ight]_{ imes}}).

它的协方差是:

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

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

[mathbf{R}=left(mathbf{M}^{ op} mathbf{M} ight)^{-1 / 2} mathbf{M}^{ op} ]

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

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

注意至少需要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}-pileft(^{C} mathbf{T}_{L}^{L} mathbf{T}_{W}left( au_{i}+ au ight)^{w} mathbf{p}_{j} ight) ]

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

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

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

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

用了高斯牛顿:

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

增量是:

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

D. Structureless Optimization Update

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

[delta mathbf{x}=left[egin{array}{l} delta mathbf{x}_{c} \ delta mathbf{x}_{s} end{array} ight], quad delta mathbf{x}_{c}=left[egin{array}{l} delta oldsymbol{xi} \ delta au end{array} ight], quad delta mathbf{x}_{s}=left[egin{array}{c} delta^{w} mathbf{p}_{1} \ cdots \ delta^{w} mathbf{p}_{J} end{array} ight] ]

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

高斯消元:

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

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

没啥.