![9c405a7cf60846a3d612630b04bb037b.png](https://i-blog.csdnimg.cn/blog_migrate/040bf3a93bf28e6e7875316fcef89159.jpeg)
论文题目: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 Livoxzhuanlan.zhihu.com![7535a0969dacd1f1ddcee7a144a47fde.png](https://i-blog.csdnimg.cn/blog_migrate/86fc6e90bf0055002311d9c8b031c039.jpeg)
loam livox是讲怎样做一个基于固态雷达的里程计,而本论文讲的是怎样给这个里程计加闭环。这个论文里公式有点多,我按照我自己的理解在这里和大家讨论,如果有表述不对的地方,欢迎指正。
一、整体概述
既然是设计闭环,而且是基于特征的闭环,那么整个算法流程就可以按照以下流程走了:
1.特征提取和保存
2.闭环检测,也即基于特征做索引,找出相似关键帧
3.闭环匹配,即对相似的关键帧做帧间匹配,获得其相对位姿
4.优化,根据获得的相对位姿在概率图中添加对应的边,执行图优化,根据结果修正地图
二、算法介绍
下面我们就流程中的各个部分依次说呗
1. 计算2D直方图
这一步又分为以下几个小步骤:
1)把点云划分为cell
把空间位置按照xyz坐标,分成多个cell,按照几何位置,把点云中的点分到对应的cell中,每个cell根据cell中点的坐标计算均值和方差,公式如下:
![777520bfccc442271c8550f529fb2d06.png](https://i-blog.csdnimg.cn/blog_migrate/55fb8cca9b48860a5288e3eafd1df7ed.png)
2)对每个cell计算特征类型和方向
有了方差矩阵以后,我们对其进行特征值分解,按照特征值大小顺序,得到
![92cfe125de182654f1b4fa87fbb908b6.png](https://i-blog.csdnimg.cn/blog_migrate/43115ce285a544b3c48add696cb0b0d5.png)
而每个cell的特征类型就是根据特征值的情况来定的:
a. 如果
b. 如果不是面特征,且
c. 不是以上两种,则木有特征
3)计算旋转矩阵
这里的旋转矩阵并不是点云位姿对应的旋转矩阵,而是特征要给一个旋转矩阵,让特征按照这个矩阵旋转以后,能够使大多数线特征的方向,以及面特征边缘(面的边缘其实就是线)方向处于X轴上,它的目的是为了使特征具有旋转不变性。
具体公式不列出了,看论文吧
4)划分区域
一帧点云有了特征,要对这些特征进行存储,作者采用的做法是这样的,我们在3)中不是计算了一个旋转矩阵吗,对这个旋转它的两个水平角,公式如下
![cad9eea66f545d99df8f1d2beef68df0.png](https://i-blog.csdnimg.cn/blog_migrate/39daf2670d1b92b8145e38c4d7059729.png)
把水平角按照每3度分一个区间,每个区间对应一个元素,这个元素是该区间内特征的数目,所有的这些元素组成了该帧对应的2d 直方图特征。
2. 闭环检测
特征介绍完了,我们看看怎么索引。
其实有了特征,索引就变得很简单了,根据特征比较两帧的相似性就行了,一个公式解决问题
![0e64f634e70292af0e6acaeb890787dc.png](https://i-blog.csdnimg.cn/blog_migrate/2fd076e581a3a97e7fbbe3fbd30fc818.png)
这里H就是上面说的每一帧对应的元素,而
整个流程下来,感觉有点像NDT。
3. 闭环匹配
作者认为用它自己的loam livox做匹配就可以
4. 优化
匹配完优化,和其他slam一样,没啥可说的
三、一点思考
看完整个论文以后,感觉和slam的真实需求有一些矛盾,不知道是不是我理解有问题,写在这里抛砖引玉,供大家讨论:
闭环检测大多数是在开环误差比较大的情况下使用,因为如果开环误差比较小,我就直接比较历史帧的位姿,找离得近的位姿去匹配就行了。那既然这样,闭环帧的查找就不能只是找两帧相似性,而且应该给出两帧粗略的相对位姿关系,而本文只找相似性,并没有用2d 直方图给出位姿。