LIO-SAM mapOptimization.cpp 函数的核心注释

1.第一个巨大的回调函数:lasercloudinfoHandle:

这个函数监听的是/feature/cloud_info,关于这个话题里面包含的内容,我已经在featureExtraction.cpp的总结部分说过了。这里回忆一下,cloud_info是作者自定义的一个特殊的msg类型,包含了当前激光帧的角点集合,平面点集合,一维数组,每根线开始的点和结束点在一维数组中的索引……

那么收到数据,先保存时间戳,然后提取已经在featureExtraction.cpp中被提取的角点和平面点信息,保存到*laserCloudCornerLast和*laserCloudSurfLast中。请记住这两个命名。

频率控制:当前时刻-上一时刻>=0.15s时,才开始下列循环:

1.1  UpdateInitGuess:当前帧初始化。

1.1.1 当关键帧集合为空时,用激光帧的imu原始数据角度数据初始化,并且把数据用lastImuTransformation保存(记住这个命名),返回。

此时推荐回顾一下imageProjection.cpp部分的总结3.2

1.1.2 假如cloudInfo.odomAvailable=true时,那么就用一个transBack来记录cloudInfo.initialGuessX等信息,(这个信息其实来自于imupreintegration.cpp中发布的imu里程计数据),然后记录增量,在之前系统状态transformTobeMapped的基础上,加上这个增量,再次存入transformTobeMapped。 注意这个transformTobeMapped,这个数据结构,在这个cpp里,我们可以理解为就是它在存储着激光里程计部分的系统状态,包括位置与姿态。

注意,这里有一个lastImuPreTransformation,这个用来保存上一时刻imu里程计的数据,根据它和当前的imu里程计来算增量。不要和lastImuTransformation变量混起来,虽然这俩变量名字长的很像。

然后覆盖lastImuTransformation(在这个case里没用到),返回;

1.1.3 假如cloudInfo.imuAvailable=true,那么进入这个case:

注意,lastImuTransformation在1.1.1和1.1.2中并未用到,只是不断的在替换成最新数据。当cloudInfo.odomAvailable一直是true的时候,程序压根也不会进入到这个case。

但是,凡事总有例外,万一哪里没有衔接好,imu里程计没有及时算出来,那么就导致此时激光帧的位姿没有一个初始化的数据(我们之后是要在初始化的基础上进行优化的),那么之后的优化就无从进行。因此,就要用到这个case。

这里主要思路是用imuRollInit数据来初始化,如果你回顾过imageProjection.cpp部分的总结3.2 那么你应该就会懂,这里的数据来源是原始imu数据的角度信息。那么如果这里有数据,就用lastImuTransformation当成最新的上一时刻数据,当前数据transBack和它算一个增量,然后累积到系统值transformTobeMapped上面去。最后更新覆盖lastImuTransformation,返回。

 1.2 extractSurroundingKeyFrames:这个是比较复杂的一个函数,以下的内容希望读者可以心平气和的,每个字都依次念一遍。

如果没有关键帧,那就算了,返回;

如果有,就调用extractNearby函数。

关键帧是啥?cloudKeyPoses3D,我们要记住这个变量,虽然到现在为止,我们还不知道它是怎么来的,但是这个东西是怎么获取的,我们在后续必须弄明白。

在这里我先剧透一下:它里面装的是历史的“关键帧”的位置,即x,y,z三个值。需要明确:这里装的绝不是历史的关键帧位置处的点云。而是历史关键帧记录时刻机器人所在的位置。

同理还有一个cloudKeyPoses6D,它比这个3D还多了三个角度信息。之所以要用一个6D一个3D分别来装关键帧,我现在直接揭晓答案:用3D装,是因为我们要根据这个来构建KD树,搜索历史上最近的节点。“最近”指的是距离上最近,即xyz空间坐标最近,和角度无关。而cloudKeyPoses6D,是用来投影点云的,把当前帧点云投影到世界坐标系下,那么投影就必须要用角度信息了,所以作者分别用了一个3D和一个6D来装数据。

Kd树的原理我这里不写,随便放一个链接:机器学习——详解KD-Tree原理 - 知乎 ,实际上代码里也只是调库,所以这里我不写。

那么接下来,我介绍extractNearby函数:

1.2.1 使用kd树,搜索当前

### LIO-SAM 中 `imageProjection.cpp` 文件使用 KD-Tree 的实现细节 在 LIO-SAM 项目中,`imageProjection.cpp` 主要负责处理激光雷达帧点云数据并将其投影到图像平面上。虽然该文件本身并不直接涉及 KD-Tree 的构建或查询操作,但是通过与其他模块交互间接利用了基于 KD-Tree 的优化。 #### 数据预处理阶段 当程序运行时,来自传感器的数据被传递至 `imageProjection.cpp` 进行初步处理。此过程中会获取由 IMU 预积分计算得到的姿态信息以及 LiDAR 扫描返回的空间坐标系下的三维点集[^2]。 ```cpp // 假设这是从IMU预积分类获得的位姿变换矩阵 Eigen::Matrix4f imuPose; ``` #### 构建局部地图索引结构 为了加速后续最近邻查找过程,在某些版本或者改进版的 LIO-SAM 实现中可能会引入 KD-Tree 来作为辅助工具帮助快速定位周围环境特征点的位置关系。尽管官方源码可能并未显式展示这部分逻辑,但在实际应用开发场景下可以考虑如下方式来增强性能: 1. **创建 KD-Tree 对象** 利用第三方库(如 FLANN 或者 PCL 提供的功能),可以在初始化阶段建立一个用于存储已知地标位置的 KD-Tree 结构实例。 ```cpp // 创建KD树对象,假设已经安装PCL库 pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; ``` 2. **更新 KD-Tree 内容** 每次接收到新的观测数据之后都需要及时同步更新 KD-Tree 内部节点分布情况以反映最新状态变化。 ```cpp std::vector<int> pointIdxNKNSearch(1); std::vector<float> pointNKNSquaredDistance(1); // 将当前扫描周期内的有效点加入KD树 for (const auto& pt : currentScanPoints){ cloud->points.push_back(pt); } kdtree.setInputCloud(cloud); ``` 3. **执行 KNN 查询** 当需要评估某个特定区域是否存在障碍物或者其他显著物体时,则可以通过调用 KD-Tree 上的相关接口高效完成近似匹配任务。 ```cpp int K = 1; // 查找最接近的一个邻居 if(kdtree.nearestKSearch(queryPoint,K,pointIdxNKNSearch,pointNKNSquaredDistance)>0){ // 成功找到至少一个临近点... } ``` 需要注意的是上述伪代码片段仅展示了如何在一个理想化的框架内集成 KD-Tree 技术,并不代表原始 LIO-SAM 项目的具体编码风格和技术选型。对于更深入的理解应当查阅具体的文献资料或是研究社区贡献者的扩展实现案例[^3]。
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值