回环检测算法：Stable Trangle Descriptor

回环检测是指检测传感器的两次测量（如图像、激光雷达扫描）是否发生在同一场景，它是对于SLAM问题至关重要。基于激光雷达的回环检测应该满足如下要求：

• 无论视点如何变化，回环检测方法应该实现旋转和平移不变性；
• 回环检测方法对不同的点云密度和环境应具有鲁棒性，因为激光扫描点云的稀疏性随距离、场景和激光雷达类型而变化；
• 回环检测算法能够更好的提供两帧激光扫描之间的相对位姿，一个好的初始值能让后续的配准算法收敛的更快更准确。

STD-LCD可以高效地检测闭环，并提供可靠的闭环几何变换。此外，它可以处理小重叠回环检测的挑战情况。与其他描述符中使用的多边形相比，三角形更稳定，因为三角形的形状是给定边长（或夹角）唯一确定的。此外，三角形的形状对于刚性变换是完全不变的。与关键点附近的的局部描述符相比，三角形的形状是旋转和平移不变的。基于这一性质，STD设计了一种算法，从三维点云中高效地提取局部关键点，并将这些关键点编码为三角形描述符。然后，通过匹配点云之间描述符的边长（和一些其他信息）来实现回环检测。从描述符匹配得到的点对关系可以进一步用于几何验证，这大大提高了回环检测的准确率。

关键点提取和描述符构建

当接收到累积子图后，STD-LCD对子图进行体素化，对体素中的点${\mathbf{p}}_i(i=1,\dots ,N)$求协方差矩$\mathbf{\Sigma }$。
$$\bar{\mathbf{p}}=\frac{1}{N}\sum _{i=1}^N{\mathbf{p}}_i;\mathbf{\Sigma }=\frac{1}{N}({\mathbf{p}}_i-\bar{\mathbf{p}})({\mathbf{p}}_i-\bar{\mathbf{p}}{)}^T$$

令${\lambda }_k$为$\mathbf{\Sigma }$第$k$大特征值，平面判定准则为${\lambda }_3<{\sigma }_1,{\lambda }_2>{\sigma }_2$，其中${\sigma }_1$和${\sigma }_2$是预设超参数，一般${\sigma }_1$为一个较小的值，${\sigma }_2$为一个较大的值。将满足平面判定准则的体素定义为平面体素，否则为非平面体素。

通过区域生长的方法将平面体素合并为更大的平面，即从一个随机的平面体素开始，如果其相邻体素有相同的平面法向量且平面距离小于阈值，则将其加入到当前正在生长的平面中。否则，如果相邻体素并不在同一平面上，则将其加入到当前正在生长的平面的边界体素表中。重复以上操作直到所有添加进来的体素都已经被扩展或者达到边界体素。

以生长得到的平面为图像平面，将边界体素的点投影到相邻平面上，以边界体素中的点到平面的最大距离为像素值。对于图像，将具有局部最大点到平面距离的像素中距离最大的点设置为关键点。将提取到的关键点构建成一个kd树，并对每一个关键点搜索其最近邻的20个关键点构建三角描述符。进行空间非极大值抑制，将具有相同边长的描述符去重，以增强提取的非重复性。

每个三角形描述符$\mathbf{\Delta }$包含的元素如下

• 三个顶点${\mathbf{p}}_1,{\mathbf{p}}_2,{\mathbf{p}}_3$，
• 三个对应投影平面的法向量${\mathbf{n}}_1,{\mathbf{n}}_2,{\mathbf{n}}_3$，
• 三个边长$l_{12},l_{23},l_{13}$且$l_{12}\le l_{23}\le l_{13}$
• 三角形的中心$\mathbf{q}$
• 描述符所属的关键帧索引$k$

除了描述符，我们还将保存从这个子图中提取的所有$n$个平面$\mathbf{\Pi }=({\mathbf{\pi }}_1,{\mathbf{\pi }}_2,\dots ,{\mathbf{\pi }}_n)$，用于之后的几何验证步骤。

回环检测与验证

回环检测与验证包括三部分：粗回环检测、细回环检测和几何验证。粗循环检测通过在描述符数据库的哈希表中搜索匹配的历史描述符，可以快速地提取出N个可能的候选。精细回环检测通过计算配对三角形之间的所有变换矩阵，并对变换进行聚类，将包含变换数最多的聚类簇的中心作为优秀候选。最后，计算当前帧到优秀候选者的平面重合度筛选误检，选择点到平面距离最小的候选作为最佳候选。由于精细回环检测提供了一个较好的初始猜测，使得之后的ICP配准更快收敛。

由于一个子图可以提取上百个描述符，为了快速查询和匹配描述符，我们使用哈希表存储所有描述符。我们使用描述子中具有旋转和平移不变性的6个属性来计算哈希密钥，分别为边长$l_{12},l_{23},l_{13}$和投影法向量的点积${\mathbf{n}}_1\cdot {\mathbf{n}}_2,{\mathbf{n}}_2\cdot {\mathbf{n}}_3,{\mathbf{n}}_1\cdot {\mathbf{n}}_3$。具有6个相似属性的描述符将具有相同的哈希密钥，因此存储在同一个容器中。对于一个查询帧，我们提取其所有描述符。对于每个描述符${\mathbf{\Delta }}_i$，计算其哈希密钥，将其定位到哈希表中对应的容器中，并对该容器中存在的所有描述符的关键帧进行投票。当查询帧中的所有描述符${\mathbf{\Delta }}_i$都被处理后，匹配过程结束。投票数前十多的关键帧将被选为描述符的候选关键帧，用于回环检测。

当给定一个候选回环关键帧时，我们进行几何验证以消除由于描述符匹配不正确而导致的误检。由于边长确定后三角形的形状是唯一确定的，一旦${\mathbf{\Delta }}_a$匹配到${\mathbf{\Delta }}_b$，它们的顶点$({\mathbf{p}}_{a1},{\mathbf{p}}_{a2},{\mathbf{p}}_{a3})$和$({\mathbf{p}}_{b1},{\mathbf{p}}_{b2},{\mathbf{p}}_{b3})$将自然匹配。然后利用这个对应关系，根据\ref{pt2ptICP}节，通过奇异值分解（Singular Value Decomposition，SVD）很容易计算出这两个关键帧之间的相对变换$\mathbf{T}=(\mathbf{R},\mathbf{t})$：
$$\begin{array}{rl}& \mathbf{W}=\sum _{i=1}^3({\mathbf{p}}_{a_i}-{\mathbf{q}}_a)({\mathbf{p}}_{b_i}-{\mathbf{q}}_b)\\ & [\mathbf{U},\mathbf{S},\mathbf{V}]=\mathbf{SVD}(\mathbf{W})\\ & \mathbf{R}=\mathbf{V}{\mathbf{U}}^T,\mathbf{t}=-\mathbf{R}\ast {\mathbf{q}}_a+{\mathbf{q}}_b.\end{array}$$

为了增加鲁棒性，使用RANSAC方法来寻找最大化正确匹配描述符数量的刚体变换。

基于此刚体变换，我们计算当前帧与候选帧之间的平面重叠度来进行几何验证。
用中心点$\mathbf{q}$和法向量$\mathbf{u}$ 表示的平面$\mathbf{\pi }$。记当前帧的平面集为${}^B\Pi =[{(}^B{\mathbf{q}}_1,{}^B{\mathbf{u}}_1),\dots {(}^B{\mathbf{q}}_n,{}^B{\mathbf{u}}_n)]$，候选帧的平面集为${}^C\Pi =[{(}^C{\mathbf{q}}_1,{}^C{\mathbf{u}}_1),\dots {(}^C{\mathbf{q}}_n,{}^C{\mathbf{u}}_n)]$，刚体变换为${}^C{\mathbf{T}}_B=({}^C{\mathbf{R}}_B,{}^C{\mathbf{t}}_B)\in SE(3)$，其中$n$为当前帧的平面数，$m$为候选框的平面数。我们以${}^C\Pi$的中心点$({}^C{\mathbf{q}}_1,{}^C{\mathbf{q}}_2,\dots ,{}^C{\mathbf{q}}_m)$构建一个kd树（k=3）。则对于每个平面中心点${}^B{\mathbf{q}}_i(i=1,2,\dots ,n)\in {}^B\Pi$，首先用${}^C{\mathbf{T}}_B$对${}^B{\mathbf{q}}_i$进行变换，然后在kd树中搜索一个最近的点${}^C{\mathbf{q}}_j$，通过法向量差和点到平面的距离判断两个平面是否重合：
$$\begin{array}{r}\Vert {}^C{\mathbf{R}}_B{}^B{\mathbf{u}}_i-{}^C{\mathbf{u}}_j{\Vert }_2<{\sigma }_n\\ {}^C{\mathbf{u}}_j^T{(}^C{\mathbf{T}}_B{}^B{\mathbf{q}}_i-{}^C{\mathbf{q}}_j)<{\sigma }_d\end{array}$$
其中${\sigma }_n$和${\sigma }_d$是用来判断平面重合的预设参数。

计算当前帧满足式\ref{plane_overlap}的平面占所有平面的比例：
$$N_c=\frac{N_{\text{coincide}}}{N_{\text{sum}}}\times 100\%,$$
其中$N_{\text{coincide}}$为当前帧所有满足约束的平面数，$N_{\text{sum}}$为当前帧所有平面的数量。

如果$N_c$超过阈值${\sigma }_{pc}$，我们就认为这是一个可靠的回环检测。由于平面的数量远远少于点云的数量，基于平面的几何验证比基于ICP的验证方法效率更高。按照式\ref{STD-ICP}优化点面距离和法向量差异可以得到回环的精确刚体变换。

$$\underset{{}^C{\mathbf{T}}_B}{\min }\Vert {}^C{\mathbf{R}}_B{}^B{\mathbf{u}}_i-{}^C{\mathbf{u}}_j{\Vert }_2^2+{\left({}^C{\mathbf{u}}_j^T{(}^C{\mathbf{T}}_B{}^B{\mathbf{q}}_i-{}^C{\mathbf{q}}_j)\right)}^2$$

STD-LCD回环检测通过求解如式所示的优化问题可以直接得到查询帧$i$与回环帧$j$之间的相对位姿约束$\Delta {\mathbf{T}}_{ij}$，构造回环因子并加入到因子图中：

$$F_{\mathcal{L}}=\sum _{(i,j)\in \mathcal{L}}{\Vert \mathrm{Log}\left(\Delta {\mathbf{T}}_{ij}^{-1}\left({\mathbf{X}}_i^{-1}{\mathbf{X}}_j\right)\right)\Vert }_{{\mathbf{\Sigma }}_{{\mathcal{L}}_{ij}}}^2$$
其中${\mathbf{X}}_i$为查询帧全局位姿变量，${\mathbf{X}}_j$为回环帧全局位姿变量，${\Sigma }_{{\mathcal{L}}_{ij}}$为对应的信息矩阵，$\mathcal{L}$为回环约束帧索引集合。