示意图
其中l1、l2为极线(极平面与像素平面I1,I2的相交线)如图,PO1O2为极平面 e1,e2为极点,p1、p2分别为P在照片I1和I2上的投影
设P的世界坐标为
(
x
y
z
)
\Bigg (\begin{matrix} x\\y\\z \end{matrix}\Bigg)
(xyz)
由针孔相机模型
s
1
(
x
′
y
′
1
)
=
(
f
x
0
C
x
0
f
y
C
y
0
0
1
)
(
x
y
z
)
=
K
P
s_1\Bigg (\begin{matrix} x'\\y'\\1 \end{matrix}\Bigg)= \Bigg (\begin{matrix} f_x&0&C_x\\0&f_y&C_y\\0&0&1 \end{matrix}\Bigg) \Bigg (\begin{matrix} x\\y\\z \end{matrix}\Bigg)=KP
s1(x′y′1)=(fx000fy0CxCy1)(xyz)=KP
即
s
1
P
1
=
K
P
s_1P_1=KP
s1P1=KP
s
1
s_1
s1为P到p1的距离,K为内参矩阵,
P
1
、
P
2
P_1、P_2
P1、P2为
p
1
p_1
p1、
p
2
p_2
p2的其次坐标形式
同理在第二个时刻的照片中
s
2
P
2
=
K
(
R
P
+
t
)
s_2P_2=K(RP+t)
s2P2=K(RP+t)
R,t为旋转平移参数
此时我们定义尺度意义下相等(就是进行归一化平面投影,将深度信息设为1)
P
1
≃
K
P
P_1\simeq KP
P1≃KP 和
P
2
≃
(
R
K
+
t
)
P
P_2\simeq (RK+t)P
P2≃(RK+t)P
令
x
1
=
K
−
1
P
1
x_1=K^{-1}P_1
x1=K−1P1
x
2
=
K
−
1
P
2
x_2=K^{-1}P_2
x2=K−1P2
将
P
1
≃
K
P
P_1\simeq KP
P1≃KP
s
2
P
2
=
K
(
R
P
+
t
)
s_2P_2=K(RP+t)
s2P2=K(RP+t)
左右同左乘
K
−
1
K^{-1}
K−1,
得
K
−
1
P
1
≃
P
K^{-1}P_1\simeq P
K−1P1≃P
K
−
1
P
2
≃
I
(
R
P
+
t
)
K^{-1}P_2 \simeq I(RP+t)
K−1P2≃I(RP+t)
所以
x
2
≃
R
P
+
t
x_2\simeq RP+t
x2≃RP+t
x
2
≃
R
x
1
+
t
x_2\simeq Rx_1+t
x2≃Rx1+t
两边同左乘
t
^
t\hat{}
t^ (
t
^
为
反
对
称
矩
阵
,
有
t
^
t
=
0
t\hat{}为反对称矩阵,有t\hat{}t=0
t^为反对称矩阵,有t^t=0)
t
^
x
2
≃
t
^
R
x
1
t\hat{}x_2\simeq t\hat{}Rx_1
t^x2≃t^Rx1
两边再同左乘
x
2
T
x_2^T
x2T
x
2
T
t
^
x
2
≃
x
2
T
t
^
R
x
1
x_2^Tt\hat{}x_2\simeq x_2^Tt\hat{}Rx_1
x2Tt^x2≃x2Tt^Rx1
因为
t
^
x
2
t\hat{}x_2
t^x2是一个与
t
t
t和
x
2
x_2
x2都垂直的向量
所以,
x
2
T
t
^
x
2
=
0
x_2^Tt\hat{}x_2=0
x2Tt^x2=0,
x
2
T
t
^
R
x
1
=
0
x_2^Tt\hat{}Rx_1=0
x2Tt^Rx1=0
将之前的
x
1
=
K
−
1
P
1
x_1=K^{-1}P_1
x1=K−1P1,
x
2
=
K
−
1
P
2
x_2=K^{-1}P_2
x2=K−1P2 带入上式得
P
2
T
K
−
1
t
^
R
K
−
1
P
1
=
0
P_2^TK^{-1} t\hat{} RK^{-1}P_1=0
P2TK−1t^RK−1P1=0
(对极约束)
此外我们还定义
本质矩阵
E
=
t
^
R
E=t\hat{}R
E=t^R
基础矩阵
F
=
K
−
T
t
^
R
K
−
1
F=K^{-T}t\hat{}RK^{-1}
F=K−Tt^RK−1
因此相机得运动估计问题转化为了求解本质矩阵或基础矩阵的问题。(因其只相差内参矩阵,所以一般求解本质矩阵E)
位姿估计具体过程
1.根据配对像素点的像素位置求出本质矩阵或者基础矩阵
2.根据本质矩阵或基础矩阵求R、t