一、小孔成像模型
世界坐标系到相机坐标系
[
x
c
y
c
z
c
]
=
R
[
x
w
y
w
z
w
]
+
t
即
[
x
c
y
c
z
c
1
]
=
[
R
t
0
T
1
]
[
x
w
y
w
z
w
1
]
=
T
P
w
(1.0)
\begin{bmatrix} x_{c}\\y_{c}\\z_{c} \end{bmatrix} = R\begin{bmatrix} x_{w}\\y_{w}\\z_{w}\end{bmatrix}+t \\\;\\ 即\begin{bmatrix} x_{c}\\y_{c}\\z_{c} \\1\end{bmatrix}=\begin{bmatrix} R&t\\0^{T}&1 \end{bmatrix}\begin{bmatrix} x_{w}\\y_{w}\\z_{w}\\1 \end{bmatrix}=TP_{w}\tag{1.0}
xcyczc
=R
xwywzw
+t即
xcyczc1
=[R0Tt1]
xwywzw1
=TPw(1.0)
相机坐标系到像平面的投影
由三角形的相似性有:
z
c
f
=
x
c
x
c
′
=
y
c
y
c
′
(1.1)
\frac{z_{c}}{f} = \frac{x_{c}}{x_{c}^{'}} = \frac{y_{c}}{y_{c}^{'}}\tag{1.1}
fzc=xc′xc=yc′yc(1.1)
即:
[
x
c
′
y
c
′
]
=
[
f
x
c
z
c
f
y
c
z
c
]
(1.2)
\begin{bmatrix}x_{c}^{'}\\ \\y^{'}_{c}\end{bmatrix}=\begin{bmatrix}f\frac{x_{c}}{z_{c}}\\\\f\frac{y_{c}}{z_{c}}\end{bmatrix}\tag{1.2}
xc′yc′
=
fzcxcfzcyc
(1.2)
其中
(
x
c
′
,
y
c
′
)
(x_{c}^{'}, y_{c}^{'})
(xc′,yc′)表示像平面坐标,坐标原点在像平面中心,
(
x
c
,
y
c
)
(x_{c}, y_{c})
(xc,yc)表示点在相机坐标系内的坐标。
像素坐标系:原点
o
′
o^{'}
o′在图像的左上角,
u
u
u轴与x轴方向平行,
u
u
u轴向右与
x
x
x轴方向平行,
v
v
v轴向下与
y
y
y轴平行。像素坐标系与成像平面之间相差一个缩放和一个原点的平移。设像素坐标在
u
u
u轴上缩放了
α
\alpha
α倍,在
v
v
v轴上缩放
β
\beta
β倍,同时原点平移了
[
u
0
,
v
0
]
T
[u_{0}, v_{0}]^T
[u0,v0]T。那么,
P
P
P的坐标与像素坐标的关系如(1.3)所示:
[
u
v
]
=
[
α
x
c
′
+
u
0
β
y
c
′
+
v
0
]
(1.3)
\begin{bmatrix}u \\\\v\end{bmatrix}= \begin{bmatrix}\alpha x_{c}^{'} + u_{0}\\\\\beta y_{c}^{'} + v_{0}\end{bmatrix}\tag{1.3}
uv
=
αxc′+u0βyc′+v0
(1.3)
将(1.2)带入(1.3)中有:
[
u
v
]
=
[
f
x
x
c
z
c
+
u
0
f
y
y
c
z
c
+
v
0
]
(1.4)
\begin{bmatrix}u\\\\v \end{bmatrix} = \begin{bmatrix}f_{x}\frac{x_{c}}{z_{c}} + u_{0}\\\\f_{y}\frac{y_{c}}{z_{c}} + v_{0}\end{bmatrix}\tag{1.4}
uv
=
fxzcxc+u0fyzcyc+v0
(1.4)
其中,
f
f
f的单位为米,
α
\alpha
α、
β
\beta
β的单位为像素每米,所以
f
x
f_{x}
fx,
f
y
f_{y}
fy的单位为像素,将其写成矩阵形式如式(1.5)所示:
z
c
{
u
v
1
}
=
{
f
x
0
u
0
0
f
y
v
0
0
0
1
}
{
x
c
y
c
z
c
}
=
K
P
c
(1.5)
z_{c}\left\{ \begin{matrix} u \\ v \\ 1 \end{matrix} \right\} = \left\{ \begin{matrix} f_{x} & 0 & u_{0} \\ 0 & f_{y} & v_{0}\\ 0 &0&1 \end{matrix} \right\}\left\{ \begin{matrix} x_{c} \\ y_{c} \\ z_{c} \end{matrix} \right\} =KP_{c}\tag{1.5}
zc⎩
⎨
⎧uv1⎭
⎬
⎫=⎩
⎨
⎧fx000fy0u0v01⎭
⎬
⎫⎩
⎨
⎧xcyczc⎭
⎬
⎫=KPc(1.5)
其中:K为相机的内参矩阵(Camera Intrinsics),
P
c
P_{c}
Pc为相机坐标系下的坐标。
二、相机成像模型
z
c
{
u
v
1
}
=
K
(
R
P
w
+
t
)
=
K
T
P
w
(2.1)
z_{c}\left\{\begin{matrix} u\\v\\1 \end{matrix}\right\} = K(RP_{w}+t) = KTP_{w}\tag{2.1}
zc⎩
⎨
⎧uv1⎭
⎬
⎫=K(RPw+t)=KTPw(2.1)
其中
P
w
P_{w}
Pw为世界坐标系下的坐标,R为世界坐标系到相机坐标系的旋转矩阵,t为平移矩阵。(R, t)称为相机的外参数。
(
u
v
1
)
=
1
z
c
(
f
α
0
u
0
0
f
β
v
0
0
0
1
)
[
R
t
0
T
1
]
(
x
w
y
w
z
w
1
)
=
1
z
c
K
[
R
t
]
(
x
w
y
w
z
w
1
)
(2.2)
\left(\begin{array}{c} u \\ v \\ 1 \end{array}\right)=\frac{1}{z_{c}}\left(\begin{array}{ccc} f_{\alpha} & 0 & u_{0} \\ 0 & f_{\beta} & v_{0} \\ 0 & 0 & 1 \end{array}\right)\left[\begin{array}{cc} \boldsymbol{R} & \boldsymbol{t} \\ \mathbf{0}^{T} & 1 \end{array}\right]\left(\begin{array}{c} x_{w} \\ y_{w} \\ z_{w} \\ 1 \end{array}\right)=\frac{1}{z_{c}} \boldsymbol{K}\left[\begin{array}{ll} \boldsymbol{R} & \boldsymbol{t} \end{array}\right]\left(\begin{array}{c} x_{w} \\ y_{w} \\ z_{w} \\ 1 \end{array}\right)\tag{2.2}
uv1
=zc1
fα000fβ0u0v01
[R0Tt1]
xwywzw1
=zc1K[Rt]
xwywzw1
(2.2)
三、径向畸变
由于相机透镜的成像模型不能完全达到理想的小孔成像模型,因此会发生径向畸变与切向畸变,一般在归一化像平面上讨论径向畸变与切向畸变。
x
=
x
c
o
r
r
e
c
t
e
d
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
y
=
y
c
o
r
r
e
c
t
e
d
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
(3.1)
x = x_{corrected}(1+k_{1}r^{2}+k_{2}r^{4}+k_{3}r^{6})\\y = y_{corrected}(1+k_{1}r^{2}+k_{2}r^{4}+k_{3}r^{6})\tag{3.1}
x=xcorrected(1+k1r2+k2r4+k3r6)y=ycorrected(1+k1r2+k2r4+k3r6)(3.1)
[
x
,
y
]
T
[x, y]^{T}
[x,y]T是相机拍摄有畸变图像上的点,
[
x
c
o
r
r
e
c
t
e
d
,
y
c
o
r
r
e
c
t
e
d
]
T
[x_{corrected}, y_{corrected}]^{T}
[xcorrected,ycorrected]T是经过矫正后的点。他们都是归一化像平面上的点。得到矫正图像的过程是:比如现在求矫正后图像上(100,100)这个点的像素的灰度值,就将
(
x
c
o
r
r
e
c
t
e
d
,
y
c
o
r
r
e
c
t
e
d
)
=
(
100
,
100
)
(x_{corrected}, y_{corrected}) = (100, 100)
(xcorrected,ycorrected)=(100,100)代入(3.1)式中,得到(x,y)的坐标值(如果x,y不是整数,需要用双线性插值求取),然后将畸变图像(x,y)上的像素灰度值copy到矫正后图像(100,100)坐标上。
通常我们只用
k
1
,
k
2
k_{1},k_{2}
k1,k2来矫正相机,对于畸变较小的图像中心区域,主要是
k
1
k_{1}
k1在起作用,对于畸变较大的图像的边缘区域,主要是
k
2
k_{2}
k2在起作用。一般对于鱼眼相机,才会使用
k
3
k_{3}
k3去矫正相机的畸变。
四、对极几何
4.1 模型推导
其中 O 1 O 2 P O_{1}O_{2}P O1O2P称为极平面, e 1 , e 2 e_{1},e_{2} e1,e2称为极点, O 1 O 2 O_{1}O_{2} O1O2称为基线, l 1 , l 2 l_{1}, l_{2} l1,l2称为极线,极线的意义在于,如果要在右视图搜索左视图中的 x 1 x_{1} x1点, x l x_{l} xl点在右视图的同名点一定落在右视图的极线 x 2 x_{2} x2上,这样可以减小搜索范围,提高搜索效率.极线校正的目的也是如此.
不妨假设相机坐标系和
O
1
O_{1}
O1坐标系一致,P的相机坐标为
X
X
X
由式(1.5)得:
d
1
x
1
=
K
1
X
d
2
x
2
=
K
2
(
R
X
+
t
)
(4.1)
d_{1}x_{1} = K_{1}X\\d_{2}x_{2} = K_{2}(RX+t)\tag{4.1}
d1x1=K1Xd2x2=K2(RX+t)(4.1)
其中
K
K
K为内参矩阵,
R
,
t
R, t
R,t为相机2相对相机1的外参矩阵。
由(4.1)与(4.2)得:
d
1
K
1
−
1
x
1
=
X
d
2
K
2
−
1
x
2
=
R
X
+
t
(4.2)
d_{1}K_{1}^{-1}x_{1} = X\\d_{2}K_{2}^{-1}x_{2} = RX+t\tag{4.2}
d1K1−1x1=Xd2K2−1x2=RX+t(4.2)
记
x
^
1
=
K
1
−
1
x
1
\hat{x}_{1}=K_{1}^{-1}x_{1}
x^1=K1−1x1,
x
^
2
=
K
2
−
1
x
2
\hat{x}_{2}=K_{2}^{-1}x_{2}
x^2=K2−1x2,则有:
d
1
x
^
1
=
X
d
2
x
^
2
=
R
X
+
t
(4.3)
d_{1}\hat{x}_{1} = X\\d_{2}\hat{x}_{2}=RX+t \tag{4.3}
d1x^1=Xd2x^2=RX+t(4.3)
结合(4.3)两个式子:
d
2
x
^
2
=
d
1
R
x
^
1
+
t
(4.4)
d_{2}\hat{x}_{2} = d_{1}R\hat{x}_{1}+t\tag{4.4}
d2x^2=d1Rx^1+t(4.4)
不妨假设像平面为归一化像平面,即
d
1
=
d
2
=
1
d_{1}=d_{2}=1
d1=d2=1,则有:
x
^
2
=
R
x
^
1
+
t
(4.5)
\hat{x}_{2} = R\hat{x}_{1}+t\tag{4.5}
x^2=Rx^1+t(4.5)
对(4.5)两侧同时左乘 t 的反对称(也就是和t做叉乘)得:
t
∗
x
^
2
=
t
∗
R
x
^
1
+
0
(4.6)
t^{*}\hat{x}_{2}=t^{*}R\hat{x}_{1}+0\tag{4.6}
t∗x^2=t∗Rx^1+0(4.6)
对(4.6)两侧同时左乘
x
2
T
x_{2}^{T}
x2T得:
x
^
2
T
t
∗
x
^
2
=
x
^
2
T
t
∗
R
x
^
1
+
0
(4.7)
\hat{x}_{2}^{T}t^{*}\hat{x}_{2}=\hat{x}_{2}^{T}t^{*}R\hat{x}_{1}+0\tag{4.7}
x^2Tt∗x^2=x^2Tt∗Rx^1+0(4.7)
观察(4.7)左侧,
t
∗
t^{*}
t∗与
x
^
2
\hat{x}_{2}
x^2做完叉积的向量一定垂直与
x
^
2
\hat{x}_{2}
x^2,所以左侧为0,即对极约束:
x
^
2
T
t
∗
R
x
^
1
=
0
(4.8)
\hat{x}_{2}^{T}t^{*}R\hat{x}_{1}=0\tag{4.8}
x^2Tt∗Rx^1=0(4.8)
将
x
^
1
=
K
1
−
1
x
1
\hat{x}_{1}=K_{1}^{-1}x_{1}
x^1=K1−1x1,
x
^
2
=
K
2
−
1
x
2
\hat{x}_{2}=K_{2}^{-1}x_{2}
x^2=K2−1x2代入(4.8)中得带内参的对极约束:
x
2
T
K
2
−
T
t
∗
R
K
1
−
1
x
1
=
0
(4.9)
x_{2}^{T}K_{2}^{-T}t^{*}RK_{1}^{-1}x_{1}=0\tag{4.9}
x2TK2−Tt∗RK1−1x1=0(4.9)
定义:
本质矩阵(Essential Matrix):
E
=
t
∗
R
(4.10)
E=t^{*}R\tag{4.10}
E=t∗R(4.10)
基础矩阵(Funfamental Matrix):
F
=
K
2
−
T
t
∗
R
K
1
−
1
=
K
2
−
T
E
K
1
−
1
(4.11)
F=K_{2}^{-T}t^{*}RK_{1}^{-1} = K_{2}^{-T}EK_{1}^{-1}\tag{4.11}
F=K2−Tt∗RK1−1=K2−TEK1−1(4.11)
4.2 基础矩阵F求解
基础矩阵F的性质:
- 维度为3x3,秩为2
- 具有7个自由度
- 奇异值为 [ δ 1 , δ 2 , 0 ] T [\delta_{1}, \delta_{2}, 0]^{T} [δ1,δ2,0]T
- 极线约束:
l
1
=
x
2
T
F
l_{1}=x_{2}^{T}F
l1=x2TF ,
l
2
=
x
1
T
F
l_{2}=x_{1}^{T}F
l2=x1TF ,
x
2
T
F
x
1
=
0
x_{2}^{T}Fx_{1}=0
x2TFx1=0
基础矩阵的求解:- 直接线性变换法
8点法
最小二乘法 - 基于Ransac的方法
- 直接线性变换法
4.2.1 直接线性变换法
对于一对匹配点
x
1
=
[
u
1
,
v
1
,
1
]
T
,
x
2
=
[
u
2
,
v
2
,
1
]
T
\boldsymbol{x}_{1}=\left[\begin{array}{lllll} u_{1}, & v_{1}, & 1 \end{array}\right]^{\mathrm{T}}, \boldsymbol{x}_{2}=\left[\begin{array}{lll} u_{2}, & v_{2}, & 1 \end{array}\right]^{\mathrm{T}}
x1=[u1,v1,1]T,x2=[u2,v2,1]T,根据对极约束
x
2
T
F
x
1
=
0
\boldsymbol{x}_{2}^{T} \boldsymbol{F} \boldsymbol{x}_{1}=\mathbf{0}
x2TFx1=0有:
(
u
1
v
1
1
)
[
F
11
F
12
F
13
F
21
F
22
F
23
F
31
F
32
F
33
]
(
u
2
v
2
1
)
=
0
(4.12)
\left(\begin{array}{lll} u_{1} & v_{1} & 1 \end{array}\right)\left[\begin{array}{ccc} F_{11} & F_{12} & F_{13} \\ F_{21} & F_{22} & F_{23} \\ F_{31} & F_{32} & F_{33} \end{array}\right]\left(\begin{array}{c} u_{2} \\ v_{2} \\ 1 \end{array}\right)=0\tag{4.12}
(u1v11)
F11F21F31F12F22F32F13F23F33
u2v21
=0(4.12)
令
f
=
[
F
11
,
F
12
,
F
13
,
F
21
,
F
22
,
F
23
,
F
31
,
F
32
,
F
33
]
T
\boldsymbol{f}=\left[\begin{array}{lllllllll} F_{11}, & F_{12}, & F_{13}, & F_{21}, & F_{22}, & F_{23}, & F_{31}, & F_{32}, & F_{33} \end{array}\right]^{T}
f=[F11,F12,F13,F21,F22,F23,F31,F32,F33]T则有:
[
u
1
u
1
,
u
1
v
2
,
u
1
,
v
2
u
1
,
v
1
v
2
,
v
1
,
u
2
,
v
2
,
1
]
f
=
0
(4.13)
\left[u_{1} u_{1}, \quad u_{1} v_{2}, \quad u_{1}, \quad v_{2} u_{1}, \quad v_{1} v_{2}, \quad v_{1}, \quad u_{2}, \quad v_{2}, \quad 1\right] \boldsymbol{f}=0\tag{4.13}
[u1u1,u1v2,u1,v2u1,v1v2,v1,u2,v2,1]f=0(4.13)
其中每对匹配点提供一个约束。
当存在n对匹配点时有:
A
f
=
0
(4.14)
Af=0\tag{4.14}
Af=0(4.14)
其中:
A
=
(
u
1
(
1
)
u
1
(
1
)
,
u
1
(
1
)
v
2
(
1
)
,
u
1
(
1
)
,
v
1
(
1
)
u
2
(
1
)
,
v
1
(
1
)
v
2
(
1
)
,
v
1
(
1
)
,
u
2
(
1
)
,
v
2
(
1
)
,
1
u
1
(
2
)
u
1
(
2
)
,
u
1
(
2
)
v
2
(
2
)
,
u
1
(
2
)
,
v
1
(
2
)
u
2
(
2
)
,
v
1
(
2
)
v
2
(
2
)
,
v
1
(
2
)
,
u
2
(
2
)
,
v
2
(
2
)
,
1
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
u
1
(
n
)
u
1
(
n
)
,
u
1
(
n
)
v
2
(
n
)
,
u
1
(
n
)
,
v
1
(
n
)
u
2
(
n
)
,
v
1
(
n
)
v
2
(
n
)
,
v
1
(
n
)
,
u
2
(
n
)
,
v
2
(
n
)
,
1
)
\boldsymbol{A}=\left(\begin{array}{ccccccccc} u_{1}^{(1)} u_{1}^{(1)}, & u_{1}^{(1)} v_{2}^{(1)}, & u_{1}^{(1)}, & v_{1}^{(1)} u_{2}^{(1)}, & v_{1}^{(1)} v_{2}^{(1)}, & v_{1}^{(1)}, & u_{2}^{(1)}, & v_{2}^{(1)}, & 1 \\ u_{1}^{(2)} u_{1}^{(2)}, & u_{1}^{(2)} v_{2}^{(2)}, & u_{1}^{(2)}, & v_{1}^{(2)} u_{2}^{(2)}, & v_{1}^{(2)} v_{2}^{(2)}, & v_{1}^{(2)}, & u_{2}^{(2)}, & v_{2}^{(2)}, & 1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ u_{1}^{(n)} u_{1}^{(n)}, & u_{1}^{(n)} v_{2}^{(n)}, & u_{1}^{(n)}, & v_{1}^{(n)} u_{2}^{(n)}, & v_{1}^{(n)} v_{2}^{(n)}, & v_{1}^{(n)}, & u_{2}^{(n)}, & v_{2}^{(n)}, & 1 \end{array}\right)
A=
u1(1)u1(1),u1(2)u1(2),⋮u1(n)u1(n),u1(1)v2(1),u1(2)v2(2),⋮u1(n)v2(n),u1(1),u1(2),⋮u1(n),v1(1)u2(1),v1(2)u2(2),⋮v1(n)u2(n),v1(1)v2(1),v1(2)v2(2),⋮v1(n)v2(n),v1(1),v1(2),⋮v1(n),u2(1),u2(2),⋮u2(n),v2(1),v2(2),⋮v2(n),11⋮1
因此,要保证模型有唯一解,至少需要8对匹配点。
- 若n=8,且A为非奇异矩阵,则f有唯一解,这种求解方式称为8点法。
- 若 n > = 8 n >=8 n>=8,该方程是一个超定方程组,使用最小二乘法求解f, A T A A^{T}A ATA最小的特征值对应的特征向量为最优解。
4.2.2 奇异值约束
直接线性变换法无法保证基础矩阵的奇异值约束——有两个非0的奇异值,为此使用奇异值约束对矩阵进行重构,目标如式4.15所示:
min
∥
F
−
F
^
∥
,
wrt.
svd
(
F
)
=
[
σ
1
,
σ
2
,
0
]
(4.15)
\min \|F-\hat{F}\|, \text { wrt. } \operatorname{svd}(F)=\left[\begin{array}{lll} \sigma_{1}, & \sigma_{2}, & 0 \end{array}\right]\tag{4.15}
min∥F−F^∥, wrt. svd(F)=[σ1,σ2,0](4.15)
上式表示在保证最小重构误差条件下,使重构后的F矩阵满足奇异值约束。
首先对求得的基础矩阵
F
^
\hat{\boldsymbol{F}}
F^进行奇异值分解,得到奇异值矩阵S:
F
^
=
U
S
V
T
with
S
=
diag
(
σ
1
,
σ
2
,
σ
3
)
(4.16)
\hat{\boldsymbol{F}}=\boldsymbol{U S V}^{T} \text { with } \boldsymbol{S}=\operatorname{diag}\left(\sigma_{1}, \sigma_{2}, \sigma_{3}\right)\tag{4.16}
F^=USVT with S=diag(σ1,σ2,σ3)(4.16)
再将奇异值矩阵的第三个奇异值直接置为0,还原得到重构后的F矩阵:
F
=
U
diag
(
σ
1
,
σ
2
,
0
)
V
T
(4,17)
\boldsymbol{F}=\boldsymbol{U} \operatorname{diag}\left(\sigma_{1}, \quad \sigma_{2}, \quad 0\right) \boldsymbol{V}^{T}\tag{4,17}
F=Udiag(σ1,σ2,0)VT(4,17)
4.2.2 RANSAC-估计基础矩阵
算法流程:
- 随机采样8对匹配点 ( x 1 ( n ) , x 2 ( n ) ) \left(x_{1}^{(n)}, x_{2}^{(n)}\right) (x1(n),x2(n))
- 使用8点法求解基础矩阵 F \boldsymbol{F} F
- 使用奇异值约束获取重构矩阵 F F F
- 使用 d ( x 1 , x 2 ) = ( x 2 T F x 1 ) 2 ( F x 1 ) x 2 + ( F x 1 ) y 2 + ( x 2 T F ) x 2 + ( x 2 T F ) y 2 d\left(\boldsymbol{x}_{1}, \boldsymbol{x}_{2}\right)=\frac{\left(\boldsymbol{x}_{2}^{T} \boldsymbol{F} \boldsymbol{x}_{1}\right)^{2}}{\left(\boldsymbol{F} \boldsymbol{x}_{1}\right)_{x}^{2}+\left(\boldsymbol{F} \boldsymbol{x}_{1}\right)_{y}^{2}+\left(\boldsymbol{x}_{2}^{T} \boldsymbol{F}\right)_{x}^{2}+\left(\boldsymbol{x}_{2}^{T} \boldsymbol{F}\right)_{y}^{2}} d(x1,x2)=(Fx1)x2+(Fx1)y2+(x2TF)x2+(x2TF)y2(x2TFx1)2计算误差,并通过 d ( x 1 , x 2 ) < τ d(x_{1}, x_{2})<\tau d(x1,x2)<τ统计内点个数
- 重复以上过程,选择内点最多的结果
- 对所有的内点重复2、3步骤,重新计算 F F F
4.3 本质矩阵E求解
本质矩阵的性质:
- 维度为3x3,秩为2
- 具有5个自由度
- 奇异值为 [ δ , δ , 0 ] T [\delta, \delta, 0]^{T} [δ,δ,0]T
求解本质矩阵E步骤(相机内参 K 1 K_{1} K1和 K 2 K_{2} K2已知):
- 求解基础矩阵F
- 由 E ^ = K 2 T F K 1 \hat{E}=K_{2}^TFK_{1} E^=K2TFK1求得估计的 E ^ \hat{E} E^
- 将 E ^ \hat{E} E^进行奇异值分解, E ^ = U d i a g ( δ 1 , δ 2 , 0 ) V T \hat{E}=Udiag(\delta_{1}, \delta_{2}, 0)V^{T} E^=Udiag(δ1,δ2,0)VT
- 规范化结果,本质矩阵重构, E = U d i a g ( δ 1 + δ 2 2 , δ 1 + δ 2 2 , 0 ) V T E=Udiag(\frac{\delta_{1}+\delta_{2}}{2},\frac{\delta_{1}+\delta_{2}}{2},0)V^{T} E=Udiag(2δ1+δ2,2δ1+δ2,0)VT
4.4 相机姿态的恢复
根据重构的本质矩阵和奇异值约束,可以求出四组R与t的结果。
筛选正确的相机姿态
相机正确的姿态需要满足以下条件:
首先利用相机姿态R,t和一系列同名点(correspondence):
p
1
p_{1}
p1、
p
2
p_{2}
p2进行三角测量得到三维坐标点P,P需要满足同时在两个相机的前方。
通过两种方法来确认相机的正确姿态:
方法1. 相机主方向与
P
O
PO
PO的夹角小于90°
(
P
−
O
1
)
T
d
1
>
0
(
P
−
O
2
)
T
d
1
>
0
(4.12)
(P-O_{1})^{T}d_{1}>0\\(P-O_{2})^{T}d_{1}>0\tag{4.12}
(P−O1)Td1>0(P−O2)Td1>0(4.12)
方法2.物体的坐标深度为正
{
x
c
y
c
z
c
}
=
R
P
+
t
,
z
c
>
0
(4.13)
\left\{\begin{matrix}\ x_{c}\\y_{c}\\z_{c} \end{matrix}\right\}= RP+t , z_{c}>0\tag{4.13}
⎩
⎨
⎧ xcyczc⎭
⎬
⎫=RP+t,zc>0(4.13)
4.5 单应矩阵
若三维中的点都落在同一个平面上,则可以通过单应矩阵来描述两个同名点之间的信息。
假设这个平面满足平面方程:
n
T
X
+
d
=
0
(4.14)
n^{T}X+d=0\tag{4.14}
nTX+d=0(4.14)
即:
−
n
T
X
d
=
1
(4.15)
-\frac{n^{T}X}{d}=1\tag{4.15}
−dnTX=1(4.15)
将(4.15)代入(4.1)中得:
x
2
=
K
2
(
R
X
+
t
)
=
K
2
(
R
X
+
t
(
−
n
T
X
d
)
)
=
K
2
(
R
−
t
n
T
d
)
X
=
K
2
(
R
−
t
n
T
d
)
K
1
−
1
x
1
(4.16)
x_{2}=K_{2}(RX+t)\\\;\\=K_{2}(RX+t(-\frac{n^{T}X}{d}))\\\;\\=K_{2}(R-\frac{tn^{T}}{d})X\\\;\\=K_{2}(R-\frac{tn^{T}}{d})K_{1}^{-1}x_{1}\tag{4.16}
x2=K2(RX+t)=K2(RX+t(−dnTX))=K2(R−dtnT)X=K2(R−dtnT)K1−1x1(4.16)
其中,记
x
2
=
H
x
1
H
=
K
2
(
R
−
t
n
T
d
)
K
1
−
1
(4.17)
x_{2}=Hx_{1}\\H=K_{2}(R-\frac{tn^{T}}{d})K_{1}^{-1}\tag{4.17}
x2=Hx1H=K2(R−dtnT)K1−1(4.17)
单应矩阵是满秩矩阵,且当t=0时候,对应的情况为相机纯旋转
4.6 单应矩阵的求解
直接线性变换法:
由
(
u
2
v
2
1
)
=
[
H
11
H
12
H
13
H
21
H
22
H
23
H
31
H
32
H
33
]
(
u
1
v
1
1
)
\left(\begin{array}{c} u_{2} \\ v_{2} \\ 1 \end{array}\right)=\left[\begin{array}{lll} H_{11} & H_{12} & H_{13} \\ H_{21} & H_{22} & H_{23} \\ H_{31} & H_{32} & H_{33} \end{array}\right]\left(\begin{array}{c} u_{1} \\ v_{1} \\ 1 \end{array}\right)
u2v21
=
H11H21H31H12H22H32H13H23H33
u1v11
得到:
u
2
=
H
11
u
1
+
H
12
v
1
+
H
13
H
31
u
1
+
H
32
v
1
+
H
33
v
2
=
H
21
u
1
+
H
22
v
1
+
H
23
H
31
u
1
+
H
32
v
1
+
H
33
\begin{array}{c} u_{2}=\frac{H_{11} u_{1}+H_{12} v_{1}+H_{13}}{H_{31} u_{1}+H_{32} v_{1}+H_{33}} \\\;\\ v_{2}=\frac{H_{21} u_{1}+H_{22} v_{1}+H_{23}}{H_{31} u_{1}+H_{32} v_{1}+H_{33}} \end{array}
u2=H31u1+H32v1+H33H11u1+H12v1+H13v2=H31u1+H32v1+H33H21u1+H22v1+H23
即:
H
11
u
1
+
H
12
v
1
+
H
13
−
H
31
u
1
u
2
−
H
32
u
2
v
1
−
H
33
u
2
=
0
H
21
u
1
+
H
22
v
1
+
H
23
−
H
31
u
1
v
2
−
H
32
v
1
v
2
−
H
33
v
2
=
0
\begin{array}{l} H_{11} u_{1}+H_{12} v_{1}+H_{13}-H_{31} u_{1} u_{2}-H_{32} u_{2} v_{1}-H_{33} u_{2}=0 \\ H_{21} u_{1}+H_{22} v_{1}+H_{23}-H_{31} u_{1} v_{2}-H_{32} v_{1} v_{2}-H_{33} v_{2}=0 \end{array}
H11u1+H12v1+H13−H31u1u2−H32u2v1−H33u2=0H21u1+H22v1+H23−H31u1v2−H32v1v2−H33v2=0
令
H
33
=
1
H_{33}=1
H33=1,即:
A
=
(
u
1
(
1
)
,
v
1
(
1
)
,
1
,
0
,
0
,
0
,
−
u
1
(
1
)
u
2
(
1
)
,
−
u
2
(
1
)
v
1
(
1
)
0
,
0
,
0
,
u
1
(
1
)
,
v
1
(
1
)
,
1
,
−
u
1
(
1
)
v
2
(
1
)
,
−
v
1
(
1
)
v
2
(
1
)
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
u
1
(
4
)
v
1
(
4
)
1
,
0
,
0
,
0
,
−
u
1
(
4
)
u
2
(
4
)
,
−
u
2
(
4
)
v
1
(
4
)
0
,
0
,
0
,
u
1
(
1
)
v
1
(
1
)
,
1
,
−
u
1
(
4
)
v
2
(
4
)
,
−
v
1
(
4
)
v
2
(
4
)
)
(
F
11
F
12
F
13
F
22
F
22
F
23
F
31
F
32
)
=
(
u
2
(
0
)
v
2
(
1
)
u
2
(
2
)
v
2
(
2
)
u
2
(
3
)
v
2
(
3
)
u
2
(
4
)
v
2
(
4
)
)
A=\left(\begin{array}{ccccccccc} u_{1}^{(1)}, & v_{1}^{(1)}, & 1, & 0, & 0, & 0, & -u_{1}^{(1)} u_{2}^{(1)}, & -u_{2}^{(1)} v_{1}^{(1)} \\ 0, & 0, & 0, & u_{1}^{(1)}, & v_{1}^{(1)}, & 1, & -u_{1}^{(1)} v_{2}^{(1)}, & -v_{1}^{(1)} v_{2}^{(1)} \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ u_{1}^{(4)} & v_{1}^{(4)} & 1, & 0, & 0, & 0, & -u_{1}^{(4)} u_{2}^{(4)}, & -u_{2}^{(4)} v_{1}^{(4)} \\ 0, & 0, & 0, & u_{1}^{(1)} & v_{1}^{(1)}, & 1, & -u_{1}^{(4)} v_{2}^{(4)}, & -v_{1}^{(4)} v_{2}^{(4)} \end{array}\right)\left(\begin{array}{l} F_{11} \\ F_{12} \\ F_{13} \\ F_{22} \\ F_{22} \\ F_{23} \\ F_{31} \\ F_{32} \end{array}\right)=\left(\begin{array}{l} u_{2}^{(0)} \\ v_{2}^{(1)} \\ u_{2}^{(2)} \\ v_{2}^{(2)} \\ u_{2}^{(3)} \\ v_{2}^{(3)} \\ u_{2}^{(4)} \\ v_{2}^{(4)} \end{array}\right)
A=
u1(1),0,⋮u1(4)0,v1(1),0,⋮v1(4)0,1,0,⋮1,0,0,u1(1),⋮0,u1(1)0,v1(1),⋮0,v1(1),0,1,⋮0,1,−u1(1)u2(1),−u1(1)v2(1),⋮−u1(4)u2(4),−u1(4)v2(4),−u2(1)v1(1)−v1(1)v2(1)⋮−u2(4)v1(4)−v1(4)v2(4)
F11F12F13F22F22F23F31F32
=
u2(0)v2(1)u2(2)v2(2)u2(3)v2(3)u2(4)v2(4)
所以至少需要4对匹配点,才能求H矩阵。
Ransac计算单应矩阵过程:
- 随机采样4对匹配点 ( x 1 ( n ) , x 2 ( n ) ) \left(x_{1}^{(n)}, x_{2}^{(n)}\right) (x1(n),x2(n))
- 使用8点法求解基础矩阵 H \boldsymbol{H} H
- 使用 E ( x 1 , x 2 , H ) = d ( x 1 , H − 1 x 2 ) 2 + d ( x 2 , H x 1 ) 2 E\left(x_{1}, x_{2}, H\right)=d\left(x_{1}, H^{-1} x_{2}\right)^{2}+d\left(x_{2}, H x_{1}\right)^{2} E(x1,x2,H)=d(x1,H−1x2)2+d(x2,Hx1)2计算误差,并通过 E ( x 1 , x 2 , H ) < τ E\left(x_{1}, x_{2}, H\right)<\tau E(x1,x2,H)<τ统计内点个数
- 重复以上过程,选择内点最多的结果
- 对所有的内点重复3、4步骤,重新计算 H H H
五、极线校正
极线校正:将相机在数学上对准到同一观察平面上,使得相机像素行是严格对齐,校正的目的是使得二维匹配搜索变成一维,节省计算量,排除虚假匹配点.
极线校正的最终效果是要将极点坐标变换到无穷远处(像平面与基线平行,两者没有交点),求取旋转矩阵,达到从相机坐标系变换到极线校正后坐标系.
- 对于X轴,将X轴旋转到与基线相同的方向,即 O l − O r O_{l}-O_{r} Ol−Or: e 1 = T ∣ T ∣ (5.1) e_{1} = \frac{T}{|T|}\tag{5.1} e1=∣T∣T(5.1)
- 对于y轴,选取为和主光轴与 e 1 e_{1} e1都垂直的方向: e 2 = e 1 × ( R l [ 0 , 0 , 1 ] T + t l ) (5.2) e_{2}=e_{1} \times\left(R_{l}[0,0,1]^{T}+t_{l}\right)\tag{5.2} e2=e1×(Rl[0,0,1]T+tl)(5.2)
- 对于z轴,选取和 e 1 和 e 2 e_{1}和e_{2} e1和e2都垂直的方向: e 3 = e 1 × e 2 (5.3) e_{3}=e_{1}\times e_{2}\tag{5.3} e3=e1×e2(5.3)
- 最终的旋转矩阵为: R n e w = [ e 1 , e 2 , e 3 ] T (5.4) R_{new}=[e_{1},e_{2},e_{3}]^{T}\tag{5.4} Rnew=[e1,e2,e3]T(5.4)
- 从世界坐标系到极线校正后坐标系的投影矩阵为: R l n e w = R n e w R l t R r n e w = R n e w R r t (5.5) R_{l}^{new} = R_{new}R_{l}^{t}\\\;\\R_{r}^{new} = R_{new}R_{r}^{t}\tag{5.5} Rlnew=RnewRltRrnew=RnewRrt(5.5)
注: R r t 和 R l t R_{r}^{t}和R_{l}^{t} Rrt和Rlt是表示齐次坐标系表示形式
校正后坐标系下基线距离:
由式(1.0)得:
[
x
w
y
w
z
w
]
=
R
−
1
[
x
c
y
c
z
c
]
+
R
−
1
t
(5.6)
\begin{bmatrix} x_{w}\\y_{w}\\z_{w}\end{bmatrix} = R^{-1} \begin{bmatrix} x_{c}\\y_{c}\\z_{c} \end{bmatrix}+ R^{-1} t\tag{5.6}
xwywzw
=R−1
xcyczc
+R−1t(5.6)
记世界坐标系到左相机坐标系和右相机坐标系的旋转矩阵的逆矩阵为
R
l
和
R
r
R_{l}和R_{r}
Rl和Rr ,记
C
=
R
−
1
t
C = R^{-1}t
C=R−1t 表示相机中心在世界坐标系的坐标.
则上式可以表示为:
[
x
w
y
w
z
w
]
=
R
l
[
x
c
l
y
c
l
z
c
l
]
+
C
l
[
x
w
y
w
z
w
]
=
R
r
[
x
c
r
y
c
r
z
c
r
]
+
C
r
[
x
c
l
y
c
l
z
c
l
]
=
R
l
r
[
x
c
r
y
c
r
z
c
r
]
+
C
l
r
\begin{bmatrix} x_{w}\\y_{w}\\z_{w}\end{bmatrix} = R_{l} \begin{bmatrix} x_{cl}\\y_{cl}\\z_{cl}\end{bmatrix}+ C_{l}\\\;\\\begin{bmatrix} x_{w}\\y_{w}\\z_{w} \end{bmatrix} = R_{r} \begin{bmatrix} x_{cr}\\y_{cr}\\z_{cr}\end{bmatrix}+ C_{r}\\\;\\\begin{bmatrix} x_{cl}\\y_{cl}\\z_{cl} \end{bmatrix} = R_{lr} \begin{bmatrix} x_{cr}\\y_{cr}\\z_{cr}\end{bmatrix}+ C_{lr}
xwywzw
=Rl
xclyclzcl
+Cl
xwywzw
=Rr
xcrycrzcr
+Cr
xclyclzcl
=Rlr
xcrycrzcr
+Clr
上图用
R
l
,
R
r
,
C
r
,
C
l
R_{l},R_{r},C_{r},C_{l}
Rl,Rr,Cr,Cl来代表
R
,
t
R, t
R,t来表示两个坐标系之间的变换关系:由上式得:
左相机原点在右相机坐标系下的坐标:
[
0
0
0
]
=
R
l
r
[
x
c
r
y
c
r
z
c
r
]
+
C
l
r
\begin{bmatrix} 0\\0\\0 \end{bmatrix} = R_{lr} \begin{bmatrix} x_{cr}\\y_{cr}\\z_{cr}\end{bmatrix}+ C_{lr}
000
=Rlr
xcrycrzcr
+Clr
即左相机原点在右相机坐标系下的坐标
O
l
e
f
t
−
i
n
−
r
i
g
h
t
O_{left-in-right}
Oleft−in−right:
O
l
e
f
t
−
i
n
−
r
i
g
h
t
=
−
R
l
r
C
l
r
(5.7)
O_{left-in-right} = -R_{lr}C_{lr}\tag{5.7}
Oleft−in−right=−RlrClr(5.7)
则左相机原点坐标在右相机极线校正坐标系下坐标为:
O
l
e
f
t
−
i
n
−
r
i
g
h
t
=
−
R
r
n
e
w
R
l
r
C
l
r
(5.8)
O_{left-in-right} = -R_{r}^{new}R_{lr}C_{lr}\tag{5.8}
Oleft−in−right=−RrnewRlrClr(5.8)
故baseline就是x方向的距离:
T
x
=
O
l
e
f
t
−
i
n
−
r
i
g
h
t
.
x
⃗
(5.9)
T_{x}=O_{left-in-right} . \vec{x}\tag{5.9}
Tx=Oleft−in−right.x(5.9)
六、视差图与深度图的关系
假设左右图像已经完成极线校正(两相机姿态平行,同名点位于同一水平线)
图中,
x
o
l
x_{ol}
xol是左像平面光轴与像平面交点的x坐标,
x
l
x_{l}
xl是P点在左像平面上的x坐标,
x
o
r
x_{or}
xor是右像平面光轴与像平面交点的x坐标,
x
r
x_{r}
xr是P点在右像平面上的x坐标,由三角形的相似性:
x
l
−
x
o
l
B
1
=
x
o
r
−
x
r
B
2
=
f
z
(6.1)
\frac{x_{l}-x_{ol}}{B_{1}} =\frac{x_{or}-x_{r}}{B_{2}} = \frac{f}{z}\tag{6.1}
B1xl−xol=B2xor−xr=zf(6.1)
整理得:
z
=
B
f
d
+
(
x
o
r
−
x
o
l
)
d
=
x
l
−
x
r
(6.2)
z=\frac{Bf}{d+(x_{or}-x_{ol})}\\d=x_{l}-x_{r}\tag{6.2}
z=d+(xor−xol)Bfd=xl−xr(6.2)
其中,
d
d
d为左右图的横坐标之差,称为视差(Disparity)
若
x
o
r
=
x
o
l
x_{or}=x_{ol}
xor=xol,即左相机光轴中心在左像素平面的坐标与右相机光轴中心在右视图中的像素平面x轴坐标一致,则
z
=
B
f
d
d
=
x
l
−
x
r
(6.3)
z=\frac{Bf}{d}\\d=x_{l}-x_{r}\tag{6.3}
z=dBfd=xl−xr(6.3)
由式(6.3)得:双目相机成像模型,焦距f与基线长度b为常数,视差d与深度z成反比,即深度越小视差越大,距离相机近的点视差较大(视差图中,近的物体颜色较深)。视差与深度的关系如下:
- 视差与深度成反比,视差接近0时,微小的视差变化会引起较大的深度变化.
- 当视差较大时,微小的视差变化不会引起深度产生很大的变化.
- 当相机距离被测物体较近时具有较高的视差精度.
七、视差图转深度图
由第五节知,立体匹配是在极线校正坐标系下完成的操作,故得到的视差图也是极线校正坐标系下的视差图,先将其在极线校正坐标系下转换成深度图:
由式(1.5)得:
depth
[
u
v
1
]
=
[
f
0
u
0
0
f
v
0
0
0
1
]
[
x
y
z
]
(7.1)
\operatorname{depth}\left[\begin{array}{l} u \\ v \\ 1 \end{array}\right]=\left[\begin{array}{lll} f & 0 & u_{0} \\ 0 & f & v_{0} \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{l} x \\ y \\ z \end{array}\right]\tag{7.1}
depth
uv1
=
f000f0u0v01
xyz
(7.1)
由(6.2)得
d
e
p
t
h
=
B
f
d
i
s
p
+
c
r
−
c
l
(7.2)
depth = \frac{Bf}{disp + c_{r}-c_{l}}\tag{7.2}
depth=disp+cr−clBf(7.2)
由(5.9)得:
B
=
−
T
x
(7.3)
B = -T_{x}\tag{7.3}
B=−Tx(7.3)
则有:
x
=
(
u
−
u
0
)
(
B
d
i
s
p
+
c
r
−
c
l
)
=
(
u
−
u
0
)
(
−
T
x
d
i
s
p
+
c
r
−
c
l
)
y
=
(
v
−
v
0
)
(
B
d
i
s
p
+
c
r
−
c
l
)
=
(
v
−
v
0
)
(
−
T
x
d
i
s
p
+
c
r
−
c
l
)
z
=
B
f
d
i
s
p
+
c
r
−
c
l
=
−
T
x
f
d
i
s
p
+
c
r
−
c
l
\begin{aligned} x=\left(u-u_{0}\right)\left(\frac{B}{d i s p+c_{r}-c_{l}}\right) &=\left(u-u_{0}\right)\left(\frac{-T x}{d i s p+c_{r}-c_{l}}\right) \\ y=\left(v-v_{0}\right)\left(\frac{B}{d i s p+c_{r}-c_{l}}\right) &=\left(v-v_{0}\right)\left(\frac{-T x}{d i s p+c_{r}-c_{l}}\right) \\ z=\frac{B f}{d i s p+c_{r}-c_{l}} &=\frac{-T_{x} f}{d i s p+c_{r}-c_{l}} \end{aligned}
x=(u−u0)(disp+cr−clB)y=(v−v0)(disp+cr−clB)z=disp+cr−clBf=(u−u0)(disp+cr−cl−Tx)=(v−v0)(disp+cr−cl−Tx)=disp+cr−cl−Txf
将其写为矩阵形式后, 极线校正坐标系下视差图和深度图之间的关系为:
Q
′
[
u
v
d
i
s
p
1
]
=
w
[
x
y
z
1
]
(7.4)
Q^{\prime}\left[\begin{array}{l} u \\ v \\ d i s p \\ 1 \end{array}\right]=w\left[\begin{array}{l} x \\ y \\ z \\ 1 \end{array}\right]\tag{7.4}
Q′
uvdisp1
=w
xyz1
(7.4)
其中:
w
=
disp
+
c
r
−
c
l
−
T
x
Q
′
=
[
1
0
0
−
u
0
0
1
0
−
v
0
0
0
0
f
0
0
−
1
/
T
x
(
c
l
−
c
r
)
/
T
x
]
w=\frac{\text { disp }+c_{r}-c_{l}}{-T_{x}}\\ \begin{array}{c} \\ Q^{\prime}=\left[\begin{array}{cccc} 1 & 0 & 0 &-u_{0} \\ 0 & 1 & 0 & -v_{0}\\ 0 & 0 & 0 & f\\ 0 & 0 & -1 / T_{x} & \left(c_{l}-c_{r}\right) / T_{x} \end{array}\right] \end{array}
w=−Tx disp +cr−clQ′=
10000100000−1/Tx−u0−v0f(cl−cr)/Tx
其中,基于哪个视差图来计算,则f就是对应相机的焦距f,由7.4可以得到从极线校正后坐标系下视差图转到极线校正后深度图的变换.
将校正后坐标系下的视差图转换为校正前坐标系下的深度图如下:
由(5.4)知:校正前坐标系下点
(
x
′
,
y
′
,
z
′
)
(x^{\prime}, y^{\prime},z^{\prime})
(x′,y′,z′)和校正后坐标系下点
(
x
,
y
,
z
)
(x,y,z)
(x,y,z)转换关系如下:
R
l
new
[
x
′
y
′
z
′
]
=
[
x
y
z
]
(7.5)
R_{l}^{\text {new }}\left[\begin{array}{l} x^{\prime} \\ y^{\prime} \\ z^{\prime} \end{array}\right]=\left[\begin{array}{l} x \\ y \\ z \end{array}\right]\tag{7.5}
Rlnew
x′y′z′
=
xyz
(7.5)
两边同乘
(
R
l
new
)
−
1
(R_{l}^{\text {new }})^{-1}
(Rlnew )−1则:
[
x
′
y
′
z
′
]
=
(
R
l
new
)
−
1
[
x
y
z
]
(7.6)
\left[\begin{array}{l} x^{\prime} \\ y^{\prime} \\ z^{\prime} \end{array}\right] =(R_{l}^{\text {new }})^{-1}\left[\begin{array}{l} x \\ y \\ z \end{array}\right]\tag{7.6}
x′y′z′
=(Rlnew )−1
xyz
(7.6)
对上式同时左乘内参矩阵
K
l
K_{l}
Kl 则将极线校正前坐标系下坐标
[
x
,
y
,
z
]
T
[x, y, z]^{T}
[x,y,z]T投影到极线校正前图像坐标系下坐标
[
u
′
,
v
′
,
1
]
T
[u^{\prime}, v^{\prime}, 1]^{T}
[u′,v′,1]T:
depth
[
u
′
v
′
1
]
=
[
X
Y
Z
]
=
K
l
(
R
l
n
e
w
)
−
1
[
x
y
z
]
(7.7)
\operatorname{depth}\left[\begin{array}{l} u^{\prime} \\ v ^{\prime}\\ 1 \end{array}\right]=\left[\begin{array}{l} X \\ Y \\ Z \end{array}\right]=K_{l}\left(R_{l}^{n e w}\right)^{-1}\left[\begin{array}{l} x \\ y \\ z \end{array}\right]\tag{7.7}
depth
u′v′1
=
XYZ
=Kl(Rlnew)−1
xyz
(7.7)
其中
Z
=
d
e
p
t
h
,
X
=
u
′
∗
d
e
p
t
h
,
Y
=
v
′
∗
d
e
p
t
h
Z = depth, X=u^{\prime}*depth, Y=v^{\prime}*depth
Z=depth,X=u′∗depth,Y=v′∗depth
(7.7)左右两边同乘w得:
w
[
X
Y
Z
]
=
K
l
(
R
l
n
e
w
)
−
1
w
[
x
y
z
]
(7.8)
w\left[\begin{array}{l} X \\ Y \\ Z \end{array}\right]=K_{l}\left(R_{l}^{n e w}\right)^{-1}w\left[\begin{array}{l} x \\ y \\ z \end{array}\right]\tag{7.8}
w
XYZ
=Kl(Rlnew)−1w
xyz
(7.8)
将(7.8)写成其次坐标形式得到:
w
[
X
Y
Z
1
]
=
[
K
l
(
R
l
n
e
w
)
−
1
0
⊤
0
⊤
1
]
w
[
x
y
z
1
]
(7.9)
w\left[\begin{array}{l} X \\ Y \\ Z \\ 1 \end{array}\right]=\left[\begin{array}{cc} \boldsymbol{K}_{l}\left(\boldsymbol{R}_{l}^{n e w}\right)^{-1} & \mathbf{0}^{\top} \\ \mathbf{0}^{\top} & 1 \end{array}\right] w\left[\begin{array}{l} x \\ y \\ z \\ 1 \end{array}\right]\tag{7.9}
w
XYZ1
=[Kl(Rlnew)−10⊤0⊤1]w
xyz1
(7.9)
将(7.4)中的
w
[
x
y
z
1
]
=
Q
′
[
u
v
d
i
s
p
1
]
w\left[\begin{array}{l} x \\ y \\ z \\ 1 \end{array}\right]=Q^{\prime}\left[\begin{array}{l} u \\ v \\ d i s p \\ 1 \end{array}\right]
w
xyz1
=Q′
uvdisp1
代入(7.9)中得:
w
[
X
Y
Z
1
]
=
[
K
l
(
R
l
n
e
w
)
−
1
0
⊤
0
⊤
1
]
Q
′
[
u
v
d
i
s
p
1
]
(7.10)
w\left[\begin{array}{l} X \\ Y \\ Z \\ 1 \end{array}\right]=\left[\begin{array}{cc} \boldsymbol{K}_{l}\left(\boldsymbol{R}_{l}^{n e w}\right)^{-1} & \mathbf{0}^{\top} \\ \mathbf{0}^{\top} & 1 \end{array}\right] Q^{\prime}\left[\begin{array}{l} u \\ v \\ d i s p \\ 1 \end{array}\right]\tag{7.10}
w
XYZ1
=[Kl(Rlnew)−10⊤0⊤1]Q′
uvdisp1
(7.10)
由此得到极线校正后坐标系下视差图
[
u
v
d
i
s
p
1
]
\left[\begin{array}{l} u \\ v \\ d i s p \\ 1 \end{array}\right]
uvdisp1
到极线校正前深度图
[
X
Y
Z
1
]
\left[\begin{array}{l} X \\ Y \\ Z \\ 1 \end{array}\right]
XYZ1
的变换,其中
Z
=
d
e
p
t
h
,
X
=
u
′
∗
d
e
p
t
h
,
Y
=
v
′
∗
d
e
p
t
h
Z = depth, X=u^{\prime}*depth, Y=v^{\prime}*depth
Z=depth,X=u′∗depth,Y=v′∗depth