Preface:.
(0)General
单独线程
//加入新的关键帧
addKeyFrame();
...
//经过此函数生成了conf_matrix[n][n]的方阵,记录着所有关键帧之间的相似度
insertKFBowVectorPL(){
...
//使用bow算法对点特征计算相似度
dbow_voc_p.transform();
...
//使用bow算法对线特征计算相似度
dbow_voc_l.transform();
...
//使用论文提出的评分策略[1]对所有关键帧评分
score_all();
}
/*基于方法[2]根据关键帧临近与分数大于最小回环评分计数满足的关键帧,如果超过4个则
判断为回环检测的候选帧,且保留最相似的帧*/
is_candidates();
//基于方法[3]估计当前帧与最相似帧的相对位姿,去除假阳性,不会影响召回率
isLoopClosure();
//基于方法[4]回环修正地图
loopClosureOptimizationCovGraphG2O();
[1]策略:
s
t
=
0.5
(
n
k
/
(
n
k
+
n
l
)
+
d
k
/
(
d
k
+
d
l
)
)
s
k
+
0.5
(
n
l
/
(
n
k
+
n
l
)
+
d
l
/
(
d
k
+
d
l
)
)
s
l
.
\begin{aligned} s_{t}=& 0.5\left(n_{k} /\left(n_{k}+n_{l}\right)+d_{k} /\left(d_{k}+d_{l}\right)\right) s_{k} \\ &+0.5\left(n_{l} /\left(n_{k}+n_{l}\right)+d_{l} /\left(d_{k}+d_{l}\right)\right) s_{l} . \end{aligned}
st=0.5(nk/(nk+nl)+dk/(dk+dl))sk+0.5(nl/(nk+nl)+dl/(dk+dl))sl.
其中:
n
k
n_k
nk,
n
l
n_l
nl分别是关键帧的点特征与线特征的数量,
d
k
d_k
dk,
d
l
d_l
dl分别是点特征与线特征的分散值;
[2]方法:
其思路是当前帧于过去评分最高的帧之间交叉验证。
[3]方法:
对当前帧与最相似帧进行点线的特征匹配,之后通过三角测量计算出有效的变换:
ξ
^
i
j
∈
s
e
(
3
)
\hat{\boldsymbol{\xi}}_{i j} \in \mathfrak{s e}(3)
ξ^ij∈se(3)
之后进行回环错误检测,去除假阳性结果,真正的阳性满足:
E
i
g
e
n
v
a
l
u
e
(
∑
ξ
^
i
j
)
<
0.01
M
a
x
(
r
o
t
a
t
i
o
n
e
r
r
o
r
)
<
3.00
°
M
a
x
(
t
r
a
n
s
l
a
t
i
o
n
e
r
r
o
r
)
<
0.50
m
i
n
l
i
e
r
s
r
a
t
i
o
n
>
50
%
Eigenvalue (\sum \hat{\boldsymbol{\xi}}_{i j})<0.01 \\ Max(rotation_{error})<3.00\degree \\ Max(translation_{error})<0.50m \\ inliers_ration>50\%
Eigenvalue(∑ξ^ij)<0.01Max(rotationerror)<3.00°Max(translationerror)<0.50minliersration>50%
[4]方法:
讲回环融入之前错误的回环地图,这是一个PGO问题;定义损失函数为当前帧与回环检测帧之间变换的
s
e
(
3
)
\mathfrak{s e}(3)
se(3) 李代数:
r
i
j
(
ξ
i
w
,
ξ
j
w
)
=
log
(
exp
(
ξ
^
i
j
)
⋅
exp
(
ξ
j
w
)
⋅
exp
(
ξ
i
w
)
−
1
)
\mathbf{r}_{i j}\left(\boldsymbol{\xi}_{i w}, \boldsymbol{\xi}_{j w}\right)=\log \left(\exp \left(\hat{\boldsymbol{\xi}}_{i j}\right) \cdot \exp \left(\boldsymbol{\xi}_{j w}\right) \cdot \exp \left(\boldsymbol{\xi}_{i w}\right)^{-1}\right)
rij(ξiw,ξjw)=log(exp(ξ^ij)⋅exp(ξjw)⋅exp(ξiw)−1)
其中运算:
l
o
g
:
S
E
(
3
)
−
>
s
e
(
3
)
e
x
p
:
s
e
(
3
)
−
>
S
E
(
3
)
log: SE(3)->\mathfrak{s e}(3) \\ exp:\mathfrak{s e}(3)->SE(3)
log:SE(3)−>se(3)exp:se(3)−>SE(3)
PGO问题可以使用g2o库中优化函数解出;
(1)Optimazation
分析:
PL-SLAM中关于回环检测的部分的相关函数有:
insertKFBowVectorPL();
is_candidates();
第一个函数计算出相似度评分,第二个函数给出最小相似评分(相当于一个阈值);直接提高某一帧的相似度评分并不能直接将此帧作为候选帧,所以我们的优化策略放在第二个函数中;
方法:
我们想着可以利用里程计几何关系并行判断回环,当里程计检测到回到回环时,我们可以降低最小相似评分,从而使最相似的帧可以通过候选判断;
这里我们定义出最小相似度系数:
S
i
m
m
i
n
Sim_{min}
Simmin
我们知道漂移问题影响最大的不是位移里程计(因为其位移时特征的深度尺度是可估计的),而是偏航角度,当偏航角越大时,视觉里程计的置信度就越低,我们定义里程计置信度:
C
o
v
d
Cov_{d}
Covd
可以知道它与最后一次回环纠正后的偏航角负相关:
C
o
v
d
∝
1
Δ
y
a
w
i
n
i
t
+
1
Cov_{d}∝\frac{1}{\Delta{yaw_{init}}+1}
Covd∝Δyawinit+11
于此同时得到形状相似判断最相似的帧
F
r
a
m
e
j
Frame_j
Framej与当前帧
F
r
a
m
e
i
Frame_i
Framei,可以计算出位置与偏航角的偏差,
k
1
k_1
k1为一个比例系数:
D
i
f
f
=
Δ
y
a
w
i
j
∗
k
1
+
Δ
T
r
a
n
i
j
Diff=\Delta{yaw_{ij}}*k_1+\Delta{Tran_{ij}}
Diff=Δyawij∗k1+ΔTranij
之后我们就可以得到我们的最小相似度系数,
k
2
k_2
k2也是一个小于1的比例系数:
S
i
m
m
i
n
=
k
2
∗
D
i
f
f
∗
C
o
v
d
=
(
Δ
y
a
w
i
j
∗
k
1
+
Δ
T
i
j
)
Δ
y
a
w
i
n
i
t
+
1
∗
k
2
Sim_{min}=k_2*Diff*Cov_d=\frac{(\Delta{yaw_{ij}}*k_1+\Delta{T_{ij}})}{\Delta{yaw_{init}}+1}*k_2
Simmin=k2∗Diff∗Covd=Δyawinit+1(Δyawij∗k1+ΔTij)∗k2
之后选择一个很好的表达本问题的负相关函数即可。
再分析:
我们这里得到的最小相似度系数一定小于1,所以我们的方法肯定可以提高算法的召回率;但同时这个方法的不足也很明显,首先以上公式都是线性相乘,并没有多考虑一下置信度会带来哪些影响,同时也没有结合之前的路径信息来判断回环。
召回率过高的缺点:
召回率高的同时准确率会下降,这样后面对位姿图验证(这点很关键,有错误的回环纠正将对建图是毁灭性影响)的次数就会增多,提升了系统的运算量,可能会降低系统实时性。