LEGO LOAM 学习理解总结
2024-05-18 01:21:03
参考资料
- LOAM等内容的博客 需要好好研读
- LeGO-LOAM 源码阅读笔记(imageProjecion.cpp)
- LeGO-LOAM 源码阅读笔记(featureAssociation.cpp)
- LeGO-LOAM 源码阅读笔记（mapOptmization.cpp）
代码理解（ongoing）
- image Projection 的原因之一是， 必须找到所有激光点对应的位置。需要将所有激光点投射到16个channel中去，并且每个channel中的激光点总数应该保持一致。激光雷达得到的数据看成一个16x1800的点云阵列
PCL_ADD_POINT4D
145 union EIGEN_ALIGN16 { \146 float data[4]; \147 struct { \148 float x; \149 float y; \150 float z; \151 }; \152 }; \144 #define PCL_ADD_POINT4D \145 union EIGEN_ALIGN16 { \146 float data[4]; \147 struct { \148 float x; \149 float y; \150 float z; \151 }; \152 }; \194 #define PCL_ADD_INTENSITY \195 struct \196 { \197 float intensity; \198 }; \pcl::PointCloud::Ptr reset
pcl::PointCloud<PointType>::Ptr fullCloud;
laserCloudIn.reset(new pcl::PointCloud<PointType>());
ImageProjection cloudHandler
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){// 1. Convert ros message to pcl point cloudcopyPointCloud(laserCloudMsg);// 2. Start and end angle of a scan// 根据节点第一个点和最后一个殿的 x,y,z 推测雷达角度差异findStartEndAngle();// 3. Range image projection// 计算竖直角度，并投射到16个channel中projectPointCloud();// 4. Mark ground points// 由上下两线之间点的XYZ位置得到两线之间的俯仰角，如果俯仰角在10度以内，则判定(i,j)为地面点,// groundRemoval();// 5. Point cloud segmentation// 将不同类型的点云放到不同的点云块中去，例如outlierCloud，segmentedCloudPure等cloudSegmentation();// 6. Publish all cloudspublishCloud();// 7. Reset parameters for next iterationresetParameters();}
可以借鉴的 Publish结构
void publishCloud(){// 1. Publish Seg Cloud InfosegMsg.header = cloudHeader;pubSegmentedCloudInfo.publish(segMsg);// 2. Publish cloudssensor_msgs::PointCloud2 laserCloudTemp;pcl::toROSMsg(*outlierCloud, laserCloudTemp);laserCloudTemp.header.stamp = cloudHeader.stamp;laserCloudTemp.header.frame_id = "base_link";pubOutlierCloud.publish(laserCloudTemp);// segmented cloud with groundpcl::toROSMsg(*segmentedCloud, laserCloudTemp);laserCloudTemp.header.stamp = cloudHeader.stamp;laserCloudTemp.header.frame_id = "base_link";pubSegmentedCloud.publish(laserCloudTemp);// projected full cloudif (pubFullCloud.getNumSubscribers() != 0){pcl::toROSMsg(*fullCloud, laserCloudTemp);laserCloudTemp.header.stamp = cloudHeader.stamp;laserCloudTemp.header.frame_id = "base_link";pubFullCloud.publish(laserCloudTemp);}// original dense ground cloudif (pubGroundCloud.getNumSubscribers() != 0){pcl::toROSMsg(*groundCloud, laserCloudTemp);laserCloudTemp.header.stamp = cloudHeader.stamp;laserCloudTemp.header.frame_id = "base_link";pubGroundCloud.publish(laserCloudTemp);}// segmented cloud without groundif (pubSegmentedCloudPure.getNumSubscribers() != 0){pcl::toROSMsg(*segmentedCloudPure, laserCloudTemp);laserCloudTemp.header.stamp = cloudHeader.stamp;laserCloudTemp.header.frame_id = "base_link";pubSegmentedCloudPure.publish(laserCloudTemp);}// projected full cloud infoif (pubFullInfoCloud.getNumSubscribers() != 0){pcl::toROSMsg(*fullInfoCloud, laserCloudTemp);laserCloudTemp.header.stamp = cloudHeader.stamp;laserCloudTemp.header.frame_id = "base_link";pubFullInfoCloud.publish(laserCloudTemp);}}
featureAssociation.cpp
- 它分为特征点的提取与匹配两部分
void runFeatureAssociation(){//判断分割点云和异常值的实时性if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05){newSegmentedCloud = false;newSegmentedCloudInfo = false;newOutlierCloud = false;}else{return;}//进行形状特征的调整adjustDistortion();calculateSmoothness();markOccludedPoints();//提取特征extractFeatures();publishCloud();//在配准之前，检验LM法是否初始化，接下来就是里程计部分if (!systemInitedLM){checkSystemInitialization();return;}//提供粗配准的先验以供优化updateInitialGuess();//优化并发送里程计信息updateTransformation();integrateTransformation();publishOdometry();//发送点云以供建图使用publishCloudsLast();}
void adjustDistortion()
将点云数据进行坐标变换，进行插补等工作
- 莫名其妙的坐标轴变换
point.x = segmentedCloud->points[i].y;
point.y = segmentedCloud->points[i].z;
point.z = segmentedCloud->points[i].x;
void calculateSmoothness()
用于计算光滑性
void calculateSmoothness()
{int cloudSize = segmentedCloud->points.size();for (int i = 5; i < cloudSize - 5; i++) {float diffRange = segInfo.segmentedCloudRange[i-5] + segInfo.segmentedCloudRange[i-4]+ segInfo.segmentedCloudRange[i-3] + segInfo.segmentedCloudRange[i-2]+ segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i] * 10+ segInfo.segmentedCloudRange[i+1] + segInfo.segmentedCloudRange[i+2]+ segInfo.segmentedCloudRange[i+3] + segInfo.segmentedCloudRange[i+4]+ segInfo.segmentedCloudRange[i+5]; cloudCurvature[i] = diffRange*diffRange;// 在markOccludedPoints()函数中对该参数进行重新修改cloudNeighborPicked[i] = 0;// 在extractFeatures()函数中会对标签进行修改，// 初始化为0，surfPointsFlat标记为-1，surfPointsLessFlatScan为不大于0的标签// cornerPointsSharp标记为2，cornerPointsLessSharp标记为1cloudLabel[i] = 0;cloudSmoothness[i].value = cloudCurvature[i];cloudSmoothness[i].ind = i;}
}
void extractFeatures()
进行特征抽取，然后分别保存到cornerPointsSharp等等队列中去。 保存到不同的队列是不同类型的点云，进行了标记的工作，这一步中减少了点云数量，使计算量减少。 函数首先清空了cornerPointsSharp,cornerPointsLessSharp,surfPointsFlat,surfPointsLessFlat 然后对cloudSmoothness队列中sp到ep之间的点的平滑数据进行从小到大的排列。 然后按照不同的要求，将点的索引放到不同的队列中去。 另外还对点进行了标记。 最后，因为点云太多时，计算量过大，因此需要对点云进行下采样，减少计算量。 代码如下：
void extractFeatures()
{cornerPointsSharp->clear();cornerPointsLessSharp->clear();surfPointsFlat->clear();surfPointsLessFlat->clear();for (int i = 0; i < N_SCAN; i++) {surfPointsLessFlatScan->clear();for (int j = 0; j < 6; j++) {// sp和ep的含义是什么???startPointer,endPointer?int sp = (segInfo.startRingIndex[i] * (6 - j) + segInfo.endRingIndex[i] * j) / 6;int ep = (segInfo.startRingIndex[i] * (5 - j) + segInfo.endRingIndex[i] * (j + 1)) / 6 - 1;if (sp >= ep)continue;// 按照cloudSmoothness.value从小到大排序std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());int largestPickedNum = 0;for (int k = ep; k >= sp; k--) {// 每次ind的值就是等于k??? 有什么意义?// 因为上面对cloudSmoothness进行了一次从小到大排序，所以ind不一定等于k了int ind = cloudSmoothness[k].ind;if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] > edgeThreshold &&segInfo.segmentedCloudGroundFlag[ind] == false) {largestPickedNum++;if (largestPickedNum <= 2) {// 论文中nFe=2,cloudSmoothness已经按照从小到大的顺序排列，// 所以这边只要选择最后两个放进队列即可// cornerPointsSharp标记为2cloudLabel[ind] = 2;cornerPointsSharp->push_back(segmentedCloud->points[ind]);cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);} else if (largestPickedNum <= 20) {// 塞20个点到cornerPointsLessSharp中去// cornerPointsLessSharp标记为1cloudLabel[ind] = 1;cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);} else {break;}cloudNeighborPicked[ind] = 1;for (int l = 1; l <= 5; l++) {// 从ind+l开始后面5个点，每个点index之间的差值，// 确保columnDiff<=10,然后标记为我们需要的点int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--) {// 从ind+l开始前面五个点，计算差值然后标记int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}}}int smallestPickedNum = 0;for (int k = sp; k <= ep; k++) {int ind = cloudSmoothness[k].ind;// 平面点只从地面点中进行选择???为什么要这样做???if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] < surfThreshold &&segInfo.segmentedCloudGroundFlag[ind] == true) {cloudLabel[ind] = -1;surfPointsFlat->push_back(segmentedCloud->points[ind]);// 论文中nFp=4，将4个最平的平面点放入队列中smallestPickedNum++;if (smallestPickedNum >= 4) {break;}cloudNeighborPicked[ind] = 1;for (int l = 1; l <= 5; l++) {// 从前面往后判断是否是需要的邻接点，是的话就进行标记int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--) {// 从后往前开始标记int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}}}for (int k = sp; k <= ep; k++) {if (cloudLabel[k] <= 0) {surfPointsLessFlatScan->push_back(segmentedCloud->points[k]);}}}// surfPointsLessFlatScan中有过多的点云，如果点云太多，计算量太大// 进行下采样，可以大大减少计算量surfPointsLessFlatScanDS->clear();downSizeFilter.setInputCloud(surfPointsLessFlatScan);downSizeFilter.filter(*surfPointsLessFlatScanDS);*surfPointsLessFlat += *surfPointsLessFlatScanDS;}
}
void updateTransformation()
中主要是两个部分，一个是找特征平面，通过面之间的对应关系计算出变换矩阵。 另一个部分是通过角、边特征的匹配，计算变换矩阵。 该函数主要由其他四个部分组成：findCorrespondingSurfFeatures,calculateTransformationSurf findCorrespondingCornerFeatures, calculateTransformationCorner 这四个函数分别是对应于寻找对应面、通过面对应计算变换矩阵、寻找对应角/边特征、通过角/边特征计算变换矩阵。
void updateTransformation(){if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)return;for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {laserCloudOri->clear();coeffSel->clear();// 找到对应的特征平面// 然后计算协方差矩阵，保存在coeffSel队列中// laserCloudOri中保存的是对应于coeffSel的未转换到开始时刻的原始点云数据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;}
}
void integrateTransformation()
计算了旋转角的累积变化量。 这个函数首先通过AccumulateRotation()将局部旋转左边切换至全局旋转坐标。 然后同坐变换转移到世界坐标系下。 再通过PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);插入imu旋转，更新姿态。
mapOptmization.cpp
mapOptmization.cpp进行的内容主要是地图优化，将得到的局部地图信息融合到全局地图中去。
publishGlobalMap()
- publishGlobalMap()主要进行了3个步骤：
- 通过KDTree进行最近邻搜索;
- 通过搜索得到的索引放进队列;
- 通过两次下采样，减小数据量;
通过储存的 cloudKeyPoses3D 关键点的位置， 找到邻近的关键帧，默认显示500m范围内的关键位置。
void publishGlobalMap(){if (pubLaserCloudSurround.getNumSubscribers() == 0)return;if (cloudKeyPoses3D->points.empty() == true)return;std::vector<int> pointSearchIndGlobalMap;std::vector<float> pointSearchSqDisGlobalMap;mtx.lock();kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D);// 通过KDTree进行最近邻搜索kdtreeGlobalMap->radiusSearch(currentRobotPosPoint, globalMapVisualizationSearchRadius, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0);mtx.unlock();for (int i = 0; i < pointSearchIndGlobalMap.size(); ++i)globalMapKeyPoses->points.push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);// 对globalMapKeyPoses进行下采样downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);for (int i = 0; i < globalMapKeyPosesDS->points.size(); ++i){int thisKeyInd = (int)globalMapKeyPosesDS->points[i].intensity;*globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],&cloudKeyPoses6D->points[thisKeyInd]);*globalMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd],&cloudKeyPoses6D->points[thisKeyInd]);*globalMapKeyFrames += *transformPointCloud(outlierCloudKeyFrames[thisKeyInd],&cloudKeyPoses6D->points[thisKeyInd]);}// 对globalMapKeyFrames进行下采样downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames);downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS);sensor_msgs::PointCloud2 cloudMsgTemp;pcl::toROSMsg(*globalMapKeyFramesDS, cloudMsgTemp);cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);cloudMsgTemp.header.frame_id = "/camera_init";pubLaserCloudSurround.publish(cloudMsgTemp); globalMapKeyPoses->clear();globalMapKeyPosesDS->clear();globalMapKeyFrames->clear();globalMapKeyFramesDS->clear();
}
void run()
void run(){if (newLaserCloudCornerLast && std::abs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&newLaserCloudSurfLast && std::abs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&newLaserCloudOutlierLast && std::abs(timeLaserCloudOutlierLast - timeLaserOdometry) < 0.005 &&newLaserOdometry){newLaserCloudCornerLast = false; newLaserCloudSurfLast = false; newLaserCloudOutlierLast = false; newLaserOdometry = false;std::lock_guard<std::mutex> lock(mtx);if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {timeLastProcessing = timeLaserOdometry;transformAssociateToMap();extractSurroundingKeyFrames();downsampleCurrentScan();scan2MapOptimization();saveKeyFramesAndFactor();correctPoses();publishTF();publishKeyPosesAndFrames();clearCloud();}}
}
参考资料
- LOAM等内容的博客 需要好好研读
- LeGO-LOAM 源码阅读笔记(imageProjecion.cpp)
- LeGO-LOAM 源码阅读笔记(featureAssociation.cpp)
- LeGO-LOAM 源码阅读笔记（mapOptmization.cpp）
LEGO LOAM 学习理解总结相关推荐
- LeGO LOAM学习
LOAM LOAM是一套非常有价值的LIDAR ODOMOTRY算法(它是一个历程计算法,没有回环检测和全局优化的部分). LEGO LOAM LeGO LOAM 它含有四个主要线程 image pr ...
- LOAM学习-代码解析（三）特征点运动估计 laserOdometry
LOAM学习-代码解析(三)特征点运动估计 laserOdometry 前言 一.初始化 二.去除位移畸变 TransformToStart TransformToEnd 三.去除角度畸变 Plugi ...
- Docker镜像原理学习理解
Docker镜像原理学习理解 一.Docker镜像的组成 1.Docker镜像图层 2.union file system 3.镜像层-bootfs 4.镜像层-rootfs 5.镜像层-依赖环境 6 ...
- lego loam 安装过程与问题处理
lego loam安装与问题处理: https://blog.csdn.net/weixin_44156680/article/details/118070387 ubuntu 20 安装lego l ...
- LTE学习理解系列——TDD LTE信源指定时隙配比设置
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档 LTE学习理解系列--TDD LTE信源指定时隙配比设置 前言 详细配置 结语 前言 目前暂时关注时隙配比的设置,因为每一种时隙配比可 ...
- python语言通俗理解_慢步学习，python语言编程，来扯扯语言的学习理解
最近慢步工作比较忙,有那么一丢丢挫伤了学习的积极性.积极性受挫的另一个原因是,慢步对自己给读者提供的内容有些困惑,"我能提供什么有价值的内容?" 不断重复书籍的知识点,好像并不能生 ...
- 5G NR学习理解系列——时频结构及相关概念
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档 5G NR学习理解系列--时频结构 频段 工作带宽 信道带宽 子载波间隔(SCS) SLOT长度 OFDM符号时长 采样点时长 无线帧 ...
- 5G NR学习理解系列——MATLAB5G信源的生成之SSB参数配置
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档 5G NR学习理解系列--MATLAB5G信源的生成之SSB参数配置 前言 SSB参数的位置 SSB参数详细解释 信号时频图 总结 前 ...
- 5G NR学习理解系列——NR小区搜索的matlab仿真
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档 5G NR学习理解系列--NR小区搜索的matlab仿真 前言 信源生成 降采样 PSS和SSS本地序列生成 相关结果 前言 既然已经 ...
最新文章
- (转)搭建企业内部yum仓库(centos6+centos7+epel源)
- 最强黑客库Blackbone使用教程
- 向silverlight传递自定义参数
- Java jar 修改（springfox-swagger-ui-2.9.2.jar 修改去掉顶部的绿色topbar，汉化）
- uniapp图片自适应_uni-app下input组件基于内容自适应宽度的实现
- Spring : Spring的ApplicationContext接口
- vue导入音乐_现在哪个软件听音乐完全免费？
- matlab引言,MATLAB论文
- Spring IoC-02
- 544. Top k Largest Numbers【medium】
- IOS文件操作（NSFileManager）
- Hyperf JSON RPC 服务 Demo
- USB闪存盘变成FDD（软盘驱动器）了？
- Win7原版镜像注入USB驱动
- android 合并分区说明,Android系统手机sd卡分区后合并图文详解
- 微信小程序：高德地图在小程序中的实践（含静态地图）
- 如何安装计算机刻录程序,详解怎么用电脑刻录光盘
- 页面访问控制的3种方法
- git与gerrit基础概念
- 多个containers 共用一个pvc_室外成品pvc