从零入门激光SLAM（十一）——LeGo-LOAM源码超详细解析1

大家好呀，我是一个SLAM方向的在读博士，深知SLAM学习过程一路走来的坎坷，也十分感谢各位大佬的优质文章和源码。随着知识的越来越多，越来越细，我准备整理一个自己的激光SLAM学习笔记专栏，从0带大家快速上手激光SLAM，也方便想入门SLAM的同学和小白学习参考，相信看完会有一定的收获。如有不对的地方欢迎指出，欢迎各位大佬交流讨论，一起进步。博主创建了一个科研互助群Q：772356582，欢迎大家加入讨论。

博主终于恢复更新了。。。今天 一起来解读一下LEGO-LOAM源码。

一、概述

首先LEGO-LOAM是在LOAM基础上基于地面优化，并且加入了简易的闭环检测，而LOAM是实时三维激光SLAM的开山鼻祖，使得实时三维重建成为可能，同时基于特征的激光SLAM基本上都是LOAM的衍生算法，思想大同小异。

LEGO-LOAM在LOAM的基础上效果有巨大提升，同时代码可读性较高，所以将它作为入门SLAM的第一个算法非常合适。LEGO-LOAM系统框架如下所示其核心思想是将同时定位和映射的复杂问题划分为两个算法，该问题旨在同时优化大量变量。高频低保真的执行里程计。低频高保真执行地图优化。代码的运行调试见下述博客。从零入门激光SLAM（五）——手把手带你编译运行Lego_loam_桦树无泪的博客-CSDN博客

简单来讲，系统分为前端与后端。

前端主要是匹配点云估计出粗位姿，匹配分为两步一个是scan2scan，一个是map2map，其中map是由10个匹配好的scan组成的。

后端是利用图优化和闭环检测优化粗位姿得到精位姿。

源码作者也分的比较清晰，包括了Image projection、Feature association、MapOptmization、Transform Fusion四个部分，下面博主将按照算法的逻辑顺序对代码中的重要函数进行讲解。

二、Image projection

2.1 Image projection输入输出

• 功能：Image projection作为系统的第一部分，主要功能是对点云进行预处理、图像投影与分割操作。
• 输入：xyzi类型的点云

•  输出：分割成各类点云

下面是对每个输出点云进行一定的解释和可视化，大家先对每种点云有个印象即可，具体这些点云是怎么被分割出来的，后面会详细讲。

2.2 算法逻辑

这部分的算法逻辑在函数cloudHandler写的很清晰

1、将ROS消息转化为PCL

2、判断一帧点云的开始和终止角度

3、对点云进行投影

4、去除地面点

5、对点云进行分割

6、发布点云

7、重载参数

1 CopyPointCloud 函数

• 作用    1.接收ROS点云消息转化pcl   2.过滤NAN点云
• 输入    原点云消息
• 源码

• 输出    将过滤NAN后的点云保存到laserCloudIn

2 findStartEndAngle函数

• 作用    找到点云的起始终止角度，可认为把一圆柱找到一个角度切开展开为四边形，方便后续处理。
• 输入    过滤后的 laserCloudIn点云
• 源码

• 输出   segMsg消息，包含了起止和终止角

3 projectPointCloud函数

• 作用   1. 通过垂直角度判断点在哪一行

            2.通过水平角度判断点在哪一列
            3.计算距离过滤较近的点
            4.保存到rangeMat二维矩阵
            5.保存投影点云和深度信息到fullInfoCloud

• 输入    过滤后的 laserCloudIn点云
• 源码

•  输出   fullCloud 具有线束信息点云，如下图所示，一种颜色代表一个线束    

       fullInfoCloud 具有深度信息点云，距离中心越近，颜色越红

4 groundRemoval函数

• 作用   分割出地面点，标记保存地面点
• 输入   有线束信息的fullCloud点云
• 源码

•   输出 groundCloud 地面点云

            groundMat    地面点云的序号    labelMat 标签点云的序号

5 cloudSegmentation函数

• 作用   1.对非地面点进行聚类   2.提取高质量分割点云   3.分割点云保存
• 输入   有线束信息的fullCloud点云
• 源码

•    输出 segMsg  点云消息

        segmentedCloud   聚类点云

        segmentedCloudPure   没有地面的聚类点云 

         outlierCloud   界外点

6 publishcloud函数

• 作用   1.发布所有分割点云
• 输入  segMsg  outlierCloud  segmentedCloud  fullCloud  groundCloud                segmentedCloudPure   fullInfoCloud  
• 源码

• 输出

/full_cloud_projected     投影点云

/full_cloud_info    带有距离信息的点云

/ground_cloud    地面点云

/segmented_cloud    已经分割的点云

/segmented_cloud_pure    没有地面点的分割点云

/segmented_cloud_info   自定义分割点云消息

/outlier_cloud    离散点云

2.3 Image projection总结

经过一顿操作猛如虎后，杂乱无序的点云被分割成简洁有序的，这样有利于后续的计算。

下一节将讲解LeGo-LOAM中的Feature association。如果内容对你有用的话，请订阅本专栏吧，免费哦~