参考
仓库 https://github.com/victorprad/InfiniTAM
论文 https://arxiv.org/pdf/1604.01093.pdf
BundleFusion框架
BoundleFusion算法的核心是抛弃了常用的slam框架(帧间匹配+回环检测优化),作者认为帧间匹配特别脆弱容易出现跟踪失败的问题,所以作者采用了一种新的框架来实现实时的全局位姿优化,既然是实时全局位姿优化那就不需要回环检测要消除累积误差。为了实现实时的全局位姿优化,作者提出了局部优化和全局优化两步走的方法,该两步优化的前提都是准确的帧间特征点匹配,因此就有了上面的新的算法框架,在获取到rgbd数据后,首先进行特征点数据关联生成关联信息,然后进行局部优化生成chunk关键帧,然后再对chunk关键帧进行全局优化生成位姿信息,最后再根据调整后的位姿进行tsdf模型的更新。
数据关联
提取SIFT关键点和描述子,然后匹配当前帧与先前所有帧生成潜在配对信息,然后再根据关键点相关性滤波、表面区域滤波和稠密验证三步来对配对信息进行过滤,确保配对的准确性。对细节感兴趣可看论文4.1节。
局部优化
为了使得后面的全局优化实时进行,数据帧的规模不能太大,因此需要使用类似于关键帧的策略。此处采取的 策略是将所有的数据帧划分成一个个小的chunk,每个chunk包含11帧数据,并且chunk之间有一帧重叠。而局部优化就是优化每个chunk内的帧的位姿,并且针对每个chunk生成一个关键帧传给全局优化环节。
优化的目标函数如下:
E
a
l
i
g
n
(
χ
)
=
ω
s
p
a
r
s
e
E
s
p
a
r
s
e
(
χ
)
+
ω
d
e
n
s
e
E
d
e
n
s
e
(
χ
)
E_{align}(\boldsymbol{\chi}) = \omega_{sparse}E_{sparse}(\boldsymbol{\chi}) + \omega_{dense}E_{dense}(\boldsymbol{\chi})
Ealign(χ)=ωsparseEsparse(χ)+ωdenseEdense(χ)
式中
(
ω
s
p
a
r
s
e
,
E
s
p
a
r
s
e
)
(\omega_{sparse},E_{sparse})
(ωsparse,Esparse)分别为稀疏匹配权重及误差函数,
(
ω
d
e
n
s
e
,
E
d
e
n
s
e
)
(\omega_{dense},E_{dense})
(ωdense,Edense)分别为稠密匹配权重及误差函数,
χ
\boldsymbol{\chi}
χ表示chunk内的位姿。稀疏匹配的误差函数如下:
E
s
p
a
r
s
e
(
χ
)
=
∑
i
=
1
∣
s
∣
∑
j
=
1
∣
s
∣
∑
(
k
,
l
)
∈
C
(
i
,
j
)
∣
s
∣
∥
τ
i
p
i
,
k
−
τ
j
p
j
,
l
∥
2
2
E_{sparse}(\boldsymbol{\chi})=\sum_{i=1}^{|s|}\sum_{j=1}^{|s|}\sum_{(k,l)\in C(i,j)}^{|s|} \left\| \tau_{i}\mathbf{p}_{i,k} - \tau_{j}\mathbf{p}_{j,l} \right\|_{2}^{2}
Esparse(χ)=i=1∑∣s∣j=1∑∣s∣(k,l)∈C(i,j)∑∣s∣∥τipi,k−τjpj,l∥22
式中
(
τ
i
,
τ
j
)
(\tau_i,\tau_j)
(τi,τj)表示匹配帧的位姿,
(
p
i
,
k
,
p
j
,
l
)
(\mathbf{p}_{i,k}, \mathbf{p}_{j,l})
(pi,k,pj,l)表示匹配帧中对应的匹配点,从误差函数的定义可以看出,优化该误差函数的目标是保证匹配的三维特征点之间的欧式距离尽可能小。稠密匹配的误差函数如下:
E
d
e
n
s
e
(
χ
)
=
ω
p
h
o
t
o
E
p
h
o
t
o
(
χ
)
+
ω
g
e
o
E
g
e
o
(
χ
)
E
p
h
o
t
o
(
χ
)
=
∑
(
i
,
j
)
∈
E
∑
k
=
0
∣
I
i
∣
∥
I
i
(
π
(
d
i
,
k
)
)
−
I
j
(
π
(
τ
j
−
1
τ
i
d
k
)
)
∥
2
2
E
g
e
o
(
χ
)
=
∑
(
i
,
j
)
∈
E
∑
k
=
0
∣
D
i
∣
[
n
i
,
k
T
(
d
i
,
k
−
τ
i
−
1
)
τ
j
π
−
1
(
D
j
(
π
(
τ
j
−
1
τ
i
d
i
,
k
)
)
)
]
2
\begin{aligned} E_{dense}(\boldsymbol{\chi}) &= \omega_{photo}E_{photo}(\boldsymbol{\chi}) + \omega_{geo}E_{geo}(\boldsymbol{\chi})\\ E_{photo}(\boldsymbol{\chi}) &= \sum_{(i,j)\in\mathbf{E}}\sum_{k=0}^{|I_{i}|}\left\| I_{i}(\pi(\mathbf{d_{i,k}}))-I_{j}(\pi(\tau_{j}^{-1}\tau_{i}\mathbf{d}_{k}))\right\|_{2}^{2}\\ E_{geo}(\boldsymbol{\chi}) &= \sum_{(i,j)\in\mathbf{E}}\sum_{k=0}^{|D_{i}|}[\mathbf{n}_{i,k}^{T}(\mathbf{d}_{i,k} - \tau_{i}^{-1})\tau_{j}\pi^{-1}(D_{j}(\pi(\tau_{j}^{-1}\tau_{i}\mathbf{d}_{i,k})))]^{2} \end{aligned}
Edense(χ)Ephoto(χ)Egeo(χ)=ωphotoEphoto(χ)+ωgeoEgeo(χ)=(i,j)∈E∑k=0∑∣Ii∣∥∥Ii(π(di,k))−Ij(π(τj−1τidk))∥∥22=(i,j)∈E∑k=0∑∣Di∣[ni,kT(di,k−τi−1)τjπ−1(Dj(π(τj−1τidi,k)))]2
包括光度梯度误差和点到面的距离误差,为什么要叫稠密匹配,主要原因就是上面的两个误差函数是针对整个图像的所有像素,为了减少计算量,图像被压缩到80x60,这里要说一下第二个误差函数,其指代的就是点到面的icp算法的误差函数。
经过上述目标函数优化之后,根据chunk内帧的位姿信息,将对应的特征点进行最小二乘融合,融合完之后,将多帧内都出现的特征点投影到起始参考帧中,这样就形成了chunk关键帧。
全局优化
全局优化的过程与上面局部优化的过程是一致的,只是操作对象变成了全局所有的chunk关键帧。
tsdf更新
由于每次优化后相机位姿都会发生改变,因此需要重新更新tsdf模型,包括de-integration和integration两个过程,分别对应下面两个公式:
integration:
D
′
(
v
)
=
D
(
v
)
W
(
v
)
+
w
i
(
v
)
d
i
(
v
)
W
(
v
)
+
w
i
(
v
)
,
W
′
=
W
(
v
)
+
w
i
(
v
)
D^{'}(v)=\frac{D(v)W(v)+w_{i}(v)d_{i}(v)}{W(v)+w_{i}(v)},W^{'}=W(v) + w_{i}(v)
D′(v)=W(v)+wi(v)D(v)W(v)+wi(v)di(v),W′=W(v)+wi(v)
de-integration:
D
′
(
v
)
=
D
(
v
)
W
(
v
)
−
w
i
(
v
)
d
i
(
v
)
W
(
v
)
−
w
i
(
v
)
,
W
′
=
W
(
v
)
−
w
i
(
v
)
D^{'}(v)=\frac{D(v)W(v)-w_{i}(v)d_{i}(v)}{W(v)-w_{i}(v)},W^{'}=W(v) - w_{i}(v)
D′(v)=W(v)−wi(v)D(v)W(v)−wi(v)di(v),W′=W(v)−wi(v)