已知第1张图像的像素点在归一化平面的坐标
f
r
e
f
f_{ref}
fref(三维向量,分别为
X
/
Z
,
Y
/
Z
,
1
X/Z,Y/Z,1
X/Z,Y/Z,1,注意这里我们不知道
X
,
Y
,
Z
X,Y,Z
X,Y,Z分别是多少,只知道它们的比值关系),当前图像的相应像素点在归一化平面的坐标
f
c
u
r
f_{cur}
fcur(三维向量,形式为
X
/
Z
,
Y
/
Z
,
1
X/Z,Y/Z,1
X/Z,Y/Z,1),并且还知道从当前图像到第1张图像的变换矩阵
T
R
C
T_{RC}
TRC(
R
R
C
R_{RC}
RRC表示旋转部分,
t
R
C
t_{RC}
tRC表示平移部分),求第1张图像的像素点的深度
d
r
e
f
d_{ref}
dref(标量值,就是
Z
Z
Z)和当前图像的相应像素点的深度
d
c
u
r
d_{cur}
dcur(标量值,就是
Z
Z
Z)。
由题意,可列,
d
r
e
f
⋅
f
r
e
f
=
d
c
u
r
⋅
(
R
R
C
f
c
u
r
)
+
t
R
C
(1)
d_{ref} \sdot f_{ref}=d_{cur} \sdot (R_{RC}f_{cur})+t_{RC} \tag{1}
dref⋅fref=dcur⋅(RRCfcur)+tRC(1)
记
R
R
C
f
c
u
r
R_{RC}f_{cur}
RRCfcur为
f
2
f_2
f2,则上式可写成,
d
r
e
f
⋅
f
r
e
f
=
d
c
u
r
⋅
f
2
+
t
R
C
(2)
d_{ref} \sdot f_{ref}=d_{cur} \sdot f_2+t_{RC} \tag{2}
dref⋅fref=dcur⋅f2+tRC(2)
在上式中三维向量
f
r
e
f
f_{ref}
fref,
f
c
u
r
f_{cur}
fcur和
t
R
C
t_{RC}
tRC均已知,求标量
d
r
e
f
d_{ref}
dref和标量
d
c
u
r
d_{cur}
dcur。整理上式,可得,
f
r
e
f
⋅
d
r
e
f
−
f
2
⋅
d
c
u
r
=
t
R
C
(3)
f_{ref} \sdot d_{ref}-f_2 \sdot d_{cur}=t_{RC}\tag{3}
fref⋅dref−f2⋅dcur=tRC(3)
公式(3)左右两边点乘三维向量
f
r
e
f
f_{ref}
fref和
f
2
f_2
f2,得到公式(4)和公式(5),
f
r
e
f
T
f
r
e
f
⋅
d
r
e
f
−
f
r
e
f
T
f
2
⋅
d
c
u
r
=
f
r
e
f
T
t
R
C
(4)
f_{ref}^Tf_{ref}\sdot d_{ref}-f_{ref}^Tf_2\sdot d_{cur}=f_{ref}^Tt_{RC} \tag{4}
frefTfref⋅dref−frefTf2⋅dcur=frefTtRC(4)
f
2
T
f
r
e
f
⋅
d
r
e
f
−
f
2
T
f
2
⋅
d
c
u
r
=
f
2
T
t
R
C
(5)
f_2^Tf_{ref}\sdot d_{ref}-f_2^Tf_2\sdot d_{cur}=f_2^Tt_{RC} \tag{5}
f2Tfref⋅dref−f2Tf2⋅dcur=f2TtRC(5)
将公式(4)和公式(5)写成矩阵形式为,
[
f
r
e
f
T
f
r
e
f
−
f
r
e
f
T
f
2
f
2
T
f
r
e
f
−
f
2
T
f
2
]
[
d
r
e
f
d
c
u
r
]
=
[
f
r
e
f
T
t
R
C
f
2
T
t
R
C
]
(6)
\left [ \begin{matrix} f_{ref}^Tf_{ref} & -f_{ref}^Tf_2\\ f_2^Tf_{ref} & -f_2^Tf_2 \end{matrix} \right] \left[ \begin{matrix} d_{ref} \\ d_{cur} \end{matrix} \right] = \left[\begin{matrix} f_{ref}^Tt_{RC} \\ f_2^Tt_{RC} \end{matrix} \right] \tag{6}
[frefTfreff2Tfref−frefTf2−f2Tf2][drefdcur]=[frefTtRCf2TtRC](6)
即得代码中所示公式。
updateDepthFilter()中求解d_ref和d_cur公式推导
最新推荐文章于 2023-06-08 17:47:04 发布