单应矩阵分解
这一篇是单应矩阵(Homography Matrix)分解得到
R
,
首先,设空间上的点
X=(XYZ)T
和点在图像平面的坐标
x=(xy1)T
,它们有如下关系:
令第一个相机坐标系为世界坐标,投影模型为 P1=K1[I|0] , P2=K2[R|t] 。设3D点所在的世界平面的方程为 aX+bY+cZ=d ,平面单位法向量 n=( abc)T ,对在平面上的点 X ,平面方程可表示为
对于两个相机坐标系的点有如下关系
由于点 X1 满足直线方程 (2) ,有 1dnTX1=1 ,则把该式乘到 (3) 式 t 的右侧得
根据单应性变换,对图像齐次坐标 x=(xy1)T 和齐次单应矩阵 H ,两个图像坐标满足
对比 (4) 和 (5) 我们有
对 A 进行奇异值分解得到
有 s=detUdetV , s2=1 ,令
我们有
取基底 e=(100)T , e=(010)T , e=(001)T ,令 n′=(x1x2x3)T=x1e1+x2e2+x3e3 ,根据 (7) 式可得
即有三个等式
注意到
n
是单位法向量,
V
是旋转矩阵,则
这里的 R′ 也是旋转矩阵,满足 R′TR′=I 。也就是 R′ 具有保范性,也就是 ∥R′X∥=∥X∥ ,对 (9) 式两边取二范数可得
(10) 式是一个关于 x21 、 x22 和 x23 的一个齐次线性方程,其必然有非零解,那么系数矩阵的秩小于 3 ,其行列式必为
根据 A 奇异值的大小顺序
- d1≠d2≠d3
- d1=d2≠d3 或者 d1≠d2=d3
- d1=d2=d3
对于上述3种情况,都有
d′=±d2
,而解
d′=±d1
和
d′=±d3
都是不可能的。对于第一种我们考虑如果
d′=d1
,则根据
(10)
式有
由于 d1≥d2≥d3 ,但是根据上式得到的是 x2=x3=0 ,那么不满足 n′ 是单位阵的条件 Σ3i=1x2i=1 ,则这样的情况不存在。
如果
d1≠d3
,根据式
(10)
以及
Σ3i=1x2i=1
,有
接下来分成两种情况进行讨论
当 d′=d2>0
1. d1≠d2≠d3
由于
x2=0
,代入式
(8.2)
得
e2=R′e2
,所以
R′
是沿着轴
e2
旋转,那么有
把 R′ 代回式 (9.3) ,有
结合式 (12) 可以解得
由于 n′Tn′=1 , (8) 式两边同乘以 n′ 然后移位可以得到
把式 (12)∼(13) 代回 (15) 有
2. d1=d2≠d3
看式子
(12)
,这时有
x1=x2=0
,
x3=±1
,这时
n′=(00±1)T
,解为
3. d1=d2=d3
这时
x1
,
x2
,
x3
未定义,根据式子
(8)
,
(9)
有
当 d′=−d2<0
1. d1≠d2≠d3
这时
d′=−d2
,代入式
(8.2)
得
−e2=R′e2
,这时的
R′
可以表示为
同样把
R′
带回
(9.3)
,得
结合式 (12) 可以解得
把式 (12) , (19) 和 (20) 代回 (8) 有
2. d1=d2≠d3
同样结合式子
(12)
,这时有
x1=x2=0
,
x3=±1
,这时的解为
3. d1=d2=d3
根据公式
(7)
有
−d′I=d′R′+t′n′T
,也就有,对于与
n′
垂直(在平面内)的向量
x
,有
R′x=−x
,根据householder变换得到:
解的分析
上述的两大类情况,除了未定义的情况,2和3都是1的特例。由于我们不知道
d′
的符号,所以一共有8个可能的解。考虑
其中 R′ 中的 sinθ 中有 ε1ε3 项, t′ 中存在 n′ ,所以对 ε1=±1 , ε3±1 一共有四种解,对于 d′<0 和 d′>0 一共有 4×2 种情况。但是从物理的角度去考虑,只有 2 种情况是可能的解。考虑
由于 A 是已经确定的,所以
所以只有其中两个 n 是满足条件的。
在Faugeras论文中提到,当观测到的点不是全部靠近一个相机光心而远离另外一个相机光心,那么只有一个解满足所有的点。如果有
d′>0
,对于上述两个解
(n′1,t′1)
和
(n′2,t′2)
,这两个解有如下关系,也就是两个解是相同的。
而 ORB-SLAM2的代码,是在求出所有的8个解之后,对每一个解进行分析,检测点是不是都在两个相机的前方,并且统计重投影误差较小的点的个数,找出8个解中统计得到点数最多的的那个解作为最终的解。
参考
- 泡泡机器人ORB-SLAM2注释
- Faugeras O D, Lustman F. Motion and structure from motion in a piecewise planar environment.1988
- Ezio Malis, Manuel Vargas, and others. Deeper understanding of the homography decomposition for vision-based control. 2007
- Z. Zhang, and A.R. Hanson. 3D Reconstruction based on homography mapping, in Proc. ARPA96, pp. 1007-1012, 1996