相机模型之鱼眼相机模型（Equidistant/Kannala-Brandt)
前言
视觉SLAM、SFM方法中少不了要和各种相机模型打交道，这里把经常碰到的各种相机模型做个系统性的梳理。
Equidistant投影模型
Equidistant模型是由Kannala-Brandt提出的相机模型，描述径向畸变，适用于FOV不超过170°的大广角相机，但不适合FOV更大的全景相机模型。
需要说明的是，鱼眼相机和鱼眼相机模型是两回事。通常FOV很大，畸变比较显著的相机都可以叫鱼眼相机，例如EUCM模型、double sphere模型的相机；但是鱼眼模型则特指Equidistant 模型。
假设相机当前姿态用SE3群表示，将W系中的点转化为相机坐标系，则世界坐标系中一个3d点
P
w
P_w
Pw
到像极坐标系下一点
P
c
P_c
Pc的变换关系为：
P
c
=
R
c
w
P
w
+
t
c
w
P_c=R_{cw}P_w+t_{cw}
Pc=RcwPw+tcw
对于大多数相机FOV都不超过180°，则意味着镜头不能看到后面的物体，因而判断
P
c
P_c
Pc的z分量是否大于0，可以作为一个点是否能被相机看见的简单依据；除此以外，点能否被看见还与相机的视野有关系。
对于相机投影来说，只需要知道
P
c
P_c
Pc就够了，不关心
P
w
P_w
Pw，求
P
w
P_w
Pw一般是SFM和SLAM的任务。
严格的说，相机只关心
P
c
P_c
Pc的方向，模长或者深度都不关心。假设
P
c
P_c
Pc在相机平面上的投影成像点是
(
u
d
,
v
d
)
(u_d,v_d)
(ud,vd)，则投影过程是：
π
(
P
c
)
=
[
f
x
d
(
θ
)
cos
ϕ
+
c
x
f
y
d
(
θ
)
sin
ϕ
+
c
y
]
=
[
u
d
v
d
]
\pi(P_c)=\left[ \begin{array}{c} f_xd(\theta )\cos\phi+c_x\\ f_yd(\theta)\sin\phi +c_y \end{array} \right]= \left[ \begin{array}{c} u_d \\ v_d \end{array} \right]\\
π(Pc)=[fxd(θ)cosϕ+cxfyd(θ)sinϕ+cy]=[udvd]
其中，
tan
(
θ
)
=
X
c
2
+
Y
c
2
Z
c
ϕ
=
a
t
a
n
2
(
Y
c
,
X
c
)
P
c
=
[
X
c
,
Y
c
,
Z
c
]
=
[
r
x
y
cos
ϕ
,
r
x
y
sin
ϕ
,
r
x
y
/
tg
θ
]
d
(
θ
)
=
θ
(
1
+
k
2
θ
2
+
k
3
θ
4
+
k
4
θ
6
+
k
5
θ
8
)
\tan(\theta)=\frac{\sqrt{X_c^2+Y_c^2}}{Z_c}\\ \phi=atan2(Y_c,X_c)\\ P_c=[X_c,Y_c,Z_c] =[r_{xy}\cos\phi,r_{xy}\sin\phi,r_{xy}/\tg\theta] \\ d(\theta) =\theta (1+k_{2}\theta^2+k_3\theta^4+k_4\theta^6+k_5\theta^8)
tan(θ)=ZcXc2+Yc2ϕ=atan2(Yc,Xc)Pc=[Xc,Yc,Zc]=[rxycosϕ,rxysinϕ,rxy/tgθ]d(θ)=θ(1+k2θ2+k3θ4+k4θ6+k5θ8)
需要注意的是，并不是
k
2
,
k
3
,
k
4
,
k
5
k_2,k_3,k_4,k_5
k2,k3,k4,k5参数等于0就对应于无畸变投影，只有当
d
(
θ
)
=
tg
(
θ
)
d(\theta)=\tg(\theta)
d(θ)=tg(θ)时才对应于无畸变的针孔相机模型。
由于
θ
\theta
θ趋向于90°时，
d
(
θ
)
d(\theta)
d(θ)函数有奇异性，因此该模型无法表示超过180°的FOV。实际使用中，
θ
\theta
θ不能超过85°。
反投影（lift 过程，参考OpenCV实现）
从3d点投影到相平面上，损失了深度信息。从图像上一个像素到3点入射方向的映射，就是反投影过程，又叫lift过程。
假设入射的点按照理想的针孔相机模型投影到
(
u
u
,
v
u
)
(u_u,v_u)
(uu,vu)点，则lift到z=1的平面的操作定义为：
π
−
1
(
u
d
,
v
d
,
z
=
1
)
=
[
u
u
−
c
x
f
x
v
u
−
c
y
f
y
1
]
d
(
θ
)
=
(
u
d
−
c
x
f
x
)
2
+
(
v
d
−
c
y
f
y
)
2
θ
=
d
−
1
(
θ
)
\pi^{-1}(u_d,v_d,z=1) = \left[ \begin{array}{c} \frac{u_u-c_x}{f_x} \\ \frac{v_u-c_y}{f_y} \\ 1 \end{array} \right] \\ d(\theta)=\sqrt{(\frac{u_d-c_x}{f_x})^2+(\frac{v_d-c_y}{f_y})^2} \\ \theta=d^{-1}(\theta)
π−1(ud,vd,z=1)=⎣⎡fxuu−cxfyvu−cy1⎦⎤d(θ)=(fxud−cx)2+(fyvd−cy)2θ=d−1(θ)
直接解析求
d
−
1
(
θ
)
d^{-1}(\theta)
d−1(θ)是不现实的，OpenCV采用的是迭代求解方法，速度也很快。
初始首先令
θ
d
=
(
u
d
−
c
x
f
x
)
2
+
(
v
d
−
c
y
f
y
)
2
\theta_d=\sqrt{(\frac{u_d-c_x}{f_x})^2+(\frac{v_d-c_y}{f_y})^2}
θd=(fxud−cx)2+(fyvd−cy)2，利用如下关系：
θ
d
=
θ
(
1
+
k
2
θ
2
+
k
3
θ
4
+
k
4
θ
6
+
k
5
θ
8
)
θ
=
θ
d
(
1
+
k
2
θ
2
+
k
3
θ
4
+
k
4
θ
6
+
k
5
θ
8
)
\theta_d=\theta (1+k_{2}\theta^2+k_3\theta^4+k_4\theta^6+k_5\theta^8) \\ \theta=\frac{\theta_d}{ (1+k_{2}\theta^2+k_3\theta^4+k_4\theta^6+k_5\theta^8)}
θd=θ(1+k2θ2+k3θ4+k4θ6+k5θ8)θ=(1+k2θ2+k3θ4+k4θ6+k5θ8)θd
迭代求解格式：
θ
0
=
θ
d
θ
i
+
1
=
θ
d
(
1
+
k
2
θ
i
2
+
k
3
θ
i
4
+
k
4
θ
i
6
+
k
5
θ
i
8
)
\theta_{0} = \theta_d \\ \theta_{i+1}=\frac{\theta_d}{ (1+k_{2}\theta_{i}^2+k_3\theta_{i}^4+k_4\theta_{i}^6+k_5\theta_{i}^8)}
θ0=θdθi+1=(1+k2θi2+k3θi4+k4θi6+k5θi8)θd
OpenCV固定迭代10次。一旦求解出
θ
\theta
θ，
(
u
u
,
v
u
)
(u_u,v_u)
(uu,vu)与
(
u
d
,
v
d
)
(u_d,v_d)
(ud,vd)的关系如下：
[
u
u
−
c
x
v
u
−
c
y
]
=
[
u
d
−
c
x
v
d
−
c
y
]
tg
(
θ
)
d
(
θ
)
\left[ \begin{array}{l} u_u-c_x \\ v_u-c_y \end{array} \right] = \left[ \begin{array}{l} u_d-c_x \\ v_d-c_y \end{array} \right] \frac{\tg(\theta)}{d(\theta)}
[uu−cxvu−cy]=[ud−cxvd−cy]d(θ)tg(θ)
事实上，对于pinhole radial-tangential 畸变，也是采用类似的迭代方法求解。