目的：通过特征点匹配，计算两个图像对应的相机位姿，理解SLAM的过程，记录下，供后面理解。
之前一篇博客，计算过fundmental 矩阵。详细见那篇博客。
计算极限的方程。
这篇博客记录计算fundmental矩阵，再通过fundmental矩阵转化为相机的 [ R , t ] [R,t] [R,t]
计算fundmetal矩阵，还是通过极限约束。计算fundmental矩阵。
假设
１） O 1 O_1 O1为世界坐标系。通过 [ R , t ] [R,t] [R,t]矩阵转化到 O 2 O_2 O2坐标系。 x x x为 x 1 , x 2 x_1,x_2 x1,x2转换到世界坐标系的交点。
２） O 1 O_1 O1相机的内参矩阵可以写为 K 1 K_1 K1, O 2 O_2 O2的相机内参矩阵为 K 2 K_2 K2;因为相机 O 1 O_1 O1为世界坐标系下,因此它的外参矩阵为 [ I , 0 ] [I,0] [I,0];同时相机 O 2 O_2 O2为 [ R , t ] [R,t] [R,t]。
因为相机的投影公式
d
1
x
1
=
K
1
I
x
(
1
)
d_1 x_1 = K_1Ix \space \space(1)
d1x1=K1Ix (1)
d
2
x
2
=
K
2
(
R
x
+
t
)
(
2
)
d_2 x_2 = K_2(Rx+t) \space \space(2)
d2x2=K2(Rx+t) (2)
由
(
1
)
(1)
(1)公式得到如下：
d
1
x
1
=
K
1
I
x
=
>
x
=
d
1
K
1
−
1
x
1
=
d
1
x
1
~
(
3
)
d_1 x_1 = K_1Ix =>x=d_1K_1^{-1}x_1 = d_1\tilde{x_1} \space \space \space \space(3)
d1x1=K1Ix=>x=d1K1−1x1=d1x1~ (3)
其中
x
1
~
=
K
1
−
1
x
1
\tilde{x_1}=K_1^{-1}x_1
x1~=K1−1x1表示像平面的顶点，意思就是
d
1
=
1
d_1=1
d1=1(深度)时候的平面，在
x
O
1
xO_1
xO1射线和这个像平面交点，坐标
x
1
~
\tilde{x_1}
x1~在
x
O
1
xO_1
xO1射线上。
由公式
(
2
)
(2)
(2)可以得到公式：
d
2
x
2
=
K
2
(
R
x
+
t
)
=
>
x
=
R
−
1
(
d
2
K
2
−
1
x
2
−
t
)
=
R
−
1
(
d
2
x
2
~
−
t
)
(
4
)
d_2 x_2 = K_2(Rx+t) \\ => x=R^{-1}(d_2K_2^{-1} x_2 -t)= R^{-1}(d_2\tilde{x_2} -t) \space \space \space \space(4)
d2x2=K2(Rx+t)=>x=R−1(d2K2−1x2−t)=R−1(d2x2~−t) (4)
其中
x
2
~
\tilde{x_2}
x2~是在
O
2
O_2
O2坐标系下，
O
2
x
O_2x
O2x和深度
d
2
=
1
d_2=1
d2=1的相交点，它也在
O
2
x
O_2x
O2x的射线上。
有公式
(
3
)
(
4
)
(3)(4)
(3)(4)得到：
d
1
x
1
~
=
R
−
1
(
d
2
x
2
~
−
t
)
=
>
d
2
x
2
~
=
R
d
1
x
1
~
+
t
=
>
d
2
(
t
×
x
2
~
)
=
d
1
(
t
×
R
x
1
~
)
+
t
×
t
(
5
)
d_1\tilde{x_1}=R^{-1}(d_2\tilde{x_2}-t) \\=>d_2\tilde{x_2}=Rd_1\tilde{x_1}+t \\=>d_2 (t \times \tilde{x_2})=d_1(t \times R\tilde{x_1})+t \times t \space \space \space \space(5)
d1x1~=R−1(d2x2~−t)=>d2x2~=Rd1x1~+t=>d2(t×x2~)=d1(t×Rx1~)+t×t (5)
因为公式中
t
×
t
=
0
t \times t=0
t×t=0,因此公式(5)中后面一项为
0
0
0。
公式
(
5
)
(5)
(5)可以得到以下：
d
2
(
t
×
x
2
~
)
=
d
1
(
t
×
R
x
1
~
)
=
>
d
2
(
x
2
~
T
R
(
t
×
x
2
~
)
)
=
d
1
(
x
2
~
T
(
t
×
R
x
1
~
)
)
(
6
)
d_2 (t \times \tilde{x_2})=d_1(t \times R\tilde{x_1}) \\ =>d_2 ( \tilde{x_2}^TR (t \times \tilde{x_2}))=d_1(\tilde{x_2}^T( t \times R\tilde{x_1})) \space \space \space \space(6)
d2(t×x2~)=d1(t×Rx1~)=>d2(x2~TR(t×x2~))=d1(x2~T(t×Rx1~)) (6)
同样在叉乘中
t
×
x
2
~
t \times \tilde{x_2}
t×x2~和
x
2
~
\tilde{x_2}
x2~垂直，因为叉乘的几何意义。公式
(
6
)
(6)
(6)为
0
0
0因此得到：
d
1
(
x
2
~
T
(
t
×
R
x
1
~
)
)
=
0
(
7
)
d_1(\tilde{x_2}^T( t \times R\tilde{x_1})) =0 \space \space \space \space(7)
d1(x2~T(t×Rx1~))=0 (7)
简化公式
(
7
)
(7)
(7)得到如下：
x
2
~
T
(
t
×
R
x
1
~
)
=
0
=
>
x
2
~
T
E
x
1
~
=
0
(
8
)
\tilde{x_2}^T( t \times R\tilde{x_1}) =0 \\ =>\tilde{x_2}^T E \tilde{x_1} =0 \space \space \space \space(8)
x2~T(t×Rx1~)=0=>x2~TEx1~=0 (8)
其中
E
=
t
×
R
E=t \times R
E=t×R
代入公式
K
1
−
1
x
1
=
x
1
~
K_1^{-1}x_1=\tilde{x_1}
K1−1x1=x1~;
K
2
−
1
x
2
=
x
2
~
=
>
x
2
~
T
=
x
2
T
K
2
−
T
K_2^{-1}x_2=\tilde{x_2} =>\tilde{x_2}^T =x_2^TK_2^{-T}
K2−1x2=x2~=>x2~T=x2TK2−T;得到如下：
x
2
~
T
E
x
1
~
=
0
=
>
x
2
T
K
2
−
T
E
K
1
−
1
x
1
=
0
=
>
x
2
T
F
x
1
=
0
(
9
)
\tilde{x_2}^T E \tilde{x_1} =0 \\ =>x_2^TK_2^{-T}EK_1^{-1}x_1=0 \\=>x_2^TFx_1=0 \space \space \space \space(9)
x2~TEx1~=0=>x2TK2−TEK1−1x1=0=>x2TFx1=0 (9)
其中
F
=
K
2
−
T
E
K
1
−
1
F=K_2^{-T}EK_1^{-1}
F=K2−TEK1−1
以上是基于极限约束计算的
F
F
F矩阵，它也是相交的内外参数的乘积。至于怎么求得
F
F
F下面将介绍。
比较基本的方法就是下面的八点法
八点线性方法获取相机直接的fundmental矩阵。
先看一下基础矩阵
F
F
F，
- 3 × 3 3 \times 3 3×3的矩阵，但是它的秩是 2 2 2
- 有７个自由度
- 满足上述的极限约束
- 有奇异值约束 [ σ 1 , σ 2 , 0 ] [\sigma_1,\sigma_2,0] [σ1,σ2,0](有两个非０奇异值)
因此可以得到８个匹配点可以优化得到 F F F矩阵。
对于一对匹配点
[
u
1
,
v
1
,
1
]
[u_1,v_1,1]
[u1,v1,1],
[
u
2
,
v
2
,
1
]
[u_2, v_2,1]
[u2,v2,1],它的基础矩阵为
F
=
[
F
11
F
12
F
13
F
21
F
22
F
23
F
31
F
32
F
33
]
F=\begin{bmatrix} F_{11} & F_{12} & F_{13} \\ F_{21} & F_{22} & F_{23} \\ F_{31} & F_{32} & F_{33} \end{bmatrix}
F=⎣⎡F11F21F31F12F22F32F13F23F33⎦⎤
用极限约束可以得到如下：
[
u
1
,
v
1
,
1
]
F
[
u
2
v
2
1
]
=
0
[u_1,v_1,1]F\begin{bmatrix} u_{2} \\ v_{2} \\ 1 \end{bmatrix} = 0
[u1,v1,1]F⎣⎡u2v21⎦⎤=0
将
F
F
F矩阵拉成一个线性，可以得到如下公式
[
u
2
u
1
,
u
2
v
1
,
u
2
,
u
1
v
2
,
v
1
v
2
,
v
2
,
u
1
,
v
1
,
1
]
f
=
0
[u_2u_1, u_2v_1,u_2,u_1v_2,v_1v_2,v_2,u_1,v_1,1]f=0
[u2u1,u2v1,u2,u1v2,v1v2,v2,u1,v1,1]f=0
其中
f
=
[
F
11
,
F
12
,
F
13
,
F
21
,
F
22
,
F
23
,
F
31
,
F
32
,
F
33
]
T
f=[F_{11},F_{12},F_{13}, F_{21},F_{22},F_{23},F_{31},F_{32},F_{33}]^T
f=[F11,F12,F13,F21,F22,F23,F31,F32,F33]T
上述的公式，如果有８个对应点，就可以计算得到
F
F
F矩阵。
上述的公式，如果有８个匹配点，它们可以组成一个矩阵，就可以求得
f
f
f的值。如果大于
8
8
8个匹配点，就可以利用最小二乘法求解。得到
F
F
F.
上述方法有自己缺点，不能保证奇异值约束，因此需要重构。重构的公式如下：
m
i
n
∣
∣
F
−
F
^
∣
∣
,
s
v
d
(
F
)
=
[
σ
1
,
σ
2
,
0
]
min||F-\hat{F}||, svd(F)=[\sigma_1,\sigma_2,0] \space
min∣∣F−F^∣∣,svd(F)=[σ1,σ2,0]
一般情况下奇异值分解如下：
F
^
=
U
S
V
T
w
i
t
h
S
=
d
i
a
g
(
σ
1
,
σ
2
,
σ
3
)
\hat{F}=USV^T \\ with \space S=diag(\sigma_1,\sigma_2, \sigma_3)
F^=USVTwith S=diag(σ1,σ2,σ3)
而基础矩阵只有两个非
0
0
0奇异值。因此得到：
F
=
U
d
i
a
g
(
σ
1
,
σ
2
,
0
)
V
T
F=Udiag(\sigma_1,\sigma_2,0)V^T
F=Udiag(σ1,σ2,0)VT
利用上述的奇异值约束对基础矩阵进行重构。
上述的只是单独获取８对匹配点
(
x
1
n
,
x
2
n
)
(x_1^n,x_2^n)
(x1n,x2n)
但是匹配顶点多数，因此需要用RANSAC作为一个框架计算基础矩阵。
下面的RANSAC的框架。
它的算法流程：
- 随机采样8个顶点匹配点
- 8个顶点求解基础矩阵 F ^ \hat{F} F^
- 奇异值约束获取基础矩阵 F F F
- 计算误差，统计内点个数
- 重复上述过程，选择内点数最多的结果
- 对所有内点执行2,3，重新计算 F F F
内点判断标准-Sampson Distance
d
(
x
1
,
x
2
)
=
(
x
2
T
F
x
1
)
2
(
F
x
1
)
x
2
+
(
F
x
2
)
y
2
+
(
x
2
T
F
)
x
2
+
(
x
2
T
F
)
y
2
d(x_1,x_2)=\frac{(x_2^TFx_1)^2}{(Fx_1)^2_x+(Fx_2)^2_y + (x_2^TF)_x^2+(x_2^TF)_y^2}
d(x1,x2)=(Fx1)x2+(Fx2)y2+(x2TF)x2+(x2TF)y2(x2TFx1)2
其中有
d
(
x
1
,
x
2
)
<
τ
d(x_1,x_2)<\tau
d(x1,x2)<τ
上述使用ransac计算基础矩阵。
上述计算基础矩阵
F
F
F，要获取本征矩阵
E
E
E
E
^
=
K
2
T
F
K
1
=
>
E
^
=
U
d
i
a
g
(
σ
1
,
σ
2
,
0
)
V
T
=
>
E
=
U
d
i
a
g
(
σ
1
+
σ
2
2
,
σ
1
+
σ
2
2
,
0
)
V
T
\hat{E}=K_2^TFK_1 \\ => \hat{E}=Udiag(\sigma_1,\sigma_2,0)V^T \\ => E = Udiag(\frac{\sigma_1+\sigma_2}{2}, \frac{\sigma_1+\sigma_2}{2}, 0)V^T
E^=K2TFK1=>E^=Udiag(σ1,σ2,0)VT=>E=Udiag(2σ1+σ2,2σ1+σ2,0)VT
5549
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
