首先,构建一个相机移动的模型。假设相机通过旋转矩阵 R R R和位移向量 T T T从 p r p_{r} pr的位置移动到 p l p_{l} pl的位置。世界坐标系中的物体 P P P在不同位置的相机坐标系中的坐标是 P r P_{r} Pr和 P l P_{l} Pl。 P P P和 P l P_{l} Pl、 P P P和 P r P_{r} Pr之间的替换公式为:
[ X l Y l Z l ] = P l = R l P + T l = R l [ X Y Z ] + T l [ X r Y r Z r ] = P r = R r P + T r = R r [ X Y Z ] + T r \begin{bmatrix} X_{l} \\ Y_{l} \\ Z_{l} \end{bmatrix} = P_{l} = R_{l}P + T_{l} = R_{l} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} + T_{l} \\ \begin{bmatrix} X_{r} \\ Y_{r} \\ Z_{r} \end{bmatrix} = P_{r} = R_{r}P + T_{r} = R_{r} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} + T_{r} ⎣⎡XlYlZl⎦⎤=Pl=RlP+Tl=Rl⎣⎡XYZ⎦⎤+Tl⎣⎡XrYrZr⎦⎤=Pr=RrP+Tr=Rr⎣⎡XYZ⎦⎤+Tr
P l P_{l} Pl和 p l p_{l} pl、 P r P_{r} Pr和 p r p_{r} pr之间的替换公式为:
[ x l y l 1 ] = p l = 1 Z l K 3 × 3 P l = 1 Z l K 3 × 3 [ X l Y l Z l ] [ x r y r 1 ] = p r = 1 Z r K 3 × 3 P r = 1 Z r K 3 × 3 [ X r Y r Z r ] \begin{bmatrix} x_{l} \\ y_{l} \\ 1 \end{bmatrix} = p_{l} = \frac{1}{Z_{l}} K_{3 \times 3}P_{l} = \frac{1}{Z_{l}} K_{3 \times 3} \begin{bmatrix} X_{l} \\ Y_{l} \\ Z_{l} \end{bmatrix} \\ \begin{bmatrix} x_{r} \\ y_{r} \\ 1 \end{bmatrix} = p_{r} = \frac{1}{Z_{r}} K_{3 \times 3}P_{r} = \frac{1}{Z_{r}} K_{3 \times 3} \begin{bmatrix} X_{r} \\ Y_{r} \\ Z_{r} \end{bmatrix} ⎣⎡xlyl1⎦⎤=pl=Zl1K3×3Pl=Zl1K3×3⎣⎡XlYlZl⎦⎤⎣⎡xryr1⎦⎤=pr=Zr1K3×3Pr=Zr1K3×3⎣⎡XrYrZr⎦⎤
由于structure from motion只能得到相机位移向量 T T T同方向的单位向量 u u u,所以真实的 T = k u T=ku T=ku,其中 k k k是一个未知正常数。 P l P_{l} Pl和 P r P_{r} Pr之间的替换公式为:
[ X r Y r Z r ] = P r = R P l + T = R [ X l Y l Z l ] + k u \begin{bmatrix} X_{r} \\ Y_{r} \\ Z_{r} \end{bmatrix} = P_{r} = RP_{l} + T = R \begin{bmatrix} X_{l} \\ Y_{l} \\ Z_{l} \end{bmatrix} + ku ⎣⎡XrYrZr⎦⎤=Pr=RPl+T=R⎣⎡XlYlZl⎦⎤+ku
通过特征点匹配、光流等得到
p
l
p_{l}
pl和
p
r
p_{r}
pr之间的像素位移,可构建等式:
[
△
x
△
y
0
]
=
p
l
−
p
r
=
K
3
x
3
(
1
Z
l
[
X
l
Y
l
Z
l
]
−
1
Z
r
[
X
r
Y
r
Z
r
]
)
=
K
3
x
3
(
1
Z
l
[
X
l
Y
l
Z
l
]
−
1
f
(
Z
l
)
(
R
[
X
r
Y
r
Z
r
]
+
k
u
)
)
=
K
3
x
3
(
(
1
Z
l
−
R
f
(
Z
l
)
)
[
X
l
Y
l
Z
l
]
−
k
u
f
(
Z
l
)
)
\begin{bmatrix} \triangle{x} \\ \triangle{y} \\ 0 \end{bmatrix} = p_{l} - p_{r} = K_{3x3}(\frac{1}{Z_{l}}\begin{bmatrix} X_{l} \\ Y_{l} \\ Z_{l} \end{bmatrix} - \frac{1}{Z_{r}}\begin{bmatrix} X_{r} \\ Y_{r} \\ Z_{r} \end{bmatrix}) \\ = K_{3x3}(\frac{1}{Z_{l}}\begin{bmatrix} X_{l} \\ Y_{l} \\ Z_{l} \end{bmatrix} - \frac{1}{f(Z_{l})}(R \begin{bmatrix} X_{r} \\ Y_{r} \\ Z_{r} \end{bmatrix} + ku)) \\ = K_{3x3}((\frac{1}{Z_{l}} - \frac{R}{f(Z_{l})})\begin{bmatrix} X_{l} \\ Y_{l} \\ Z_{l} \end{bmatrix} - \frac{ku}{f(Z_{l})})
⎣⎡△x△y0⎦⎤=pl−pr=K3x3(Zl1⎣⎡XlYlZl⎦⎤−Zr1⎣⎡XrYrZr⎦⎤)=K3x3(Zl1⎣⎡XlYlZl⎦⎤−f(Zl)1(R⎣⎡XrYrZr⎦⎤+ku))=K3x3((Zl1−f(Zl)R)⎣⎡XlYlZl⎦⎤−f(Zl)ku)
上式为带有4个自变量 X l X_{l} Xl、 Y l Y_{l} Yl、 Z l Z_{l} Zl和 k k k的三个等式。由于剩下一个自由度,structure from motion只能提取出景物的比例信息。(stereo vision可以确定 k k k值,上式便仅有三个自变量 X l X_{l} Xl、 Y l Y_{l} Yl和 Z l Z_{l} Zl,因此可以确定景物的尺寸信息。)