从零入门激光SLAM(十三)——LeGo-LOAM源码超详细解析2
- 一、整体输入输出
- 1.1 输入如下
- 1.2 功能如下
- 1.3 输出如下
- 二、Feature Extraction
- **…详情请参照** <https://www.guyuehome.com/46787>
一、整体输入输出
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系的算法均用角点和面点特征进行匹配。