参考书籍
参考书籍:《视觉SLAM十四讲》 高翔博士
整体流程
1. 坐标系变换
已知:匹配的像素点对
(
u
,
v
)
,
(
u
′
,
v
′
)
(u,v),(u^{'},v^{'})
(u,v),(u′,v′),相机内参矩阵
K
K
K
求解:相机归一化坐标
(
X
′
,
Y
′
,
1
)
(X^{'},Y^{'},1)
(X′,Y′,1)
通过相机与像素间的坐标变换关系,可以得到下面公式:
[
u
v
1
]
=
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
[
X
′
Y
′
1
]
\left[ \begin{matrix} u\\ v \\ 1 \end{matrix} \right]= \left[ \begin{matrix} f_{x} & 0 & c_{x}\\ 0 & f_{y} & c_{y}\\ 0 & 0 & 1 \end{matrix} \right]\left[ \begin{matrix} X^{'}\\ Y^{'}\\ 1 \end{matrix} \right]
⎣⎡uv1⎦⎤=⎣⎡fx000fy0cxcy1⎦⎤⎣⎡X′Y′1⎦⎤表示像素坐标与相机归一化平面坐标的变换关系。
2. 对极约束
利用对极约束关系,具体推导可参见博客,利用求得的相机归一化坐标
x
1
,
x
2
x_{1},x_{2}
x1,x2,可以求解基础矩阵和本质矩阵:
x
2
E
x
1
=
0
x_{2}Ex_{1}=0
x2Ex1=0分解
E
E
E便可以得到变换矩阵
R
,
t
R,t
R,t,注意,由于E(本质矩阵)本身具有尺度等价性,它分解得到的
R
、
t
R、t
R、t也有一个尺度等价性。
3. 单目初始化
在单目视觉中,我们对两张图像的t进行归一化,相当于固定了一个尺度。虽然我们不知道它的实际长度是多少,但是我们可以以这时的 t t t为单目1,计算相机运动和特征点的3D位置,这一步称为单目SLAM的初始化。
通常把 t t t 进行归一化,让它的长度等于1.
初始化的两张图片必须要有一定程度的平移,而后的轨迹和地图都将会以这一步的平移为单位。单目初始化不能只有纯旋转,必须要有一定程度的平移,如果没有平移,单目将无法初始化。
4. 三角测量
按照对极几何中的定义,如果设
x
1
,
x
2
x_1,x_{2}
x1,x2为两个特征点的归一化坐标,于是有下列关系式:
s
1
x
1
=
s
2
x
2
R
+
t
s_{1}x_{1}=s_{2}x_{2}R+t
s1x1=s2x2R+t已知:坐标系
o
到
o
1
o到o_{1}
o到o1的变换矩阵
R
、
t
R、t
R、t归一化坐标
x
1
,
x
2
x_{1},x_{2}
x1,x2,现在要求的就是两个特征点的深度
s
1
,
s
2
s_{1},s_{2}
s1,s2
左乘
[
x
1
]
×
[x_{1}]_{\times}
[x1]×可得
s
1
[
x
1
]
×
x
1
=
0
s_{1}[x_{1}]_{\times}x_{1}=0
s1[x1]×x1=0:
s
2
[
x
1
]
×
x
2
R
+
[
x
1
]
×
t
=
0
s_{2}[x_{1}]_{\times}x_{2}R+[x_{1}]_{\times}t =0
s2[x1]×x2R+[x1]×t=0可以解出
s
2
s_{2}
s2, 将其带入原来的式子可解出
s
1
s_{1}
s1.
但是,由于噪声的存在,我们求出来的R,t不一定能够使得上式精确等于0,因此在实际情况中,更常见的做法是求最小二乘解而不是零解。
5. 求解空间3D坐标
已知:像素点坐标
(
u
,
v
)
(u,v)
(u,v),相机内参矩阵
K
K
K,像素点的空间深度
s
s
s
求解:相机坐标
(
X
,
Y
,
Z
)
(X,Y,Z)
(X,Y,Z)
通过相机与像素间的坐标变换关系,可以得到下面公式:
s
[
u
v
1
]
=
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
[
X
Y
Z
]
s\left[ \begin{matrix} u\\ v \\ 1 \end{matrix} \right]= \left[ \begin{matrix} f_{x} & 0 & c_{x}\\ 0 & f_{y} & c_{y}\\ 0 & 0 & 1 \end{matrix} \right]\left[ \begin{matrix} X\\ Y\\ Z \end{matrix} \right]
s⎣⎡uv1⎦⎤=⎣⎡fx000fy0cxcy1⎦⎤⎣⎡XYZ⎦⎤
利用计算得到的相邻两帧相机变换关系
R
,
t
R,t
R,t 以及相机坐标
P
c
=
(
X
,
Y
,
Z
)
P_{c}=(X,Y,Z)
Pc=(X,Y,Z),可以求解世界坐标
P
w
P_{w}
Pw
P
c
=
R
∗
P
w
+
t
P_{c}=R*P_{w}+t
Pc=R∗Pw+t注意:这是以第一帧相机原点作为世界坐标系的原点进行求得,后面由于相机在不断移动,因此相机间的变换矩阵在不断改变,通过变换矩阵累乘的方式将相机坐标变化为世界坐标。
重点:
如果要计算第
n
n
n 个相机下,其相机坐标到世界坐标的变换,需要用以下的公式计算:
已知:第一帧相机到第二帧相机的变换矩阵 T 12 T_{12} T12(世界坐标到相机坐标系变换就很),第二帧相机到第三帧相机的变换矩阵 T 23 T_{23} T23,以此类推,第 n − 1 n-1 n−1帧相机到第 n n n帧相机的变换矩阵 T ( n − 1 ) n T_{(n-1)n} T(n−1)n, 第 n n n帧相机下的相机坐标 P c ( n ) P^{(n)}_{c} Pc(n)
求解:第 n n n帧相机下的世界坐标 P w ( n ) P^{(n)}_{w} Pw(n)
P w ( n ) = T 12 ∗ T 23 ∗ . . . . . . ∗ T ( n − 1 ) n ∗ P c ( n ) P^{(n)}_{w}=T_{12}*T_{23}*......*T_{(n-1)n}*P^{(n)}_{c} Pw(n)=T12∗T23∗......∗T(n−1)n∗Pc(n)其中 T 12 ∗ T 23 ∗ . . . . . . ∗ T ( n − 1 ) n ∗ P c ( n ) T_{12}*T_{23}*......*T_{(n-1)n}*P^{(n)}_{c} T12∗T23∗......∗T(n−1)n∗Pc(n)就是相机到世界坐标系的欧式变换矩阵,是重投影误差的初始值。
6. 最小化重投影误差
已知:3D空间坐标 P i P_{i} Pi,及其投影像素坐标 u i u_{i} ui,相机的初始位姿 T T T(相机到世界坐标系的变换矩阵),可以由第(5)步得到。
求解目标函数:
T
∗
=
arg min
T
1
2
∣
∣
u
i
−
1
s
i
K
T
P
i
∣
∣
2
T^{*}=\argmin_{T} \frac{1}{2}||u_{i}-\frac{1}{s_{i}}KTP_{i}||^{2}
T∗=Targmin21∣∣ui−si1KTPi∣∣2根据高斯牛顿法,可得:
e
(
x
+
Δ
x
)
=
e
(
x
)
+
J
T
Δ
x
e(x+\Delta{x})=e(x)+J^{T}\Delta{x}
e(x+Δx)=e(x)+JTΔx其中
J
T
J^{T}
JT表示目标函数(误差)关于
T
T
T的一阶导数,雅可比矩阵,根据链式法则可以将其分解如下:
其中
P
′
P^{'}
P′表示相机坐标,因此可以将求解的世界坐标转换为对相机坐标求导。
可以得雅可比矩阵:
通过高斯牛顿或者LM法不断优化重投影误差,便可以得到较为准确的位姿。