激光雷达和相机的在线语义初始化与校准
论文名称: SOIC: Semantic Online Initialization and Calibration for LiDAR and Camera
作者: Weimin Wang, Shohei Nobuhara, Ryosuke Nakamura and Ken Sakurada
论文下载:https://arxiv.org/pdf/2003.04260v1.pdf
论文代码:https://github.com/−−/SOIC
本文的核心思想
文章提出了一种基于语义信息的雷达和相机的标定方法. 以往的在线标定方法需要先验信息作为算法的初始值,我们通过引入语义质心(SCs)将初始化问题转化为PnP问题,从而消除了这种局限性.尽管现有的工作已经给出了PnP问题的解析解, 但是由于点云的质心通常与其对应图像的质心不匹配, 即使经过非线性化处理, 参数的精度也无法得到提高.我们基于点云和图像数据的语义元素之前的对应关系, 构造了一个代价函数, 通过最小化代价函数来估计最优的外部参数. 并在KITTI数据集上对算法进行了评估.
研究背景
雷达传感器能够稳定地获取物体的空间数据,但是分辨率低, 无法获得物体的颜色信息; 而相机传感器能够获得高分辨率的RGB图像,但是其对光照敏感同时也无法得到距离信息. 当代移动机器人和无人驾驶汽车通过激光雷达与相机传感器的信息融合,来弥补彼此的不足. MV3D[1], AVOD[2],F-PointNet[3]等神经网络利用融合过后的信息来提高物体检测和语义分割任务的性能. 准确的外参标定是进行融合的前提条件,通常是第一步,也是最重要的一步.
在过去的几年中, 研究人员提出了许多激光雷达与相机的标定方法, 传统方法使用棋盘格标定法, 需要人工从点云和图像中选择特征之间的对应关系. 有些学者提出了更为便利的方法, 可以自动检测特征之间的匹配关系.为了提高灵活性, 在线无目标检测方法得到广泛使用,一种方法是基于观测数据, 利用观测到的点云和图像数据之间的强度或边缘特征的相关性来寻找外部参数. 同时也有基于神经网络的方法. 这类方法通常需要良好的初值. 另外一种方法是通过匹配两个传感器的运动来获得标定参数, 为了达到较高的精度, 需要充分和准确的自我运动估计。
在本文中, 我们提出了在线语义标定方法, 通过利用点云和相机的语义信息来解决初值的问题. 如图1所示.SOIC利用语义分割的结果估计初值和最终的外参.因为只要有足够多的语义对象之间的变化而不需要整个场景SOIC就可以工作. 它甚至可以用于离线场景(如室内机器人的校准). 本文的主要贡献如下:
-
(1).提出了一种基于语义分割的雷达和相机的外参标定方法
-
(2).引入语义质心(SC)来估计优化的初始值.
-
(3).在KITTI数据集上对方法进行了评估, 以验证可行性.
-
(4).源代码已开源
图1 SOIC通过点云和图像的语义标签匹配计算外参
方法
我们的方法包括三个步骤:
- (1).利用现有的方法对图像和点云进行了预训练, 得到语义分割结果, 基于这些语义分割结果,通过语义质心(SCs)来得到一个初始的位姿估计值.
- (2).在语义对应信息约束下, 定义了代价函数.
- (3).最后以初始值为基准, 对代价函数进一步的优化,得到更加精确的参数.
A.公式形式
对于点云和图像的匹配点, 我们描述
P
L
=
{
p
1
L
,
p
2
L
,
…
,
p
n
L
}
P^{L}=\left\{\mathbf{p}_{1}^{L}, \mathbf{p}_{2}^{L}, \ldots, \mathbf{p}_{n}^{L}\right\}
PL={p1L,p2L,…,pnL}, 其中
p
i
L
=
(
x
i
,
y
i
,
z
i
)
∈
R
3
\mathbf{p}_{i}^{L}=\left(x_{i}, y_{i}, z_{i}\right) \in \mathbb{R}^{3}
piL=(xi,yi,zi)∈R3, n是点云的数量, L表示为激光雷达坐标系.
另外, 我们标记每个点
p
i
p_i
pi的语义标签,
ℓ
[
l
,
m
]
i
m
g
∈
S
\ell_{[l, m]}^{i m g} \in S
ℓ[l,m]img∈S,
S
=
{
0
,
1
,
2
…
N
}
S=\{0,1,2 \ldots N\}
S={0,1,2…N}表示语义集合.
类似地, 我们对
W
x
H
W x H
WxH的图像每个像素也进行了标注,
ℓ
[
l
,
m
]
i
m
g
∈
S
\ell_{[l, m]}^{i m g} \in S
ℓ[l,m]img∈S表示像素
I
[
l
,
m
]
I[l, m]
I[l,m]的类别. 其中
l
∈
[
0
,
W
]
and
m
∈
[
0
,
H
]
l \in[0, W] \text { and } m \in[0, H]
l∈[0,W] and m∈[0,H], 由于分辨率的差异像素的数量远大于点云的数量.
我们定义旋转角
θ
=
(
θ
x
,
θ
y
,
θ
z
)
\boldsymbol{\theta}=\left(\theta_{x}, \theta_{y}, \theta_{z}\right)
θ=(θx,θy,θz)和平移向量
t
=
(
t
x
,
t
y
,
t
z
)
\mathbf{t}=\left(t_{x}, t_{y}, t_{z}\right)
t=(tx,ty,tz)表示从点云
P
L
P^{L}
PL到相机
P
C
P^{C}
PC的坐标变换. 因此许多点被投影到相同类别的图像像素上.
激光雷达坐标系下的点可以通过旋转角和平移向量通过下式变换到相机坐标系下:
p
i
C
=
R
(
θ
)
⋅
p
i
L
+
t
\mathbf{p}_{i}^{C}=\mathcal{R}(\theta) \cdot \mathbf{p}_{i}^{L}+\mathbf{t}
piC=R(θ)⋅piL+t
如果我们知道相机内参
K
\mathbf{K}
K和投影函数
P
\mathcal{P}
P, 可以通过如下方法将位于相机坐标系下的空间点投影到像素坐标
[
u
i
,
v
i
]
\left[u^{i}, v^{i}\right]
[ui,vi]上.
[
u
i
,
v
i
]
=
P
(
K
,
p
i
C
)
\left[u^{i}, v^{i}\right]=\mathcal{P}\left(\mathbf{K}, \mathbf{p}_{i}^{C}\right)
[ui,vi]=P(K,piC)
注意: 由于错误的外参估计可能会使一些空间点投影到图像后, 超出像素范围.
我们定义代价函数来使得点云的标签
ℓ
i
p
c
d
∈
S
\ell_{i}^{p c d} \in S
ℓipcd∈S和像素标签
ℓ
[
u
i
,
v
i
]
i
m
g
∈
S
\ell_{\left[u^{i}, v^{i}\right]}^{i m g} \in S
ℓ[ui,vi]img∈S的一致性最大化, 因此我们定义代价函数为:
C
=
1
−
e
−
ϵ
−
1
∣
(
ℓ
i
p
c
d
−
ℓ
∣
u
i
,
v
i
∣
i
m
g
)
)
∣
\mathcal{C}=1-e^{\left.-\epsilon^{-1} \mid\left(\ell_{i}^{p c d}-\ell_{\mid u^{i}, v^{i} \mid}^{i m g}\right)\right) \mid}
C=1−e−ϵ−1∣(ℓipcd−ℓ∣ui,vi∣img))∣
其中
ϵ
\epsilon
ϵ是一个很小量这也导致了
e
−
ϵ
−
1
e^{-\epsilon^{-1}}
e−ϵ−1接近于0. 如果点
p
i
p_i
pi和像素
[
u
i
,
v
i
]
\left[u^{i}, v^{i}\right]
[ui,vi]标签是一致的, 那么代价函数
C
\mathcal{C}
C近似为0, 如果类别不相同代价函数
C
\mathcal{C}
C将会趋近于1.
对于转换到相机坐标系的点
p
i
C
p^C_i
piC, 如果超出图像或者与像素标签不一致, 则通过定义一个距离函数
D
\mathcal{D}
D来计算原始激光雷达坐标系下的点
p
i
L
p^L_i
piL的损失.
D
(
p
i
L
)
=
min
ℓ
[
l
,
m
]
i
m
g
=
ℓ
i
p
c
d
(
M
(
[
u
,
v
]
i
,
[
l
,
m
]
)
)
∣
p
i
L
∣
2
\mathcal{D}\left(\mathbf{p}_{i}^{L}\right)=\min _{\ell_{[l, m]}^{i m g}=\ell_{i}^{p c d}}\left(\mathcal{M}\left([u, v]^{i},[l, m]\right)\right)\left|p_{i}^{L}\right|^{2}
D(piL)=ℓ[l,m]img=ℓipcdmin(M([u,v]i,[l,m]))∣∣piL∣∣2
因此这个函数计算图像中具有相同标签的曼哈顿距离. 因此上式的
M
\mathcal{M}
M定义如下:
M
(
[
u
,
v
]
,
[
l
,
m
]
)
=
∣
u
−
l
∣
+
∣
v
−
m
∣
\mathcal{M}([u, v],[l, m])=|u-l|+|v-m|
M([u,v],[l,m])=∣u−l∣+∣v−m∣
由于语义分割会有多个类别, 我们对不同类别进行了加权, 这样可以根据不同类别对损失函数进行计算, 我们定义的类别加权函数
1
A
\mathbf{1}_{A}
1A如下:
1
A
(
x
)
:
=
{
1
if
x
∈
A
0
if
x
∉
A
1_{A}(x):=\left\{\begin{array}{ll} 1 & \text { if } x \in A \\ 0 & \text { if } x \notin A \end{array}\right.
1A(x):={10 if x∈A if x∈/A
结合上述的公式, 我们可以得到点云和图像的最终的代价函数, 分母表示有效的语义标签的个数
L
=
∑
s
∈
S
∑
i
n
1
{
s
}
(
ℓ
i
p
c
d
)
C
(
p
i
L
)
D
(
p
i
L
)
∑
s
∈
S
∑
i
n
1
{
s
}
(
ℓ
i
p
c
d
)
\mathcal{L}=\frac{\sum_{s \in S} \sum_{i}^{n} 1_{\{s\}}\left(\ell_{i}^{p c d}\right) \mathcal{C}\left(\mathbf{p}_{i}^{L}\right) \mathcal{D}\left(\mathbf{p}_{i}^{L}\right)}{\sum_{s \in S} \sum_{i}^{n} 1_{\{s\}}\left(\ell_{i}^{p c d}\right)}
L=∑s∈S∑in1{s}(ℓipcd)∑s∈S∑in1{s}(ℓipcd)C(piL)D(piL)
最终我们通过最小化代价函数来求解外参变量
θ
^
\hat{\boldsymbol{\theta}}
θ^和
t
^
\hat{\mathrm{t}}
t^
θ
^
,
t
^
=
arg
min
θ
,
t
L
\hat{\boldsymbol{\theta}}, \hat{\mathbf{t}}=\underset{\boldsymbol{\theta}, \mathbf{t}}{\arg \min } \mathcal{L}
θ^,t^=θ,targminL
B. 代价函数的初始化
受到解决PnP问题的控制点决策的启发, 我们提出了语义质心来构造一个可以解析求解的含噪声的PnP问题. 如图2所示, 我们通过如下式子类别
s
s
s的语义信息.
P
s
L
=
{
p
s
,
1
L
,
p
s
,
2
L
,
…
∣
p
s
,
i
L
∈
P
L
,
ℓ
i
p
c
d
=
s
}
P_{s}^{L}=\left\{\mathbf{p}_{s, 1}^{L}, \mathbf{p}_{s, 2}^{L}, \ldots \mid \mathbf{p}_{s, i}^{L} \in P^{L}, \ell_{i}^{p c d}=s\right\}
PsL={ps,1L,ps,2L,…∣ps,iL∈PL,ℓipcd=s}
语义质心如下
S
C
s
L
=
∑
p
i
∈
{
P
d
L
}
p
i
∣
{
P
s
L
}
∣
\mathbf{S C}_{\mathbf{s}}^{L}=\frac{\sum_{\mathbf{p}_{i} \in\left\{P_{d}^{L}\right\}} \mathbf{p}_{i}}{\left|\left\{P_{s}^{L}\right\}\right|}
SCsL=∣{PsL}∣∑pi∈{PdL}pi
在PnP问题中, 我们将点云的语义质心
3
D
−
S
C
3D-SC
3D−SC与图像的语义质心
2
D
−
S
C
2D-SC
2D−SC视为3D-2D的匹配点对.需要注意的是, 点云的三维语义质心通常与图像的几何中心不一致. 这意味着点云的语义质心和图像的语义质心可能没有很好的对应. 因此, 这里计算的结果仅作为代价函数的初始值.
图2 展示了点云和图像的语义分割结果以及语义质心
实验结果
我们首先在KITTI有标签的数据集上评估了所提出的方法的性能. 并将其与在线的方法进行了比较.
随后我们证明不同的外参变量对代价函数的影响以及收敛情况
图3 代价函数的收敛情况
图4 展示了100帧数据车辆类别的语义质心分布情况
我们使用RANSAC算法从3D-SCs中估计出绿色平面, 红色箭头表示平面的法线, 可以看出所有的质心都近似地分布在同一个平面上.
图5 展示了语义质心与参数估计值的对应关系
绿色数字表示图像的语义质心, 蓝色数据表示投影点云的语义质心, 数字表示图像点云对的索引值.
由于我们发现在图4中可视化的语义质心都近似分布在一个平面上, 使用EPnP的方法来求解这个PnP问题将会变得很困难, 因此我们选择使用IPPE方法来进行求解.
显然语义分割的标签的可以帮助提升算法的性能, 因此我们选择训练好的预测KITTI车辆的网络PointRCNN来完成语义标签的预测工作.
图6 展示了使用网络预测出来的语义标签和真实的GT对优化结果的影响
图7 展示了使用MI的结果
表1 展示了不同条件下的误差值
图8 展示了实例场景的实验结果
我们用于语义分割的图像是图8(a), 图8(b)展示了通过PointRCNN网络预测出来的属于车辆类别的点云. 并利用SOIC预测出来的外参将属于车辆类别的点云投影到图像中.图8©使用SOIC的预测的外参将GT指定的车辆类别的点云投影到图像上, 图8(d)是使用GT提供的车辆点云的类别, 并且使用GT的外参将属于车辆类别的点云投影到图像上.粉红色框的内容表示车辆类别的点云,在经过外参投影到图像的像素中时会和原始语义图像(图像的语义类别由图像的GT提供)车辆的像素位置发生轻微错位.图8(b)的错位情况较为严重,是由于车辆的类别和外参都是预测得到的.
本文由计算机视觉life旗下 从零开始学习SLAM 知识星球翻译
欢迎加入公众号读者群一起和同行交流,目前有SLAM、检测分割识别、三维视觉、医学影像、GAN、自动驾驶、计算摄影、算法竞赛等微信群(以后会逐渐细分),请扫描下面微信号加群,备注:”昵称+学校/公司+研究方向“,例如:”张三 + 上海交大 + 视觉SLAM“。请按照格式备注,否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告,否则会请出群,谢谢理解~
投稿、合作也欢迎联系:simiter@126.com
长按关注计算机视觉life