三角测量
考虑图像 I 1 I_{1} I1 和 I 2 , I_{2}, I2, 以左图为参考,右图的变换矩阵为 T T T 。相机光心为 O 1 O_{1} O1 和 $O_{2} 。 在 。 在 。在I_{1}$ 中有特征点 p 1 , p_{1}, p1, 对应 I 2 I_{2} I2 中有特征点 p 2 p_{2} p2 。理论上直线 O 1 p 1 O_{1} p_{1} O1p1 与 O 2 p 2 O_{2} p_{2} O2p2 在 场景中会相交于一点 P,该点即是两个特征点所对应的地图点在三维场景中的位置。然而由于噪声的影响,这两条直线往往无法相交。因此,可以通过最二小乘去求解。
设
x
1
,
x
2
x _{1}, x _{2}
x1,x2 为两个特征点的归一化坐标,那么它们满足:
s
1
x
1
=
s
2
R
x
2
+
t
s_{1} x _{1}=s_{2} R x _{2}+ t
s1x1=s2Rx2+t
比方说先来看 $s_{2} $。如果我要算
s
2
,
s_{2},
s2, 那么先对上式两侧左乘一个
x
1
∧
,
x _{1}^{\wedge},
x1∧, 得:
0
=
s
2
x
1
∧
R
x
2
+
x
1
∧
t
0=s_{2} x _{1}^{\wedge} R x _{2}+ x _{1}^{\wedge} t
0=s2x1∧Rx2+x1∧t
可看成
s
2
s_{2}
s2 的一个方程,可以根据它直接求得
s
2
s_{2}
s2。
3D-2D: PnP(Perspective-n-Point)
PnP
是求解 3D 到 2D 点对运动的方法。它描述了当我们知道
n
n
n个3D空间点以及它们的投影位置时,如何估计相机所在的位姿。前面已经说了,2D-2D的对极几何方法需要8个或8个以上的点对(以八点法为例),且存在着初始化、纯旋转和尺度的问题。然而,如果两张图像中,其中一张特征点的3D位置已知,那么最少只需三个点对(需要至少一个额外点验证结果)就可以估计相机运动。
直接线性变
考虑某个空间点
P
,
P,
P, 它的齐次坐标为
P
=
(
X
,
Y
,
Z
,
1
)
T
P =(X, Y, Z, 1)^{T}
P=(X,Y,Z,1)T 。在图像
I
1
I_{1}
I1 中,投影到特征 点
x
1
=
(
u
1
,
v
1
,
1
)
T
x _{1}=\left(u_{1}, v_{1}, 1\right)^{T}
x1=(u1,v1,1)T (以归一化平面齐次坐标表示)。此时相机的位姿
R
,
t
R , t
R,t 是未知的。与 单应矩阵的求解类似,我们定义增广矩阵
[
R
∣
t
]
[R|t]
[R∣t] 为一个
3
×
4
3 \times 4
3×4 的矩阵,包含了旋转与平移信息。我们把它的展开形式列写如下:
(
u
1
v
1
1
)
=
(
t
1
T
t
2
T
t
3
T
)
(
X
Y
Z
1
)
\left(\begin{array}{c} u_{1} \\ v_{1} \\ 1 \end{array}\right) =\left(\begin{array}{c} \boldsymbol{t_1}^T \\ \boldsymbol{t_2}^T \\ \boldsymbol{t_3}^T \end{array}\right)\left(\begin{array}{c} X \\ Y \\ Z \\ 1 \end{array}\right)
⎝⎛u1v11⎠⎞=⎝⎛t1Tt2Tt3T⎠⎞⎝⎜⎜⎛XYZ1⎠⎟⎟⎞
t
1
,
t
2
,
t
3
\boldsymbol{t_1}, \boldsymbol{t_2}, \boldsymbol{t_3}
t1,t2,t3为3个4维向量,则我们得到两个约束:
t
1
T
P
−
t
3
T
P
u
1
=
0
t
2
T
P
−
t
3
T
P
v
1
=
0
\begin{aligned} &t _{1}^{T} P - t _{3}^{T} P u_{1}=0\\ &t _{2}^{T} P - t _{3}^{T} P v_{1}=0 \end{aligned}
t1TP−t3TPu1=0t2TP−t3TPv1=0
请注意
t
t
t 是待求的变量,可以看到每个特征点提供了两个关于
t
t
t 的线性约束。假设 共有 N 个特征点,可以列出线性方程组:
(
P
1
T
0
−
u
1
P
1
T
0
P
1
T
−
v
1
P
1
T
⋮
⋮
⋮
P
N
T
0
−
u
N
P
N
T
0
P
N
T
−
v
N
P
N
T
)
(
t
1
t
2
t
3
)
=
0
\left(\begin{array}{ccc} P _{1}^{T} & 0 & -u_{1} P _{1}^{T} \\ 0 & P _{1}^{T} & -v_{1} P _{1}^{T} \\ \vdots & \vdots & \vdots \\ P _{N}^{T} & 0 & -u_{N} P _{N}^{T} \\ 0 & P _{N}^{T} & -v_{N} P _{N}^{T} \end{array}\right)\left(\begin{array}{l} t _{1} \\ t _{2} \\ t _{3} \end{array}\right)=0
⎝⎜⎜⎜⎜⎜⎛P1T0⋮PNT00P1T⋮0PNT−u1P1T−v1P1T⋮−uNPNT−vNPNT⎠⎟⎟⎟⎟⎟⎞⎝⎛t1t2t3⎠⎞=0
由于
t
t
t 一共有12维,因此最少通过六对匹配点,即可实现矩阵
T
T
T 的线性求解,这种方法称为直接线性变换
(Direct Linear Transform, DLT)。
最小化重投影误差
除了使用线性方法之外,我们可以把PnP问题构建成一个定义于李代数上的非线性最小二乘问题。
假设某空间点坐标为
P
i
=
[
X
i
,
Y
i
,
Z
i
]
T
,
P _{i}=\left[X_{i}, Y_{i}, Z_{i}\right]^{T},
Pi=[Xi,Yi,Zi]T, 其投影的像素坐标为
u
i
=
[
u
i
,
v
i
]
T
u _{i}=\left[u_{i}, v_{i}\right]^{T}
ui=[ui,vi]T。我们定义增广矩阵
T
=
[
R
∣
t
]
T=[R|t]
T=[R∣t] 为一个
3
×
4
3 \times 4
3×4 的矩阵,它的李代数表示为
ξ
\xi
ξ,包含了旋转与平移。
s
i
u
i
=
K
(
T
P
i
)
[
1
:
3
]
,
T
=
[
R
t
0
T
1
]
=
exp
(
ξ
∧
)
s_{i} u _{i}= K (T P _{i})_{[1:3]},\\ T=\left[\begin{array}{ll} R & t \\ 0^{T} & 1 \end{array}\right] =\exp \left( \xi ^{\wedge}\right)
siui=K(TPi)[1:3],T=[R0Tt1]=exp(ξ∧)
隐含着的齐次坐标到非齐次的转换,否则按矩阵的乘法来说,维度是不对的。
T
P
i
T P_{i}
TPi 结果是
4
×
1
4 \times 1
4×1 的,而它左侧的
K
K
K 是
3
×
3
3 \times 3
3×3 的,所以必责把
T
P
i
T P _{i}
TPi 的前三维取出来,变成三维的非齐次坐标。或者:
s
i
[
u
i
v
i
1
]
=
K
[
R
∣
t
]
3
×
4
[
X
i
Y
i
Z
i
1
]
s_{i}\left[\begin{array}{c} u_{i} \\ v_{i} \\ 1 \end{array}\right]= K [R|t]_{3\times 4}\left[\begin{array}{c} X_{i} \\ Y_{i} \\ Z_{i} \\ 1 \end{array}\right]
si⎣⎡uivi1⎦⎤=K[R∣t]3×4⎣⎢⎢⎡XiYiZi1⎦⎥⎥⎤
该问题的误差项,是将像素坐标(观测到的投影位置)与 3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差,所以称之为重投影误差。构建最小二乘问题,然后寻找最好的相机位姿,使它最小化。
T
∗
=
arg
min
ξ
1
2
∑
i
=
1
n
∥
u
i
−
1
s
i
K
(
T
P
i
)
[
1
:
3
]
∥
2
2
T ^{*}=\arg \min _{ \xi } \frac{1}{2} \sum_{i=1}^{n}\left\| u _{i}-\frac{1}{s_{i}} K (T P _{i})_{[1:3]}\right\|_{2}^{2}
T∗=argξmin21i=1∑n∥∥∥∥ui−si1K(TPi)[1:3]∥∥∥∥22
3D-3D: ICP(Iterative Closest Point)
假设我们有一组配对好的3D点:
P
=
{
p
1
,
…
,
p
n
}
,
P
′
=
{
p
1
′
,
…
,
p
n
′
}
P =\left\{ p _{1}, \ldots, p _{n}\right\}, \quad P ^{\prime}=\left\{ p _{1}^{\prime}, \ldots, p _{n}^{\prime}\right\}
P={p1,…,pn},P′={p1′,…,pn′}
现在,想要找一个欧氏变换
R
,
t
,
R , t ,
R,t, 使得:
∀
i
,
p
i
=
R
p
i
′
+
t
\forall i, p _{i}= R p _{i}^{\prime}+ t
∀i,pi=Rpi′+t
SVD 方法
根据前面描述的ICP问题,我们先定义第
i
i
i 对点的误差项:
e
i
=
p
i
−
(
R
p
i
′
+
t
)
e _{i}= p _{i}-\left( R p _{i}^{\prime}+ t \right)
ei=pi−(Rpi′+t)
然后,构建最小二乘问题,求使误差平方和达到极小的
R
,
t
:
R, t:
R,t:
min
R
,
t
J
=
1
2
∑
i
=
1
n
∥
(
p
i
−
(
R
p
i
′
+
t
)
)
∥
2
2
\min _{ R , t } J=\frac{1}{2} \sum_{i=1}^{n}\left\|\left( p _{i}-\left( R p _{i}^{\prime}+ t \right)\right)\right\|_{2}^{2}
R,tminJ=21i=1∑n∥(pi−(Rpi′+t))∥22
非线性优化方法
求解ICP的另一种方式是使用非线性优化,以迭代的方式去找最优值。该方法和我们前面讲述的PnP非常相似。以李代数表达位姿时,目标函数可以写成:
min
ξ
=
1
2
∑
i
=
1
n
∥
(
p
i
−
exp
(
ξ
∧
)
p
i
′
)
∥
2
2
\min _{\xi}=\frac{1}{2} \sum_{i=1}^{n}\left\|\left( p _{i}-\exp \left( \xi ^{\wedge}\right) p _{i}^{\prime}\right)\right\|_{2}^{2}
ξmin=21i=1∑n∥(pi−exp(ξ∧)pi′)∥22
单个误差项关于位姿导数已经在前面推导过了,使用李代数扰动模型即可:
∂
e
∂
δ
ξ
=
−
(
exp
(
ξ
∧
)
p
i
′
)
⊙
\frac{\partial e}{\partial \delta \xi}=-\left(\exp \left(\xi^{\wedge}\right) p_{i}^{\prime}\right)^{\odot}
∂δξ∂e=−(exp(ξ∧)pi′)⊙