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

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

一、整体输入输出

LEGO-LOAM的第二个文件是Feature association，主要功能是对分割后的点云进行特征点提取和匹配操作，估算出位姿，本节将介绍整体功能和怎么从杂乱的点云中提取特征。该文件的整体框架如下：

1.1 输入如下

// 订阅了分割之后的点云
subLaserCloud=nh.subscribe("/segmented_cloud", 1, &FeatureAssociation::laserCloudHandler, this);
// 订阅的分割点云含有图像的深度信息
subLaserCloudInfo=nh.subscribe("/segmented_cloud_info", 1, &FeatureAssociation::laserCloudInfoHandler, this);
// 非聚类的点
subOutlierCloud=nh.subscribe("/outlier_cloud", 1, &FeatureAssociation::outlierCloudHandler, this);
// IMU传感器的消息
subImu=nh.subscribe(imuTopic, 50, &FeatureAssociation::imuHandler, this);

1.2 功能如下

首先对无序点云进行特征提取，之后利用特征点进行匹配计算出位姿

1. Feature Extraction特征提取
// 1.1 点云运动补偿
adjustDistortion();
// 1.2 计算平滑度
calculateSmoothness();
// 1.3 标记遮挡点
markOccludedPoints();
// 1.4 提取特征
extractFeatures();
// 1.5 发布点云
publishCloud();
2. Feature Association特征匹配
if (!systemInitedLM) {
// 2.1 检查系统初始化，点云投影到结束点
checkSystemInitialization();
return;}
// 2.2 更新初始猜测位置
updateInitialGuess();
// 2.3 特征匹配，输出Transformation
updateTransformation();
// 2.4 融合IMU坐标Transformation
integrateTransformation();
// 2.5 发布雷达里程计
publishOdometry();
// 2.6 发布点云用于建图
publishCloudsLast();

1.3 输出如下

pubCornerPointsSharp = nh.advertise("/laser_cloud_sharp", 1); //当前帧角点
pubCornerPointsLessSharp = nh.advertise("/laser_cloud_less_sharp", 1); //当前帧较缓的角点
pubSurfPointsFlat = nh.advertise("/laser_cloud_flat", 1);  //当前帧面点
pubSurfPointsLessFlat = nh.advertise("/laser_cloud_less_flat", 1); //当前帧较缓的面点
pubAdjustPoints= nh.advertise("/adjust_pointcloud", 1);  //修正后的分割点云
pubLaserCloudCornerLast = nh.advertise("/laser_cloud_corner_last", 2); //前一帧所有角点
pubLaserCloudSurfLast = nh.advertise("/laser_cloud_surf_last", 2);  //前一帧所有面点
pubOutlierCloudLast = nh.advertise("/outlier_cloud_last", 2); //前一帧所有界外点pubLaserOdometry = nh.advertise (“/laser_odom_to_init”, 5); //激光里程计

二、Feature Extraction

提取特征能够避免使用全部点云进行计算和匹配，大大降低了内存资源，是3D激光SLAM能够实时建图的特新思想。所有LOAM系的算法均用角点和面点特征进行匹配。

…详情请参照 https://www.guyuehome.com/46787 、 https://www.guyuehome.com/46790