skimage直方图如何保存_激光SLAM | 基于2D 直方图的快速闭环

论文题目：A fast, complete, point cloud based loop closure for LiDAR odometry and mapping

开源代码：https://github.com/hku-mars/loam_livox

这个论文和上次讨论的"loam livox"是同一个作者写的，而且两个论文的算法在同一套开源代码里。loam livox的论文讲解链接如下：

激光SLAM | 基于固态雷达的里程计：Loam Livox​zhuanlan.zhihu.com

loam livox是讲怎样做一个基于固态雷达的里程计，而本论文讲的是怎样给这个里程计加闭环。这个论文里公式有点多，我按照我自己的理解在这里和大家讨论，如果有表述不对的地方，欢迎指正。

一、整体概述

既然是设计闭环，而且是基于特征的闭环，那么整个算法流程就可以按照以下流程走了：

1.特征提取和保存

2.闭环检测，也即基于特征做索引，找出相似关键帧

3.闭环匹配，即对相似的关键帧做帧间匹配，获得其相对位姿

4.优化，根据获得的相对位姿在概率图中添加对应的边，执行图优化，根据结果修正地图

二、算法介绍

下面我们就流程中的各个部分依次说呗

1. 计算2D直方图

这一步又分为以下几个小步骤：

1）把点云划分为cell

把空间位置按照xyz坐标，分成多个cell，按照几何位置，把点云中的点分到对应的cell中，每个cell根据cell中点的坐标计算均值和方差，公式如下：

2）对每个cell计算特征类型和方向

有了方差矩阵以后，我们对其进行特征值分解，按照特征值大小顺序，得到

而每个cell的特征类型就是根据特征值的情况来定的：

a. 如果

远大于

则认为是面特征，特征的方向是方差矩阵的第三列，也就是面的法向量

b. 如果不是面特征，且

远大于

则认为是线特征，特征方向是方差矩阵的第一列，也就是线的方向

c. 不是以上两种，则木有特征

3）计算旋转矩阵

这里的旋转矩阵并不是点云位姿对应的旋转矩阵，而是特征要给一个旋转矩阵，让特征按照这个矩阵旋转以后，能够使大多数线特征的方向，以及面特征边缘（面的边缘其实就是线）方向处于X轴上，它的目的是为了使特征具有旋转不变性。

具体公式不列出了，看论文吧

4）划分区域

一帧点云有了特征，要对这些特征进行存储，作者采用的做法是这样的，我们在3）中不是计算了一个旋转矩阵吗，对这个旋转它的两个水平角，公式如下

把水平角按照每3度分一个区间，每个区间对应一个元素，这个元素是该区间内特征的数目，所有的这些元素组成了该帧对应的2d 直方图特征。

2. 闭环检测

特征介绍完了，我们看看怎么索引。

其实有了特征，索引就变得很简单了，根据特征比较两帧的相似性就行了，一个公式解决问题

这里H就是上面说的每一帧对应的元素，而

就是元素的索引，

是对应的平均值，所以这个公式就是通过比较两帧所有元素的相似性来比较两帧的相似性。相似性满足一定阈值，则认为是相似帧。

整个流程下来，感觉有点像NDT。

3. 闭环匹配

作者认为用它自己的loam livox做匹配就可以

4. 优化

匹配完优化，和其他slam一样，没啥可说的

三、一点思考

看完整个论文以后，感觉和slam的真实需求有一些矛盾，不知道是不是我理解有问题，写在这里抛砖引玉，供大家讨论：

闭环检测大多数是在开环误差比较大的情况下使用，因为如果开环误差比较小，我就直接比较历史帧的位姿，找离得近的位姿去匹配就行了。那既然这样，闭环帧的查找就不能只是找两帧相似性，而且应该给出两帧粗略的相对位姿关系，而本文只找相似性，并没有用2d 直方图给出位姿。