The Double Sphere Camera Model
摘要
基于视觉的运动估计和 3D 重建具有许多应用(例如,自动驾驶、机载设备的导航系统和增强现实)正在受到广泛的研究关注。为了提高准确性和鲁棒性,一些研究人员最近证明了在此类应用中使用大视场相机的好处。
在本文中,我们对大型视场相机的现有模型进行了广泛的回顾。对于每个模型,我们提供投影和非投影函数以及导致有效投影的点的子空间。然后,我们提出了双球相机模型,该模型非常适合大视场镜头,计算成本低并且具有封闭形式的逆。我们使用具有多个不同镜头的校准数据集评估模型,并使用与视觉里程计相关的指标来比较模型,即重投影误差,以及投影和非投影函数及其雅可比矩阵的计算时间。我们还提供定性结果并讨论所有模型的性能。
1.引言
视觉里程计和同步定位和映射对于许多应用来说变得越来越重要。为了提高这些方法的准确性和鲁棒性,必须改进硬件和软件。
使用大视场相机可以解决几个问题。首先,大视场更容易捕捉环境中更多纹理区域,这是基于视觉的稳定运动估计所必需的。其次,与具有相同分辨率的较小视野的相机相比,具有大视野的大相机运动可以映射到较小的像素运动。这确保了连续帧之间的小光流,这对于直接方法特别有益。
以前的研究表明,大视场有利于基于视觉的运动估计[14][11]。 折反射相机机械复杂且昂贵; 然而,鱼眼镜头体积小、重量轻,并且在消费市场上广泛使用。 因此,在本文中,我们专注于描述其投影的鱼眼镜头和模型。
图1: 建议的双球 (DS) 投影模型。最初,该点被投影到第一个球体(绿色)上,然后投影到第二个球体上,第二个球体相对于第一个球体移动了 ξ(红色)。然后,将该点投影到针孔相机的图像平面上,该图像平面从第二个球体偏移 α 1 − α \frac{\alpha}{1-\alpha} 1−αα。下图是使用提出的 DS 模型校准后的图案角点的重投影,这表明该模型很好地拟合了镜头。
本文的提醒组织如下。在第 2 节中,我们对可与鱼眼镜头一起使用的相机型号进行了广泛的回顾。为了使论文自成一体,我们提供了投影和非投影函数,并为每个模型定义了有效投影的子空间。在第 3 节中,我们提出了一种新颖的鱼眼相机投影模型,具有以下优点。
- 所提出的投影模型非常适合表示鱼眼镜头的失真。
- 所提出的模型不需要用于投影和非投影的计算昂贵的三角运算。
- 与基于高阶多项式[6] [12] 的投影模型不同,该模型使用迭代方法对点进行反投影,投影函数的逆函数以封闭形式存在。
在第 5 节中,我们根据与基于视觉的运动估计相关的指标评估所有提出的模型。在这里,我们使用使用几个不同镜头收集的数据集来评估每个模型的重投影误差。我们还介绍了投影和非投影函数所需的计算时间,以及计算雅可比行列相对于它们的参数所需的时间。本研究中使用的数据集以及所提出模型的开源实现可在项目页面上找到: https://vision.in.tum.de/research/vslam/double-sphere
2.相关工作
在审查可与鱼眼镜头一起使用的现有相机模型之前,我们定义了本文中使用的符号。我们使用小写字母表示标量,例如 u \text{u} u,粗体大写字母表示矩阵,例如 R \bold{R} R,以及粗体小写字母表示向量,例如 x \bold{x} x。
通常,我们将像素坐标表示为 u = [ u,v ] T ∈ Θ ⊂ R 2 \bold{u}=[\text{u,v}]^T\in\Theta\subset\mathbb{R}^2 u=[u,v]T∈Θ⊂R2 ,其中 Θ \Theta Θ表示点可以投影到的图像域。 3D点坐标表示为 x = [ x,y,z ] T ∈ Ω ⊂ R 3 \bold{x}=[\text{x,y,z}]^T\in\Omega\subset\mathbb{R}^3 x=[x,y,z]T∈Ω⊂R3,其中 Ω \Omega Ω表示导致有效投影的 3D 点集。
对于所有相机模型,我们假设所有投影都穿过定义相机坐标系位置的单个点(即中心投影)。相机框架的方向定义如下。 z z z轴与相机的主轴对齐,另外两个正交方向 ( x , y ) (x,y) (x,y)与图像平面的相应轴对齐。我们定义了一个刚性附加到校准模式的坐标系,使得作为特殊欧几里得群的矩阵的变换 T c a n ∈ S E ( 3 ) \bold{T}_{ca_n}\in SE(3) Tcan∈SE(3)将 3D 坐标从校准模式坐标系转换为相机坐标图像系统 n n n。
通常,相机投影函数是一个映射
π
:
Ω
→
Θ
\pi:\Omega\to\Theta
π:Ω→Θ。它的逆
π
−
1
:
Θ
→
S
2
\pi^{−1}:\Theta\to\mathbb{S}^2
π−1:Θ→S2将图像坐标不投影到单位长度的方位矢量,它定义了一条射线,所有点都通过该射线投影到这些图像坐标上。
对于本节讨论的所有相机模型,我们提供了
π
\boldsymbol{\pi}
π,
π
−
1
\boldsymbol{\pi}^{-1}
π−1的定义,即内在参数
i
\bold{i}
i、
Ω
\Omega
Ω和
Θ
\Theta
Θ的向量。
图 2:统一摄像机模型 (UCM) 和扩展统一摄像机模型 (EUCM) 的示意图。首先将一个 3D 点投影到一个单位球体上,然后投影到从球体中心偏移
α
1
−
α
\frac{\alpha}{1-\alpha}
1−αα的针孔相机的图像平面上。在 EUCM 中,使用系数
β
\beta
β将球体转换为椭球体。
2.1.针孔相机模型(Pinhole Camera Model, PCM)
针孔相机模型有四个参数
i
=
[
f
x
,
f
y
,
c
x
,
c
y
]
T
\bold{i} = [f_x,f_y,c_x,c_y]^T
i=[fx,fy,cx,cy]T,其投影函数定义如下:
π
(
x
,
i
)
=
[
f
x
x
z
f
y
y
z
]
+
[
c
x
c
y
]
(
1
)
\boldsymbol{\pi}(\bold{x,i})= \begin{bmatrix}f_x\frac{x}{z}\\f_y\frac{y}{z}\end{bmatrix} +\begin{bmatrix}c_x\\c_y\end{bmatrix}\qquad(1)
π(x,i)=[fxzxfyzy]+[cxcy](1)
很容易看出,投影是为
Ω
=
{
x
∈
R
3
∣
z
>
0
}
\Omega=\{\text{x}\in\mathbb{R}^3\;|\;z > 0\}
Ω={x∈R3∣z>0},理论上将视场限制在小于 180°。然而,在实践中,即使添加了失真模型,针孔相机在大于 120° 的视场中也表现出次优性能。我们可以使用以下函数来反投影一个点:
π
−
1
(
u
,
i
)
=
1
m
x
2
+
m
y
2
+
1
[
m
x
m
y
1
]
(
2
)
m
x
=
u
−
c
x
f
x
(
3
)
m
y
=
v
−
c
y
f
y
(
4
)
\begin{aligned} \boldsymbol{\pi}^{-1}(\bold{u,i})&= \frac{1}{\sqrt{m_x^2+m_y^2+1}} \begin{bmatrix}m_x\\m_y\\1\end{bmatrix}\qquad&(2)\\ m_x&=\frac{\text{u}-c_x}{f_x}&(3)\\ m_y&=\frac{\text{v}-c_y}{f_y}&(4) \end{aligned}
π−1(u,i)mxmy=mx2+my2+11⎣
⎡mxmy1⎦
⎤=fxu−cx=fyv−cy(2)(3)(4)
其中反投影定义为
Θ
=
R
2
\Theta=\mathbb{R}^2
Θ=R2。
2.2.统一相机模型(Unified Camera Model, UCM)
统一相机模型 (UCM) 有五个参数
i
=
[
γ
x
,
γ
y
,
c
x
,
c
y
,
ξ
]
T
\text{i} = [\gamma_x,\gamma_y,c_x,c_y,\xi ]^T
i=[γx,γy,cx,cy,ξ]T,通常用于折反射相机 [9]。先前的一项研究 [4] 表明,UCM 可以表示具有抛物线、双曲线、椭圆和平面镜的系统。该模型也可以应用于带有鱼眼镜头的相机[13]。但是,它并不完全适合大多数鱼眼镜头;因此,通常会添加额外的失真模型。
在 UCM 中,投影定义如下:
π
(
x
,
i
)
=
[
γ
x
x
ξ
d
+
z
γ
y
y
ξ
d
+
z
]
+
[
c
x
c
y
]
(
5
)
d
=
x
2
+
y
2
+
z
2
(
6
)
\begin{aligned} \boldsymbol{\pi}(\bold{x,i})&= \begin{bmatrix}\gamma_x\frac{x}{\xi d+z}\\\gamma_y\frac{y}{\xi d+z}\end{bmatrix} +\begin{bmatrix}c_x\\c_y\end{bmatrix}\qquad&(5)\\ d&=\sqrt{x^2+y^2+z^2} &(6) \end{aligned}
π(x,i)d=[γxξd+zxγyξd+zy]+[cxcy]=x2+y2+z2(5)(6)
在这个模型中,一个点首先投影到单位球面上,然后投影到针孔相机的像平面上,从单位球的中心偏移
ξ
\xi
ξ。对于实际应用,我们建议将该模型重写如下:
π
(
x
,
i
)
=
[
f
x
x
α
d
+
(
1
−
α
)
z
f
y
y
α
d
+
(
1
−
α
)
z
]
+
[
c
x
c
y
]
(
7
)
\boldsymbol{\pi}(\bold{x,i})= \begin{bmatrix}f_x\frac{x}{\alpha d+(1-\alpha )z}\\ f_y\frac{y}{\alpha d+(1-\alpha )z}\end{bmatrix} +\begin{bmatrix}c_x\\c_y\end{bmatrix}\qquad(7)
π(x,i)=[fxαd+(1−α)zxfyαd+(1−α)zy]+[cxcy](7)
该模型的这个公式还有五个参数
i
=
[
f
x
,
f
y
,
c
x
,
c
y
,
α
]
T
,
α
∈
[
0
,
1
]
\bold{i}=[f_x,f_y,c_x,c_y,\alpha]^T,\alpha \in [0,1]
i=[fx,fy,cx,cy,α]T,α∈[0,1]并且在数学上等价于前一个参数
(
ξ
=
α
1
−
α
,
γ
x
=
f
x
1
−
α
,
γ
y
=
f
y
1
−
α
)
(\xi= \frac{\alpha}{1-\alpha},\gamma_x = \frac{f_x}{1−\alpha},\gamma_y=\frac{f_y}{1−\alpha})
(ξ=1−αα,γx=1−αfx,γy=1−αfy)。然而,正如第 5 节所讨论的,它具有更好的数值特性。请注意,对于
α
=
0
\alpha = 0
α=0,模型退化为针孔模型。产生有效投影的 3D 点集定义如下:
Ω
=
{
x
∈
R
3
∣
z
>
−
w
d
}
(
8
)
w
=
{
α
1
−
α
,
if
α
≤
0.5
1
−
α
α
,
if
α
>
0.5
(
9
)
\begin{aligned} \Omega&=\{\bold{x} \in \mathbb{R}^3 \; | \;z>-wd\} \qquad & (8)\\ w&= \begin{cases} \frac{\alpha}{1-\alpha},\quad & \text{if}\; \alpha \le 0.5\\ \frac{1-\alpha}{\alpha},\quad & \text{if}\; \alpha > 0.5 \end{cases} &(9) \end{aligned}
Ωw={x∈R3∣z>−wd}={1−αα,α1−α,ifα≤0.5ifα>0.5(8)(9)
其中(对于
α
>
0.5
\alpha > 0.5
α>0.5)
w
w
w表示示意图(图 2)上的水平轴与针孔相机焦点圆的切线的垂线之间的角度的正弦值。
图 3:Kannala-Brandt 相机模型 (KB) 的示意图。投影距光学中心的位移与
d
(
θ
)
d(\theta)
d(θ)成正比,
d
(
θ
)
d(\theta)
d(θ)是点与光轴
θ
\theta
θ之间夹角的多项式函数。
逆投影函数定义如下:
π
−
1
(
u
,
i
)
=
ξ
+
1
+
(
1
−
ξ
2
)
r
2
1
+
r
2
[
m
x
m
y
1
]
−
[
0
0
ξ
]
,
(
10
)
m
x
=
u
−
c
x
f
x
(
1
−
α
)
,
(
11
)
m
y
=
v
−
c
y
f
y
(
1
−
α
)
,
(
12
)
r
2
=
m
x
2
+
m
y
2
,
(
13
)
ξ
=
α
1
−
α
,
(
14
)
\begin{aligned} \boldsymbol{\pi}^{-1}(\bold{u,i})&= \frac{\xi +\sqrt{1+(1-\xi^2)r^2}}{1+r^2} \begin{bmatrix}m_x\\ m_y\\ 1 \end{bmatrix}- \begin{bmatrix}0\\ 0\\ \xi \end{bmatrix},\qquad &(10)\\ m_x&=\frac{u-c_x}{f_x}(1-\alpha), &(11)\\ m_y&=\frac{v-c_y}{f_y}(1-\alpha), &(12)\\ r^2&=m_x^2+m_y^2, &(13)\\ \xi &= \frac{\alpha}{1-\alpha}, &(14) \end{aligned}
π−1(u,i)mxmyr2ξ=1+r2ξ+1+(1−ξ2)r2⎣
⎡mxmy1⎦
⎤−⎣
⎡00ξ⎦
⎤,=fxu−cx(1−α),=fyv−cy(1−α),=mx2+my2,=1−αα,(10)(11)(12)(13)(14)
其中
Θ
\Theta
Θ定义如下。
Θ
=
{
R
2
if
α
≤
0.5
{
u
∈
R
2
∣
r
2
≤
(
1
−
α
)
2
2
α
−
1
}
if
α
>
0.5
(
15
)
\Theta= \begin{cases} \mathbb{R}^2 \quad & \text{if} \; \alpha \le 0.5 \\ \{\bold{u} \in \mathbb{R}^2 \; | \; r^2 \le \frac{(1-\alpha)^2}{2 \alpha - 1}\} \quad & \text{if} \; \alpha > 0.5 \end{cases} \qquad (15)
Θ={R2{u∈R2∣r2≤2α−1(1−α)2}ifα≤0.5ifα>0.5(15)
2.3.扩展统一相机模型(Extended Unified Camera Modedl, EUCM)
先前的一项研究 [7] 将统一相机模型 (EUCM) 扩展为具有六个参数
i
=
[
f
x
,
f
y
,
c
x
,
c
y
,
α
,
β
]
T
,
α
∈
[
0
,
1
]
,
β
>
0
\bold{i} = [f_x,f_y,c_x,c_y, \alpha , \beta ]^T , \alpha \in [0,1], \beta >0
i=[fx,fy,cx,cy,α,β]T,α∈[0,1],β>0并定义如下投影功能。
π
(
x
,
i
)
=
[
f
x
x
α
d
+
(
1
−
α
)
z
f
y
y
α
d
+
(
1
−
α
)
z
]
+
[
c
x
c
y
]
(
16
)
d
=
β
(
x
2
+
y
2
)
+
z
2
(
17
)
\begin{aligned} \boldsymbol{\pi}(\bold{x,i}) &= \begin{bmatrix}f_x\frac{x}{\alpha d+(1-\alpha )z}\\ f_y\frac{y}{\alpha d+(1-\alpha )z}\end{bmatrix} +\begin{bmatrix}c_x\\c_y\end{bmatrix} \qquad &(16)\\ d &= \sqrt{\beta(x^2+y^2)+z^2} &(17) \end{aligned}
π(x,i)d=[fxαd+(1−α)zxfyαd+(1−α)zy]+[cxcy]=β(x2+y2)+z2(16)(17)
EUCM 可以解释为 UCM 的概括,其中点投影到围绕
z
z
z 轴对称的椭圆体上(图 2)。该研究还表明,当将模型视为二次曲面上的投影,然后在图像平面上进行正交投影时,该模型是完整的,因为它可以表示所有可能的二次曲面。
使用 EUCM,一个集合
Ω
\Omega
Ω 的定义类似于 UCM,不同之处在于
d
d
d 由公式17得到。请注意,当
β
=
1
\beta=1
β=1 时,EUCM 退化为常规 UCM。如前所述,EUCM 投影在椭球体上。因此,不能直接得到非投影的单位长度向量;因此,我们必须采用标准化。逆投影函数定义如下:
π
−
1
(
u
,
i
)
=
1
m
x
2
+
m
y
2
+
m
z
2
[
m
x
m
y
m
z
]
(
18
)
m
x
=
u
−
c
x
f
x
,
(
19
)
m
y
=
v
−
c
y
f
y
,
(
20
)
r
2
=
m
x
2
+
m
y
2
,
(
21
)
m
z
=
1
−
β
α
2
r
2
α
1
−
(
2
α
−
1
)
β
r
2
+
(
1
−
α
)
,
(
22
)
\begin{aligned} \boldsymbol{\pi}^{-1}(\bold{u,i})&= \frac{1}{\sqrt{m_x^2+m_y^2+m_z^2}} \begin{bmatrix}m_x\\ m_y\\ m_z \end{bmatrix} \qquad &(18)\\ m_x &=\frac{u-c_x}{f_x}, &(19)\\ m_y &=\frac{v-c_y}{f_y}, &(20)\\ r^2 &=m_x^2+m_y^2, &(21)\\ m_z &= \frac{1-\beta \alpha^2 r^2}{\alpha \sqrt{1-(2 \alpha -1)\beta r^2} +(1-\alpha)}, &(22) \end{aligned}
π−1(u,i)mxmyr2mz=mx2+my2+mz21⎣
⎡mxmymz⎦
⎤=fxu−cx,=fyv−cy,=mx2+my2,=α1−(2α−1)βr2+(1−α)1−βα2r2,(18)(19)(20)(21)(22)
其中
Θ
\Theta
Θ定义如下。
Θ
=
{
R
2
if
α
≤
0.5
{
u
∈
R
2
∣
r
2
≤
1
β
(
2
α
−
1
)
}
if
α
>
0.5
(
23
)
\Theta= \begin{cases} \mathbb{R}^2 \quad & \text{if} \; \alpha \le 0.5 \\ \{\bold{u} \in \mathbb{R}^2 \; | \; r^2 \le \frac{1}{\beta(2 \alpha - 1)}\} \quad & \text{if} \; \alpha > 0.5 \end{cases} \qquad (23)
Θ={R2{u∈R2∣r2≤β(2α−1)1}ifα≤0.5ifα>0.5(23)
2.4.Kannala-Brandt 相机模型(Kannala-Brandt, KBCM)
先前的研究 [6] 提出了 Kannala-Brandt (KB) 模型,这是一种通用相机模型,非常适合常规、广角和鱼眼镜头。 KB 模型假设从图像的光学中心到投影点的距离与该点与主轴之间的夹角的多项式成正比(图 3)。我们评估 KB 模型的两个版本,即:有六个参数
i
=
[
f
x
,
f
y
,
c
x
,
c
y
,
k
1
,
k
2
]
T
\bold{i} = [f_x,f_y,c_x,c_y,k_1,k_2]^T
i=[fx,fy,cx,cy,k1,k2]T 和八个参数
i
=
[
f
x
,
f
y
,
c
x
,
c
y
,
k
1
,
k
2
,
k
3
,
k
4
]
T
\bold{i} = [f_x, f_y, c_x, c_y, k_1, k_2 , k_3, k_4]^T
i=[fx,fy,cx,cy,k1,k2,k3,k4]T 。 KB模型的投影函数定义如下:
π
(
x
,
i
)
=
[
f
x
d
(
θ
)
x
r
f
y
d
(
θ
)
y
r
]
+
[
c
x
c
y
]
,
(
24
)
r
=
x
2
+
y
2
,
(
25
)
θ
=
a
t
a
n
2
(
r
,
z
)
,
(
26
)
d
(
θ
)
=
θ
+
k
1
θ
3
+
k
2
θ
5
+
k
3
θ
7
+
k
4
θ
9
,
(
27
)
\begin{aligned} \boldsymbol{\pi}(\bold{x,i}) &= \begin{bmatrix}f_x \; d(\theta) \; \frac{x}{r} \\ f_y \; d(\theta) \; \frac{y}{r}\end{bmatrix} + \begin{bmatrix}c_x\\c_y\end{bmatrix}, \qquad &(24)\\ r &= \sqrt{x^2+y^2}, &(25) \\ \theta &= atan2(r,z), &(26) \\ d(\theta) &= \theta+k_1 \theta^3+k_2 \theta^5+k_3 \theta^7+k_4 \theta^9, &(27) \end{aligned}
π(x,i)rθd(θ)=[fxd(θ)rxfyd(θ)ry]+[cxcy],=x2+y2,=atan2(r,z),=θ+k1θ3+k2θ5+k3θ7+k4θ9,(24)(25)(26)(27)
假设多项式
d
(
θ
)
d(\theta)
d(θ) 是单调的
Ω
=
R
3
\
[
0
,
0
,
0
]
T
\Omega = \mathbb{R}^3 \backslash [0,0,0]^T
Ω=R3\[0,0,0]T 。
KB 模型的逆投影函数需要找到一个高阶多项式的根,以从 d(θ) 中恢复角度 θ。这可以通过迭代优化来实现,例如牛顿法。非投影函数可以表示如下:
π
−
1
(
x
,
i
)
=
[
s
i
n
(
θ
∗
)
m
x
r
u
s
i
n
(
θ
∗
)
m
y
r
u
c
o
s
(
θ
∗
)
]
,
(
28
)
m
x
=
u
−
c
x
f
x
,
(
29
)
m
y
=
v
−
c
y
f
y
,
(
30
)
r
u
=
m
x
2
+
m
y
2
,
(
31
)
θ
∗
=
d
−
1
(
r
u
)
,
(
32
)
\begin{aligned} \boldsymbol{\pi}^{-1}(\bold{x,i}) &= \begin{bmatrix}sin(\theta^*) \frac{m_x}{r_u} \\sin(\theta^*) \frac{m_y}{r_u} \\cos(\theta^*) \end{bmatrix}, \qquad &(28)\\ m_x &=\frac{u-c_x}{f_x}, &(29)\\ m_y &=\frac{v-c_y}{f_y}, &(30)\\ r_u &=\sqrt{m_x^2+m_y^2}, &(31)\\ \theta^* &=d^{-1}(r_u), &(32) \end{aligned}
π−1(x,i)mxmyruθ∗=⎣
⎡sin(θ∗)rumxsin(θ∗)rumycos(θ∗)⎦
⎤,=fxu−cx,=fyv−cy,=mx2+my2,=d−1(ru),(28)(29)(30)(31)(32)
其中
θ
∗
\theta^*
θ∗ 是
d
(
θ
)
=
r
u
d(\theta) = r_u
d(θ)=ru 的解。如果
d
(
θ
)
d(\theta)
d(θ) 是单调的
Θ
=
R
2
\Theta=\mathbb{R}^2
Θ=R2。
图 4:视场相机模型 (FOV) 的示意图。投影从光学中心的位移与点和光轴之间的角度成正比
KB 模型有时用作针孔相机的畸变模型,例如 Kalibr[3] 中的等距畸变模型或 OpenCV 中的鱼眼相机模型。该模型在数学上是相同的;然而,由于它首先使用针孔模型投影点然后应用失真,因此它在 z = 0 z = 0 z=0 处具有奇点,这使得它不适用于视场接近 180°的鱼眼镜头,当以这种方式实现时。
存在其他几种基于高阶多项式的大视场镜头模型。例如,[12] 和 KB 模型之间的主要区别如下:该模型为投影和非投影校准了两个独立的多项式,为两者提供了一个封闭形式的解决方案,对于投影,它使用之间的角度图像平面和点,而不是光轴和点之间的角度。我们希望该模型具有相似的性能,并且不会将其明确包含在我们的评估中。
图 5:用于评估相机模型的镜头;从左到右:BF2M2020S23 (195°)、BF5M13720 (183°)、BM4018S118 (126°)、BM2820 (122°) 和 GoPro 替换镜头 (150°)。
2.5.视场相机模型(Field-of-View Camera Model, FOVCM)
先前提出的视野相机模型(FOV)[2],具有五个参数
i
=
[
f
x
,
f
y
,
c
x
,
c
y
,
w
]
T
\bold{i} = [f_x,f_y,c_x,c_y,w]^T
i=[fx,fy,cx,cy,w]T,并假设图像点和主点之间的距离通常与相应的 3D 点和光轴之间的角度大致成比例(图 4)。根据作者的说法,参数
w
w
w 大约对应于理想鱼眼镜头的视场。该模型的投影函数定义如下:
π
(
x
,
i
)
=
[
f
x
r
d
x
r
u
f
y
r
d
y
r
u
]
+
[
c
x
c
y
]
,
(
33
)
r
u
=
x
2
+
y
2
,
(
34
)
r
d
=
a
t
a
n
2
(
2
r
u
t
a
n
w
z
,
z
)
w
,
(
35
)
\begin{aligned} \boldsymbol{\pi}(\bold{x,i}) &= \begin{bmatrix}f_x \; r_d \; \frac{x}{r_u} \\ f_y \; r_d \; \frac{y}{r_u}\end{bmatrix} + \begin{bmatrix}c_x\\c_y\end{bmatrix}, \qquad &(33)\\ r_u &= \sqrt{x^2+y^2}, &(34) \\ r_d &= \frac{atan2(2r_u \; tan\frac{w}{z},z)}{w}, &(35) \end{aligned}
π(x,i)rurd=[fxrdruxfyrdruy]+[cxcy],=x2+y2,=watan2(2rutanzw,z),(33)(34)(35)
其中
Ω
=
R
3
\
[
0
,
0
,
0
]
T
\Omega = \mathbb{R}^3 \backslash [0,0,0]^T
Ω=R3\[0,0,0]T。
FOV 模型有一个用于取消投影点的封闭形式的解决方案,其定义如下:
π
−
1
(
x
,
i
)
=
[
m
x
s
i
n
(
r
d
w
)
2
r
d
t
a
n
(
w
2
)
m
y
s
i
n
(
r
d
w
)
2
r
d
t
a
n
(
w
2
)
c
o
s
(
r
d
w
)
]
,
(
36
)
m
x
=
u
−
c
x
f
x
,
(
37
)
m
y
=
v
−
c
y
f
y
,
(
38
)
r
d
=
m
x
2
+
m
y
2
,
(
39
)
\begin{aligned} \boldsymbol{\pi}^{-1}(\bold{x,i}) &= \begin{bmatrix} m_x \frac{sin(r_d \,w)}{2 \, r_d \, tan(\frac{w}{2})} \\ m_y \frac{sin(r_d \,w)}{2 \, r_d \, tan(\frac{w}{2})} \\ cos(r_d \, w) \end{bmatrix}, \qquad &(36)\\ m_x &=\frac{u-c_x}{f_x}, &(37)\\ m_y &=\frac{v-c_y}{f_y}, &(38)\\ r_d &=\sqrt{m_x^2+m_y^2}, &(39) \end{aligned}
π−1(x,i)mxmyrd=⎣
⎡mx2rdtan(2w)sin(rdw)my2rdtan(2w)sin(rdw)cos(rdw)⎦
⎤,=fxu−cx,=fyv−cy,=mx2+my2,(36)(37)(38)(39)
其中
Θ
=
R
2
\Theta = \mathbb{R}^2
Θ=R2。
与 KB 模型类似,FOV 模型可以用作针孔相机的畸变模型。
3.双球面相机模型(Double Sphere Camera Model, DSCM)
我们提出了双球 (DS) 相机模型,它更适合带有鱼眼镜头的相机,具有封闭形式的逆,并且不需要计算昂贵的三角运算。 在提出的 DS 模型中,一个点被连续投影到两个单位球体上,其中心偏移了
ξ
\xi
ξ 。 然后,使用偏移
α
1
−
α
\frac{\alpha}{1-\alpha}
1−αα 的针孔模型将该点投影到图像平面上(图 1)。 该模型有六个参数
i
=
[
f
x
,
f
y
,
c
x
,
c
y
,
ξ
,
α
]
T
\bold{i} = [f_x,f_y,c_x,c_y,\xi ,\alpha]^T
i=[fx,fy,cx,cy,ξ,α]T 和定义如下的投影函数:
π
(
x
,
i
)
=
[
f
x
x
α
d
2
+
(
1
−
α
)
(
ξ
d
1
+
z
)
f
y
y
α
d
2
+
(
1
−
α
)
(
ξ
d
1
+
z
)
]
+
[
c
x
c
y
]
(
40
)
d
1
=
x
2
+
y
2
+
z
2
(
41
)
d
2
=
x
2
+
y
2
+
(
ξ
d
1
+
z
)
2
(
42
)
\begin{aligned} \boldsymbol{\pi}(\bold{x,i}) &= \begin{bmatrix} f_x \frac{x}{\alpha d_2 + ( 1 - \alpha)(\xi d_1 + z)}\\ f_y \frac{y}{\alpha d_2 + ( 1 - \alpha)(\xi d_1 + z)} \end{bmatrix}+ \begin{bmatrix} c_x \\ c_y \end{bmatrix} \qquad &(40)\\ d_1 &= \sqrt{x^2 + y^2 + z^2} &(41) \\ d_2 &= \sqrt{x^2 + y^2 + (\xi d_1 + z)^2} &(42) \end{aligned}
π(x,i)d1d2=[fxαd2+(1−α)(ξd1+z)xfyαd2+(1−α)(ξd1+z)y]+[cxcy]=x2+y2+z2=x2+y2+(ξd1+z)2(40)(41)(42)
导致有效投影的一组 3D 点表示如下:
Ω
=
{
x
∈
R
3
∣
z
>
−
w
2
d
1
}
(
43
)
w
2
=
w
1
+
ξ
2
w
1
ξ
+
ξ
2
+
1
(
44
)
w
1
=
{
α
1
−
α
,
if
α
≤
0.5
1
−
α
α
,
if
α
>
0.5
(
45
)
\begin{aligned} \Omega&=\{\bold{x} \in \mathbb{R}^3 \; | \;z>-w_2 d_1\} \qquad & (43)\\ w_2 & = \frac{w_1 + \xi}{\sqrt{2w_1 \xi + {\xi}^2 + 1}} & (44) \\ w_1 &= \begin{cases} \frac{\alpha}{1-\alpha},\, & \text{if}\; \alpha \le 0.5\\ \frac{1-\alpha}{\alpha},\, & \text{if}\; \alpha > 0.5 \end{cases} &(45) \end{aligned}
Ωw2w1={x∈R3∣z>−w2d1}=2w1ξ+ξ2+1w1+ξ={1−αα,α1−α,ifα≤0.5ifα>0.5(43)(44)(45)
逆投影函数计算如下:
π
−
1
(
u
,
i
)
=
m
z
ξ
+
m
z
2
+
(
1
−
ξ
2
)
r
2
m
z
2
+
r
2
[
m
x
m
y
m
z
]
+
[
0
0
ξ
]
(
46
)
m
x
=
u
−
c
x
f
x
,
(
47
)
m
y
=
v
−
c
y
f
y
,
(
48
)
r
2
=
m
x
2
+
m
y
2
,
(
49
)
m
z
=
1
−
α
2
r
2
α
1
−
(
2
α
−
1
)
r
2
+
(
1
−
α
)
,
(
50
)
\begin{aligned} \boldsymbol{\pi}^{-1}(\bold{u,i})&= \frac{m_z \xi + \sqrt{m_z^2 + (1- {\xi}^2) r^2}}{m_z^2 + r^2} \begin{bmatrix}m_x\\ m_y\\ m_z \end{bmatrix} + \begin{bmatrix}0\\ 0\\ \xi \end{bmatrix} \qquad &(46)\\ m_x &=\frac{u-c_x}{f_x}, &(47)\\ m_y &=\frac{v-c_y}{f_y}, &(48)\\ r^2 &=m_x^2+m_y^2, &(49)\\ m_z &= \frac{1- \alpha ^2 r^2}{\alpha \sqrt{1-(2 \alpha -1) r^2} +(1-\alpha)}, &(50) \end{aligned}
π−1(u,i)mxmyr2mz=mz2+r2mzξ+mz2+(1−ξ2)r2⎣
⎡mxmymz⎦
⎤+⎣
⎡00ξ⎦
⎤=fxu−cx,=fyv−cy,=mx2+my2,=α1−(2α−1)r2+(1−α)1−α2r2,(46)(47)(48)(49)(50)
下式成立。
Θ
=
{
R
2
if
α
≤
0.5
{
u
∈
R
2
∣
r
2
≤
1
2
α
−
1
}
if
α
>
0.5
(
51
)
\Theta= \begin{cases} \mathbb{R} ^2 & \text{if} \; \alpha \le 0.5\\ \{\bold{u} \in \mathbb{R}^2 \, | \,r^2 \le \frac{1}{2 \alpha - 1} \} & \text{if} \; \alpha > 0.5 \end{cases} \qquad (51)
Θ={R2{u∈R2∣r2≤2α−11}ifα≤0.5ifα>0.5(51)
4.标定
为了估计每个模型的相机参数,我们使用可以在图像中自动检测到的 AprilTag 标记 [10](图 1)网格。 对于校准序列中的每个图像
n
n
n,检测为我们提供了角
k
k
k 在图像平面上投影的 2D 位置
u
n
k
\bold{u}_{nk}
unk 以及角的相关 3D 位置
x
k
\bold{x}_{k}
xk。 在初始标记检测之后,我们对每个角使用局部亚像素细化来获得更好的校准精度。 我们制定了取决于状态
s
=
[
i
,
T
c
a
1
,
.
.
.
,
T
c
a
N
]
\bold{s} = [\bold{i},\bold{T}_{ca_1} ,...,\bold{T}_{ca_N} ]
s=[i,Tca1,...,TcaN] 的优化函数,如下所示:
E
(
s
)
=
∑
n
=
1
N
∑
k
∈
K
ρ
(
(
π
(
T
c
a
n
x
k
,
i
)
−
u
n
k
)
2
)
(
52
)
E(s) = \sum_{n=1}^{N} \sum_{k \in K} \rho \big(({\pi(\bold{T}_{ca_n} \bold{x}_k , \bold{i})- \bold{u}_{nk}})^2 \big) \qquad (52)
E(s)=n=1∑Nk∈K∑ρ((π(Tcanxk,i)−unk)2)(52)
其中
i
\bold{i}
i 是内在参数的向量,
π
π
π 是投影函数,
T
c
a
n
∈
S
E
(
3
)
T_{ca_n} ∈SE(3)
Tcan∈SE(3) 是从校准网格的坐标系到图像
n
n
n 的相机坐标系的变换。
K
K
K 是图像
n
n
n 的一组检测到的角点,
ρ
\rho
ρ 是鲁棒的 Huber 范数。
我们使用向量
Δ
s
=
[
Δ
i
,
Δ
t
0
,
.
.
.
,
Δ
t
N
]
T
\Delta \bold{s} = [\Delta \bold{i}, \Delta \bold{t}_0,...,\Delta \bold{t}_N ]^T
Δs=[Δi,Δt0,...,ΔtN]T 对状态的更新进行参数化,如下所示:
s
⊕
Δ
s
=
[
i
+
Δ
i
T
c
a
1
exp
(
Δ
t
1
)
.
.
.
T
c
a
N
exp
(
Δ
t
N
)
]
,
(
53
)
\bold{s} \oplus \Delta \bold{s}= \begin{bmatrix} \bold{i} + \Delta \bold{i} \\ \bold{T}_{ca_1} \text{exp}(\Delta \bold{t}_1) \\ ... \\ \bold{T}_{ca_N} \text{exp}(\Delta \bold{t}_N) \end{bmatrix}, \qquad (53)
s⊕Δs=⎣
⎡i+ΔiTca1exp(Δt1)...TcaNexp(ΔtN)⎦
⎤,(53)
给定当前状态
s
l
\bold{s}_l
sl,我们可以将优化函数重写为:
E
(
s
l
⊕
Δ
s
)
=
r
(
s
l
⊕
Δ
s
)
T
W
r
(
s
l
⊕
Δ
s
)
,
(
54
)
E(\bold{s}_l \oplus \Delta \bold{s}) = \bold{r}(\bold{s}_l \oplus \Delta \bold{s})^T \bold{Wr}(\bold{s}_l \oplus \Delta \bold{s}), \qquad (54)
E(sl⊕Δs)=r(sl⊕Δs)TWr(sl⊕Δs),(54)
并使用 Gauss-Newton 算法计算当前迭代的更新,如下所示:
Δ
s
=
(
J
l
T
W
J
l
)
−
1
J
l
T
W
r
l
,
(
55
)
\Delta \bold{s} = (\bold{J}_l^T \bold{W} \bold{J}_l)^{-1} \bold{J}_l^T \bold{Wr}_l, \qquad (55)
Δs=(JlTWJl)−1JlTWrl,(55)
其中
r
l
\bold{r}_l
rl 是在
s
l
\bold{s}_l
sl 处评估的残差的堆叠向量,
J
l
\bold{J}_l
Jl 是残差关于
Δ
s
\Delta \bold{s}
Δs 的雅可比行列式,
W
\bold{W}
W 是对应于 Huber 范数的加权矩阵。有了这个,我们更新了当前的状态估计
s
l
+
1
=
s
l
⊕
Δ
s
,
(
56
)
\bold{s}_{l+1} = \bold{s}_l \oplus \Delta \bold{s}, \qquad (56)
sl+1=sl⊕Δs,(56)
并迭代直到收敛。
由于优化函数是非凸的,内在参数
i
\bold{i}
i 和相机位姿
T
c
a
\bold{T}_{ca}
Tca 的良好初始化对于优化收敛很重要。我们使用先前提出的方法 [5] 初始化内在参数(EUCM 的
β
=
1
\beta = 1
β=1,DS 的
ξ
=
0
\xi = 0
ξ=0)并使用 UPnP 算法 [8] 找到初始位姿。
表 1:评估相机模型的平均重投影误差(以像素为单位)。最佳和次佳结果分别以绿色和橙色显示。该表还显示了与序列中重投影误差最小的模型相比的开销百分比。结果表明,所提出的模型尽管只有六个参数,但其平均重投影误差比具有八个参数的最佳性能模型大不到 1%。
5.评估
我们使用具有 16 个序列的数据集来评估所呈现的相机模型。该数据集包含使用五个不同镜头(每个镜头三个序列)捕获的校准序列和来自 EuRoC 数据集 [1] 的一个校准序列。用于收集序列的镜头如图 5 所示。为了确保公平比较,我们首先检测所有序列的校准角,并使用所有模型的相同数据执行第 4 节中描述的优化。
重投影误差表明模型能够很好地代表实际镜头的投影功能,是相机模型最重要的指标之一。 表 1 显示了优化后的平均重投影误差或使用不同相机模型为所有数据集计算的姿势和内在参数。 每个序列的最佳和次佳结果分别以绿色和橙色显示。 对于所有条目,我们还提供计算为
c
−
b
b
×
100
%
\frac{c-b}{b}×100 \%
bc−b×100% 的开销,其中
b
b
b 是序列中最小的重投影误差,
c
c
c 是当前模型的重投影误差。
图 6:针对不同相机型号优化相机位姿和内在参数后,投影到图像上的校准图案(紫色)的角落。 DS、EUCM 和 KB 8 模型显示出很高的重投影精度,而 UCM 和 KB 6 模型在校准图案的左下角有轻微的角位置偏移。对于 FOV 模型,左下角的位移清晰可见,这表明该模型与镜头的拟合效果不佳。
对于大多数序列,具有八个参数的 KB 模型显示出最好的结果,而提出的模型(DS)是次优的。尽管 KB 模型有 8 个内在参数,而建议的 DS 模型中有 6 个,但所有序列的重投影误差开销小于 1%。 EUCM 的重投影误差略大于 DS 模型,而重投影误差小于 KB 模型(具有六个参数)。 UCM 和 FOV 模型在所有测试模型中显示出更大的重投影误差。
计算时间是相机模型的另一个重要方面,因为在基于视觉的运动估计的每次迭代中,投影和非投影函数都会被评估数千次。此外,对于优化算法,我们必须计算这些函数相对于点和内在参数的雅可比行列;因此,还应考虑这些操作的计算时间。
表 2 总结了所提出模型的这些操作的计算时间。对于每个相机型号,我们使用 Intel Xeon E5-1620 CPU 上的 Google Benchmark (3)库测量 10000 次操作的时间。为了编译基准,我们使用具有 O3 优化级别的 GCC 7 并在单线程中执行代码。请注意,仅计算函数和使用雅可比计算函数之间的微小时间差异可以通过现代 CPU 的超标量架构来解释,该架构在内部并行执行。
表 2:在 Intel Xeon E5-1620 上测量的 10000 次操作的时间(以微秒为单位)。
J
\bold{J}
J 表示函数的雅可比行列式。结果表明,在具有相似精度的情况下,我们的模型显示投影函数的计算时间比 KB 8 模型快五倍左右。
时序结果表明,FOV 和 KB 模型比其他模型慢得多。例如,在评估投影函数时,具有八个参数的 KB 模型比 EUCM 慢大约 9 倍,比 DS 模型慢 5 倍。这是因为 KB 模型涉及计算量大的三角运算 ( a t a n 2 atan2 atan2)。
KB 模型中的非投影需要迭代优化来求解多项式根,这与三角运算一起使其比 UCM、EUCM 和 DS 模型慢几倍。 FOV 模型相对于非投影来说是最慢的,这可能是由于它的多个三角运算。
评估模型的重投影质量的定性结果如图 6 所示。在这里,我们在优化姿势和内在参数后投影校准图案的角,并将它们可视化在取自 BF2M2020S23-3 序列的相应图像上.
DS EUCM 和 KB 8 模型提供了人眼难以区分的相似结果。 UCM 和 KB 6 模型很好地拟合了图像中间的角落;然而,这些模型在靠近边缘的地方有一个小的偏移。请注意,使用 FOV 模型可以清楚地看到缺陷。
表 3:在每个镜头的三个不同序列上计算的内在参数的平均值和标准偏差(以 % 为单位)。结果表明,我们的 UCM 公式(第一列,比较等式 7)与标准公式 [9](第二列)相比具有更小的标准偏差,其中
γ
\gamma
γ 和
ξ
\xi
ξ 的变化对彼此有显着影响。
根据结果的数值稳定性评估 UCM 的不同公式。表 3 显示了在每个镜片的三个不同序列上计算的内在参数的平均值和标准偏差(以 % 为单位)。对于 UCM,我们提供了两个具有相同重投影误差的公式,它们是用不同的内在参数公式化的。文献 [9] ( i = [ γ x , γ y , c x , c y , ξ ] T ) (\bold{i} = [\gamma_x,\gamma_y,c_x,c_y,\xi ]^T ) (i=[γx,γy,cx,cy,ξ]T) 中定义的标准公式的结果显示在第二列中,并显示出比使用参数化的模型的结果更高的标准偏差 i = [ f x , f y , c x , c y , α ] T \bold{i} = [f_x,f_y,c_x,c_y,α]^T i=[fx,fy,cx,cy,α]T 。这可以通过 γ x \gamma_x γx、 γ y \gamma_y γy 和 ξ \xi ξ 之间的强耦合来解释,而建议的参数化并非如此。此外,对于这个公式,焦距保持接近其他相机型号的焦距。
6.总结
在本文中,我们介绍了非常适合鱼眼相机的新型双球相机模型。我们将提出的相机模型与其他最先进的相机模型进行比较。此外,我们使用 16 种不同的校准序列和 6 种不同的镜头对所展示的相机模型进行了广泛的评估。评估结果表明,基于高阶多项式(如 KB 8)的模型显示出最低的重投影误差,但比竞争模型慢 5-10 倍。提出的 DS 模型和 EUCM 都显示出非常低的重投影误差,DS 模型稍微更准确(与所有序列上的 KB 8 相比,重投影误差大不到 1%),而 EUCM 稍快一些(投影评估比 KB 8 快 9 倍)。此外,这两个模型都具有封闭形式的逆,并且不需要计算昂贵的三角运算。这些结果表明,对于需要快速投影、非投影和封闭形式逆的应用,基于球面投影的模型可以很好地替代基于高阶多项式的模型。
原文参考文献:
[1] M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. W. Achtelik, and R. Siegwart. The EuRoC micro aerial vehicle datasets. The International Journal of Robotics Research, 2016.
[2] F. Devernay and O. Faugeras. Straight lines have to be straight. Machine vision and applications, 13(1):14–24, 2001.
[3] P. Furgale, J. Rehder, and R. Siegwart. Unified temporal and spatial calibration for multi-sensor systems. In 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 1280–1286, Nov 2013.
[4] C. Geyer and K. Daniilidis. A unifying theory for central panoramic systems and practical implications. In D. Ver- non, editor, Computer Vision — ECCV 2000, pages 445–461, Berlin, Heidelberg, 2000. Springer Berlin Heidelberg.
[5] L. Heng, G. H. Lee, and M. Pollefeys. Self-calibration and visual slam with a multi-camera system on a micro aerial vehicle. Autonomous robots, 39(3):259–277, 2015.
[6] J. Kannala and S. S. Brandt. A generic camera model and cal- ibration method for conventional, wide-angle, and fish-eye lenses. IEEE transactions on pattern analysis and machine intelligence, 28(8):1335–1340, 2006.
[7] B. Khomutenko, G. Garcia, and P. Martinet. An enhanced unified camera model. IEEE Robotics and Automation Let- ters, 1(1):137–144, Jan 2016.
[8] L. Kneip, H. Li, and Y. Seo. Upnp: An optimal o (n) solution to the absolute pose problem with universal applicability. In European Conference on Computer Vision, pages 127–142. Springer, 2014.
[9] C. Mei and P. Rives. Single view point omnidirectional camera calibration from planar grids. In Proceedings 2007 IEEE International Conference on Robotics and Automation, pages 3945–3950, April 2007.
[10] E. Olson. AprilTag: A robust and flexible visual fiducial sys- tem. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pages 3400–3407. IEEE, May 2011.
[11] A. Rituerto, L. Puig, and J. Guerrero. Comparison of om- nidirectional and conventional monocular systems for visual slam. In 10th OMNIVIS with RSS, 2010.
[12] D. Scaramuzza, A. Martinelli, and R. Siegwart. A flexible technique for accurate omnidirectional camera calibration and structure from motion. In Fourth IEEE International Conference on Computer Vision Systems (ICVS’06), pages 45–45, Jan 2006.
[13] X. Ying and Z. Hu. Can we consider central catadiop- tric cameras and fisheye cameras within a unified imaging model. In T. Pajdla and J. Matas, editors, Computer Vi- sion - ECCV 2004, pages 442–455, Berlin, Heidelberg, 2004. Springer Berlin Heidelberg.
[14] Z. Zhang, H. Rebecq, C. Forster, and D. Scaramuzza. Benefit of large field-of-view cameras for visual odometry. In 2016 IEEE International Conference on Robotics and Automation (ICRA), pages 801–808, May 2016.