基于全局特征描述子的激光SLAM回环检测方法( 二 ) _图像配准

和
分别为边角特征点云和平面特征点云对应的特征向量；
为关键帧
的全局描述子向量；
和
分别为关键帧
的边角特征和平面特征。
图1 基于全局特征描述子的回环检测算法流程
1.1 特征向量提取
使用全局点云中具有代表性信息的边角特征和平面特征进行数据处理，对里程计模块中提取的边角特征点云
和平面特征点云
分别提取特征向量。取
或
中的任意一点
，使用
-维(KD)树搜索其所在点云中邻近的
个点，计算该部分点云的协方差矩阵，具体步骤如下。
步骤1 计算邻近
点的质心：
步骤2 计算协方差矩阵
，即质心
到邻近点云中所有点
的向量与其转置的内积：
【基于全局特征描述子的激光SLAM回环检测方法】步骤3 对协方差矩阵进行奇异值分解(SVD)，求特征值：
式中：
为矩阵
对应的特征值；为矩阵
对应的特征向量；
为对角线
组成的对角矩阵；
为奇异值分解后的实正交矩阵。
步骤4 由
解得对应的特征向量
。
其中，边角特征提取线向量为特征向量
，邻近点投影至该方向上的向量方差最最小，为最大特征值
对应的特征向量
；平面特征提取法向量为特征向量
，邻近点投影至该方向上的向量方差最最大，为最小特征值λ1对应的特征向量
[14] 。
1.2 特征描述和相似度计算
设计全局特征描述子的计算方法。通过计算全局点云
中所有点的三维坐标平均值得到当前帧的点云质心
，以点云
和
中所有点对应的特征向量的均值作为质心
处对应的特征向量
，进而计算点云的全局特征描述子，边角点云
和平面点云
分开计算。
遍历点云
中的所有点，确定以质心
为原点
的参考坐标系
：
式中：
为点云
或
中任意选取的一点；
为点云质心处的归一化特征向量；
为点
到质心
之间的欧氏距离。以
，
确定的坐标系如图2所示，其中：
为点
处归一化之后的特征向量。
图2 全局点云下的uvw坐标系
以上述计算得到的
坐标系为基准，计算点云特征向量相对于基准坐标系3个坐标轴的角度分布，则有：
式中：
为点
，
之间的欧氏距离；
为特征向量
与坐标轴v之间的夹角，值域为[-1，1]；
为
，
之间连线方向与坐标轴
之间的夹角，值域为[-1，1]；
为特征向量
投影至
平面时与坐标轴
之间的夹角，值域为[?π/2，π/2] 。
利用上述公式进行计算后，
，
，
，
为非线性相关的，可以替代两点的(x，y，z，
，
，
)共12维数据对点云的分布进行几何关系描述。
遍历点云
和
中所有点，与质心点
构成点对，计算对应的
，
，
，d四要素。将
，
，
的值域区间进行等分，统计全局点云三要素在各区间内的分布情况，构成特征描述子的角度分量。
计算全局点云
中距离质心点
最远的位置，将最大距离均分为多个区间，统计各个点相对于质心距离在区间中的分布，以此构成特征描述子的尺度分量。
最终设计的点云全局描述特征描述子为230维向量，其中前105维为边角特征点云
计算得到的角度分量
，
，
，中间105维数据为平面特征点云