大家好呀，我是一个SLAM方向的在读博士，深知SLAM学习过程一路走来的坎坷，也十分感谢各位大佬的优质文章和源码。随着知识的越来越多，越来越细，我准备整理一个自己的激光SLAM学习笔记专栏，从0带大家快速上手激光SLAM，也方便想入门SLAM的同学和小白学习参考，相信看完会有一定的收获。如有不对的地方欢迎指出，欢迎各位大佬交流讨论，一起进步。博主创建了一个科研互助群Q：951026257，欢迎大家加入讨论。
Lego_Loam包括了Image projection、Feature association、MapOptmization、Transform Fusion四个部分，下面博主将按照算法的逻辑顺序对代码中的重要函数进行讲解。本节是解析Feature association文件中的特征匹配部分。
该专栏的其他章节链接如下
从零入门激光SLAM系列——总目录与更新情况_kiss icp-CSDN博客
一、Feature Extraction
经过上一节的点云特征提取，我们已经获得了稳健的角点和面点如下图所示，下面将介绍如何配准得到位姿Feature Association部分的源码解析
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 特征匹配，输出TransformationupdateTransformation();
// 2.4 融合IMU坐标TransformationintegrateTransformation();
二、函数解析
2.1 publishCloudsLast
- 作用：将当前帧点云投影到结束点并发布，作为下一帧匹配的对象，（匹配之前需要有一个匹配对象点云）
- 输入：
cornerPointsSharp角特征 cornerPointsLessSharp缓角特征 surfPointsFlat面特征 surfPointsLessFlat缓面特征 adjustCloud修正点云 - 输出：
/laser_cloud_corner_last/laser_cloud_surf_last/outlier_cloud_last/laser_odom_to_init - 代码：
下图中红色为前一帧，绿色为当前帧void publishCloudsLast(){ //把特征点云投影到每帧的结尾时刻 updateImuRollPitchYawStartSinCos(); int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size(); for (int i = 0; i < cornerPointsLessSharpNum; i++) {TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]); }int surfPointsLessFlatNum = surfPointsLessFlat->points.size(); for (int i = 0; i < surfPointsLessFlatNum; i++) {TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);} //用KD树存储当前帧点云，为后续匹配做准备 pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp; cornerPointsLessSharp = laserCloudCornerLast; laserCloudCornerLast = laserCloudTemp; laserCloudTemp = surfPointsLessFlat; surfPointsLessFlat = laserCloudSurfLast; laserCloudSurfLast = laserCloudTemp; laserCloudCornerLastNum = laserCloudCornerLast->points.size(); laserCloudSurfLastNum = laserCloudSurfLast->points.size(); if (frameCount >= skipFrameNum + 1) {frameCount = 0; //发布上一帧outlier pubOutlierCloudLast.publish(outlierCloudLast2); //发布上一帧线特征 pubLaserCloudCornerLast.publish(laserCloudCornerLast2); //发布上一帧面特征 pubLaserCloudSurfLast.publish(laserCloudSurfLast2);}
2.2 updateInitialGuess
- 作用：把当前IMU数值作为位姿先验
- 输入：IMU数值
- 输出：transformCur位姿矩阵
// 如果IMU有旋转，则更新变换矩阵以反映新的旋 void updateInitialGuess(){ if (imuAngularFromStartX != 0 || imuAngularFromStartY != 0 || imuAngularFromStartZ != 0){transformCur[0] = - imuAngularFromStartY; // 根据Y轴旋转更新变换矩阵的第一个元素 transformCur[1] = - imuAngularFromStartZ; // 根据Z轴旋转更新变换矩阵的第二个元素 transformCur[2] = - imuAngularFromStartX; // 根据X轴旋转更新变换矩阵的第三个元素} // 如果IMU有移动，则更新变换矩阵以反映新的平移 if (imuVeloFromStartX != 0 || imuVeloFromStartY != 0 || imuVeloFromStartZ != 0) {transformCur[3] -= imuVeloFromStartX * scanPeriod; // 根据X轴移动更新变换矩阵的第四个元素 transformCur[4] -= imuVeloFromStartY * scanPeriod; // 根据Y轴移动更新变换矩阵的第五个元素 transformCur[5] -= imuVeloFromStartZ * scanPeriod; // 根据Z轴移动更新变换矩阵的第六个元素 } }2.3 updateTransformation
- 作用：匹配角和面特征点，计算位姿
- 输入：
laserCloudSurfLastsurfPointsFlatlaserCloudCornerLastcornerPointsSharp - 输出：transformCur位姿矩阵
-
代码
void updateTransformation(){//检查特征点if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)return; // 面特征匹配for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {laserCloudOri->clear(); coeffSel->clear(); findCorrespondingSurfFeatures(iterCount1); if (laserCloudOri->points.size() < 10)continue;// 通过面特征的匹配，计算变换矩阵if (calculateTransformationSurf(iterCount1) == false)break;} // 线特征匹配for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {laserCloudOri->clear();coeffSel->clear();findCorrespondingCornerFeatures(iterCount2); if (laserCloudOri->points.size() < 10)continue;if (calculateTransformationCorner(iterCount2) == false) break;}}
详情请见...
从零入门激光SLAM（十三）——LeGo-LOAM源码超详细解析3 - 古月居 (guyuehome.com)
