一、概述
程序看得太累了,就算及时记下来函数的用途,不停地跳转还是会头晕,我果然不适合玩3D游戏吗。
所以这里先把DSO论文梳理一遍,把算法弄明白大概会更好理解一些。
碎碎念:DSO的程序对新手真实不友好,要是能按模块分开该有多好。
二、简介
DSO论文全名《Direct Sparse Odometry》,作者是慕尼黑技术大学Jakob Engel,发表时间是2016年。
相关工作:
稀疏间接:monoSLAM、PTAM、ORBSLAM;
稠密间接:《Dense monocular
depth estimation in complex dynamic scenes》
稠密直接:DTAM、REMODE、LSDSLAM
稀疏直接:《A semi-direct approach to structure from motion》
混合:SVO
具体参考文献略。
动机:
1、直接法对像素操作,能生成更精细的模型,在稀疏纹理环境更加鲁棒;
2、稠密法需要考虑空间点之间的几何相关性,实时性降低,而且稠密建图时,几何相关性会引入偏置,降低精度;
贡献:
1、唯一的完整的估计所有模型参数的直接法。包括相机位姿、相机内参、逆深度;
2、进行完整几何光度校正,包括距离衰减、gamma校正和曝光时间,精度和鲁棒性都有提高;
3、在CPU上实时。(在笔记本上实时运行其实挺困难)
三、模型
参数说明:
x表示向量,H表示矩阵,t表示标量,
I
I
I表示函数,
T
i
∈
SE
(
3
)
T_i \in \textup{SE}(3)
Ti∈SE(3)表示点从世界坐标系到相机坐标系,
x
i
∈
s
e
(
3
)
x_i \in se(3)
xi∈se(3)表示李代数
⊕
\oplus
⊕
:
s
e
(
3
)
×
SE
(
3
)
→
SE
(
3
)
:se(3) \times \textup{SE}(3) \rightarrow \textup{SE}(3)
:se(3)×SE(3)→SE(3)表示左乘。
校正模型:
(1)几何相机校正模型:
采用针孔相机模型
定义
Π
c
:
R
3
→
Ω
\Pi _c: \mathbb{R}^{3} \rightarrow \Omega
Πc:R3→Ω为投影;
定义
Π
c
−
1
:
Ω
×
R
→
R
3
\Pi _c^{-1}: \Omega \times \mathbb{R} \rightarrow \mathbb{R} ^{3}
Πc−1:Ω×R→R3为逆投影;
c定义为相机内参,
Ω
\Omega
Ω表示像素坐标。
(2)光度相机校正模型
定义非线性响应函数
G
:
R
→
[
0
,
255
]
G:\mathbb{R}\rightarrow[0,255]
G:R→[0,255],距离衰减函数
V
:
Ω
→
[
0
,
1
]
V:\Omega\rightarrow[0,1]
V:Ω→[0,1]。
模型可以写成
I
i
(
x
)
=
G
(
t
i
V
(
x
)
B
i
(
x
)
)
I_i(\mathbf{x})=G(t_iV(\mathbf{x})B_i(\mathbf{x}))
Ii(x)=G(tiV(x)Bi(x))其中
B
i
B_i
Bi和
I
i
I_i
Ii为第i帧的辐照度和观测到的像素灰度,
t
i
t_i
ti为曝光时间。
这样可以计算光度校正灰度
I
i
′
(
x
)
=
t
i
B
i
(
x
)
=
G
−
1
(
I
i
(
x
)
)
V
(
x
)
I_i'(\mathbf{x})=t_iB_i(\mathbf{x})=\frac{G^{-1}(I_i(\mathbf{x}))}{V(\mathbf{x})}
Ii′(x)=tiBi(x)=V(x)G−1(Ii(x))
这里想分享一下我对 B i B_i Bi的理解,我查了一下辐射度量,找到7个不同的量,描述辐射能量,和论文比较接近的有两个,一个是辐射出射度M,一个是辐照度E。
辐射出射度是指离开光源表面单位面元的辐射通量
M = d Φ d A = d Q d A d t M=\frac{d \Phi }{dA}=\frac{dQ }{dAdt} M=dAdΦ=dAdtdQ
辐照度是指单位面元被照射的辐射通量
E = d Φ d A = d Q d A d t E=\frac{d \Phi }{dA}=\frac{dQ }{dAdt} E=dAdΦ=dAdtdQ
公式上差不多,看定义的话,这里的 B i B_i Bi应该是辐照度E。说起来,要是物体吸收了一部分辐射,那不是说明 V ( x ) V(\mathbf{x}) V(x)其实不只是距离衰减,意外发现。
稀疏直接法模型:
参考帧:
I
i
I_i
Ii
目标帧:
I
j
I_j
Ij
像素点:
p
∈
Ω
i
\mathbf{p}\in \Omega_i
p∈Ωi
然后论文中写到像素小邻域内的SSD类似于为中心像素添加一阶和二阶辐照度导数常数项。我查到这里和图像模板匹配很类似,参考【图像配准】基于灰度的模板匹配算法(一):MAD、SAD、SSD、MSD、NCC、SSDA、SATD算法,SSD公式如下:
D
(
i
,
j
)
=
∑
s
=
1
M
∑
t
=
1
N
[
S
(
i
+
s
−
1
,
j
+
t
−
1
)
−
T
(
s
,
t
)
]
2
D(i,j)=\sum_{s=1}^{M}\sum_{t=1}^{N}[S(i+s-1,j+t-1)-T(s,t)]^2
D(i,j)=s=1∑Mt=1∑N[S(i+s−1,j+t−1)−T(s,t)]2通过遍历图像,匹配
M
×
N
M\times N
M×N大小的子图可以使用SSD。
注:误差平方和算法(Sum of Squared Differences,简称SSD算法)
这样能量函数可以写成:
E
p
j
=
∑
p
∈
N
p
ω
p
∣
∣
I
j
′
[
p
′
]
−
I
i
′
[
p
]
∣
∣
γ
E_{\mathbf{p}_j}=\sum_{\mathbf{p}\in N_{\mathbf{p}}}\omega_{\mathbf{p}}||I_j'[\mathbf{p'}]-I_i'[\mathbf{p}]||_{\gamma}
Epj=p∈Np∑ωp∣∣Ij′[p′]−Ii′[p]∣∣γ其中
N
p
N_{\mathbf{p}}
Np表示小邻域,
γ
\gamma
γ表示鲁棒核函数,
I
j
′
I_j'
Ij′表示目标帧光度校正灰度。
即便添加了鲁棒核函数也还是和论文公式不同,主要是观测像素
I
j
I_j
Ij和光度不变辐射
I
j
′
I_j'
Ij′的关系还不明朗。
作者在另一篇论文(Large-Scale Direct SLAM with Stereo Cameras)中对能量函数进行过研究:
从论文图5的实验可以看到,两帧之间有较大的光度变化,图像匹配点的灰度曲线可以用直线拟合。
所以这里简化光度残差为(论文这里应该有笔误,不过问题不大):
r
u
(
ξ
)
=
a
I
1
(
u
)
+
b
−
I
2
(
u
′
)
r_{\mathbf{u}}(\xi)=aI_1(\mathbf{u})+b-I_2(\mathbf{u}')
ru(ξ)=aI1(u)+b−I2(u′)看到这里我才大致明白这个最重要的能量函数公式:
E
p
j
=
∑
p
∈
N
p
ω
p
∣
∣
(
I
j
[
p
′
]
−
b
j
)
−
t
j
e
a
j
t
i
e
a
i
(
I
i
[
p
]
−
b
i
)
∣
∣
γ
E_{\mathbf{p}_j}=\sum_{\mathbf{p}\in N_{\mathbf{p}}}\omega_{\mathbf{p}}||(I_j[\mathbf{p}']-b_j)-\frac{t_je^{a_j}}{t_ie^{a_i}}(I_i[\mathbf{p}]-b_i)||_{\gamma}
Epj=p∈Np∑ωp∣∣(Ij[p′]−bj)−tieaitjeaj(Ii[p]−bi)∣∣γ在《Direct Sparse Odometry》里将原来的代价函数修改为
e
−
a
i
(
I
i
−
b
i
)
e^{-a_i}(I_i-b_i)
e−ai(Ii−bi),目的是避免出现负值和一些数值问题。
其中
p
′
=
Π
c
(
R
Π
c
−
1
(
p
,
d
p
)
+
t
)
\mathbf{p}'=\Pi_c(\mathbf{R}\Pi_c^{-1}(\mathbf{p},d_{\mathbf{p}})+\mathbf{t})
p′=Πc(RΠc−1(p,dp)+t)
[
R
t
0
1
]
=
T
j
T
i
−
1
\begin{bmatrix} \mathbf{R} & \mathbf{t} \\ 0 & 1 \end{bmatrix}=\mathbf{T}_j\mathbf{T}_i^{-1}
[R0t1]=TjTi−1
影响能量函数的变量:
(1)像素点p的逆深度
d
p
d_{\mathbf{p}}
dp
(2)相机内参
c
\mathbf{c}
c
(3)对应帧位姿
T
i
,
T
j
\mathbf{T_i},\mathbf{T_j}
Ti,Tj
(4)光度变换参数
a
i
,
b
i
,
a
j
,
b
j
a_i,b_i,a_j,b_j
ai,bi,aj,bj。
所有点、所有帧的完整光度误差:
E
p
h
o
t
o
=
∑
i
∈
F
∑
p
∈
P
i
∑
j
∈
o
b
s
(
p
)
E
p
j
E_{photo}=\sum_{i\in\mathcal{F}}\sum_{\mathbf{p}\in\mathcal{P}_i}\sum_{j\in obs(\mathbf{p})}E_{\mathbf{p}_j}
Ephoto=i∈F∑p∈Pi∑j∈obs(p)∑Epji表示所有的帧,p表示第i帧所有点,j表示看见点p的所有帧。
窗口优化:
高斯牛顿优化模型
具体推导参考《视觉SLAM十四讲》,
H
=
J
T
W
J
\mathbf{H}=\mathbf{J}^T\mathbf{W}\mathbf{J}
H=JTWJ
b
=
−
J
T
W
r
\mathbf{b}=-\mathbf{J}^T\mathbf{W}\mathbf{r}
b=−JTWr其中J表示雅克比矩阵。
雅克比矩阵计算参考直接法光度误差导数推导,其实其他大部分推导问题也都可以在大神的博客里找到答案Jinge。
四、视觉里程计前端
目的:
(1)选择关键帧,特征点,包括去除外点算法和遮挡检测算法;
(2)初始化参数,并且要保证
p
′
\mathbf{p}'
p′的计算精度在2个像素以内;
(3)决定要边缘化的点和帧。
帧管理:
流程:
(1)初始帧跟踪:判断创建新的关键帧,所有激活点都投影到该关键帧,创建半稠密深度地图(应该就是dso运行时候那些彩色的点了),新的帧仅针对这一关键帧进行跟踪。
如果一帧的最终RMSE大于前一帧的两倍,认为图像匹配失败,这时尝试通过在不同方向上进行多达27个不同的小旋转来恢复,每次花费0.5ms。(这里应该就是对应程序里trackNewCoarse那一大段push_back了)
注:均方根误差(Root Mean Squared Error,简称RMSE)
这里分享一下我的疑问:图像RMSE的计算是不是所有像素的光度误差求均方根?
(2)关键帧创建
三个标准决定是否创建关键帧:
a、均方光流(视场变化)
f
=
(
1
n
Σ
i
=
1
n
∣
∣
p
−
p
′
∣
∣
2
)
1
2
f=(\frac{1}{n}\Sigma _{i=1}^{n}||\mathbf{p}-\mathbf{p}'||^2)^{\frac{1}{2}}
f=(n1Σi=1n∣∣p−p′∣∣2)21b、无旋转均值光流(相机平移)
f
=
(
1
n
Σ
i
=
1
n
∣
∣
p
−
p
t
′
∣
∣
2
)
1
2
f=(\frac{1}{n}\Sigma _{i=1}^{n}||\mathbf{p}-\mathbf{p}_t'||^2)^{\frac{1}{2}}
f=(n1Σi=1n∣∣p−pt′∣∣2)21c、两帧间相对光度因子(相机曝光)
a
=
∣
l
o
g
(
e
a
j
−
a
i
t
j
t
i
−
1
)
∣
a=|log(e^{a_j-a_i}t_jt_i^{-1})|
a=∣log(eaj−aitjti−1)∣
(3)关键帧边缘化
边缘化策略:
a、最新的两个关键帧不参与;
b、帧的可见点数小于5%;
c、计算的距离分数s最大:
s
(
I
i
)
=
d
(
i
,
1
)
∑
j
∈
[
3
,
n
]
/
i
(
d
(
i
,
j
)
+
ϵ
)
−
1
s(I_i)=\sqrt{d(i,1)}\sum_{j\in[3,n]/{i}}(d(i,j)+\epsilon )^{-1}
s(Ii)=d(i,1)j∈[3,n]/i∑(d(i,j)+ϵ)−1其中
d
(
i
,
j
)
d(i,j)
d(i,j)是
I
i
I_i
Ii和
I
j
I_j
Ij之间的欧氏距离,(图像之间的距离计算要怎么做?是稀疏特征之间吗?)
点管理:
实验说明图像数据高度冗余,dso维护一个固定的数量激活点(2000个),这些点服从均匀分布。
(1)候选点选取:
选择策略
a、在图像中分布均匀的点
b、相对于周围环境有足够高的图像梯度值的点
将图像分割成
32
×
32
32\times 32
32×32 个块,得到一个区域自适应梯度阈值,每个块计算一个阈值;
将图像分割为
d
×
d
d\times d
d×d个块,为每个块选择梯度最大的像素,实验发现弱梯度也是有用的,因此需要多次提取,阈值不断减低,块大小d不断调整;
(2)候选点跟踪:
候选点沿极线离散搜索来跟踪以最小化光度误差。
极线搜索具体参考《视觉SLAM十四讲》对极几何。
(3)候选点激活
当一组旧的点被边缘化后,新的候选点会激活来取代它们。
a、所有激活点投影到最新的关键帧
b、候选点投影到关键帧,到所有点的距离最大,则激活候选点(应该是块内,维持均匀分布)
(4)外点和遮挡检测:
a、候选点极线搜索时,丢弃最小值不够明显的点;
b、去除光度误差超过阈值的点;
五、总结
很多理解还挺稚嫩的,看程序有新的理解再修改。