Details of PL-slam loopclosure detection and its optimization method

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]策略：
$$\begin{array}{rl}{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{array}$$
其中：$n_k$,$n_l$分别是关键帧的点特征与线特征的数量，$d_k$,$d_l$分别是点特征与线特征的分散值；

[2]方法：

其思路是当前帧于过去评分最高的帧之间交叉验证。
[3]方法：
对当前帧与最相似帧进行点线的特征匹配，之后通过三角测量计算出有效的变换：
$${\hat{\mathbf{\xi }}}_{ij}\in \mathfrak{s}\mathfrak{e}(3)$$
之后进行回环错误检测，去除假阳性结果，真正的阳性满足：
$$\begin{gathered} Eigenvalue(\sum {\hat{\mathbf{\xi }}}_{ij})<0.01 \\ Max(rotatio{n}_{error})<3.00° \\ Max(translatio{n}_{error})<0.50m \\ inlier{s}_{r}ation>50\% \end{gathered}$$

[4]方法：
讲回环融入之前错误的回环地图，这是一个PGO问题；定义损失函数为当前帧与回环检测帧之间变换的 $\mathfrak{s}\mathfrak{e}(3)$ 李代数：
$${\mathbf{r}}_{ij}\left({\mathbf{\xi }}_{iw},{\mathbf{\xi }}_{jw}\right)=\log \left(\exp \left({\hat{\mathbf{\xi }}}_{ij}\right)\cdot \exp \left({\mathbf{\xi }}_{jw}\right)\cdot \exp {\left({\mathbf{\xi }}_{iw}\right)}^{-1}\right)$$
其中运算：
$$\begin{gathered} log:SE(3)->\mathfrak{s}\mathfrak{e}(3) \\ exp:\mathfrak{s}\mathfrak{e}(3)->SE(3) \end{gathered}$$
PGO问题可以使用g2o库中优化函数解出；

(1)Optimazation

分析：
PL-SLAM中关于回环检测的部分的相关函数有：

insertKFBowVectorPL();

is_candidates();

第一个函数计算出相似度评分，第二个函数给出最小相似评分（相当于一个阈值）；直接提高某一帧的相似度评分并不能直接将此帧作为候选帧，所以我们的优化策略放在第二个函数中；

方法：
我们想着可以利用里程计几何关系并行判断回环，当里程计检测到回到回环时，我们可以降低最小相似评分，从而使最相似的帧可以通过候选判断；
这里我们定义出最小相似度系数：
$$Si{m}_{min}$$
我们知道漂移问题影响最大的不是位移里程计（因为其位移时特征的深度尺度是可估计的），而是偏航角度，当偏航角越大时，视觉里程计的置信度就越低，我们定义里程计置信度：
$$Co{v}_d$$
可以知道它与最后一次回环纠正后的偏航角负相关：
$$Co{v}_d\propto \frac{1}{\Delta ya{w}_{init}+1}$$
于此同时得到形状相似判断最相似的帧$Fram{e}_j$与当前帧$Fram{e}_i$，可以计算出位置与偏航角的偏差，$k_1$为一个比例系数：
$$Diff=\Delta ya{w}_{ij}\ast k_1+\Delta Tra{n}_{ij}$$
之后我们就可以得到我们的最小相似度系数,$k_2$也是一个小于1的比例系数：
$$Si{m}_{min}=k_2\ast Diff\ast Co{v}_d=\frac{(\Delta ya{w}_{ij}\ast k_1+\Delta T_{ij})}{\Delta ya{w}_{init}+1}\ast k_2$$
之后选择一个很好的表达本问题的负相关函数即可。

再分析：
我们这里得到的最小相似度系数一定小于1，所以我们的方法肯定可以提高算法的召回率；但同时这个方法的不足也很明显，首先以上公式都是线性相乘，并没有多考虑一下置信度会带来哪些影响，同时也没有结合之前的路径信息来判断回环。

召回率过高的缺点：
召回率高的同时准确率会下降，这样后面对位姿图验证（这点很关键，有错误的回环纠正将对建图是毁灭性影响）的次数就会增多，提升了系统的运算量，可能会降低系统实时性。