LeGo-LOAM源码篇:源码分析(3)

Lego_Loam--源码分析 0 整体框架分析 翻看 LEGO-Loam 的代码目录,首先进入到launch 文件中,看到: <!--- LeGO-LOAM --> <node pkg="lego_loam" type="imageProjection" name="imageProjection" output="screen"/> <node pkg="lego_loam" type="featureAssociation" name="featureAssoci 阅读详情

src/mapOptmization.cpp
整体功能分为回环检测、可视化以及位姿全局优化,核心是位姿优化。主体流程:订阅特征提取后的点云、里程计数据->计算当前姿态优化的初始值->提取局部关键帧->降采样->scan-to-map地图优化(线特征、面特征、L-M)->保存关键帧与因子图优化->闭环检测->发布TF变换、点云

int main(int argc, char** argv)
{
   
   
    ros::init(argc, argv, "lego_loam");
    ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");
    mapOptimization MO;//订阅点云、里程计信息
    
    std::thread loopthread(&mapOptimization::loopClosureThread, &MO);//闭环检测线程
    std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);//可视化线程

    ros::Rate rate(200);
    while (ros::ok())
    {
   
   
        ros::spinOnce();
        MO.run();//全局位姿优化
        rate.sleep();
    }
    
    loopthread.join();
    visualizeMapThread.join();
    
    return 0;
}

...
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);
        //设置优化的间隔为0.3秒
        if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
   
   

            timeLastProcessing = timeLaserOdometry;
            //将点云转换到世界坐标系
            //根据当前(transformSum)与上次全局优化时的里程计(transformBefMapped)、上次全局优化的结果(transformAftMapped),计算当前姿态优化(transformTobeMapped)的初始值
            transformAssociateToMap();                
            //根据当前位置,提取局部关键帧集合以及对应的点云集合
            extractSurroundingKeyFrames();
            //降采样
            downsampleCurrentScan();
            //scan-to-map配准优化,获得transformTobeMapped,参考IMU姿态获得最终位姿 transformAftMapped
            scan2MapOptimization();
            //保存关键帧与因子图优化
            saveKeyFramesAndFactor();
            //闭环检测
            correctPoses()
LeGo-LOAM 源码解析 文章目录0、整体框架1、imageProjection —— 点云分割0. main()1. cloudHandler()2. copyPointCloud()3. findStartEndAngle()4. projectPointCloud()5. groundRemoval()6. cloudSegmentation()7. labelComponents()8. publishCloud()9. resetParameters()2、featureAssociation —— 特征提取与匹配0. m 阅读详情

相关推荐

LeGO-LOAM代码解析---基础知识

在学习LeGO-LOAM之前,先介绍一下基础知识: 位姿是旋转和平移的统称。在三维世界中,物体的位姿变换既要考虑物体的位置的变化,也要考虑姿态的变化。 举个例子,你在宿舍里,以宿舍某个点取个固定坐标系,例如东北天坐标系。现在你要去食堂吃饭,无容置疑在这个坐标系中你的位置肯定发生变化,此外还有一个“东东”也在不断发生变化,也就是你脸的朝向(PS:假设是刚体变换)。假设在起点,你脸的朝向正东面,左手在北面,右手在南面,在行进过程中,显然你自身的姿态肯定不断发生变化。 我

weixin_42068054的博客 2164

LIO-sam:安装 - 运行 - 论文介绍 - 源码阅读

一. 相关背景介绍 二. 安装和运行 2.1 安装gtsam 4.0.2 2.2 安装lio-sam 2.3 运行 三. 论文介绍 3.1 背景 3.2 方法 3.2.1 Lidar和IMU坐标系 3.2.1 因子图框架介绍 3.2.3 IMU因子 3.2.4 激光里程计因子 3.2.5 GPS因子 3.2.6 回环因子 四. 源码阅读 4.1 源码的整体框架 4.2 源码解读

非晚非晚的博客 7048

HALCON卡尺模型实战:5分钟搞定工件尺寸测量(附完整代码)

本文详细介绍了HALCON卡尺模型在工件尺寸测量中的实战应用,通过5分钟快速搭建测量系统的完整指南,包括基础配置、高级参数调优和完整代码示例。卡尺模型能自动识别边缘并计算精确尺寸,显著提升工业视觉检测的效率和一致性,特别适用于实时测量场景。

weixin_42570296的博客 464

最全卷积总结篇(分组、深度可分离、空洞、转置等)

分组卷积(Group Convolution)是一种卷积操作方式,它最早在AlexNet中引入,并在深度学习中被广泛使用,尤其是在高效模型设计(如MobileNet和ResNeXt)中。分组卷积通过将输入特征通道分为若干组,并在每组内单独进行卷积操作,可以减少计算复杂度和参数量,同时有效利用模型的结构特性。分组卷积的工作原理通道分组:输入特征图的通道图为C_in,输出特征图的通道数为C_out,将每组的C_in和C_out分为G组,每组的通道数为C_in/G和C_out/G。组内卷积。

qq_44815135的博客 4230

Zemax操作33--5倍激光扩束器

5倍扩束

lzy750750的博客 2936

AURIX™ TC3xx 调试口保护 secure debug

AURIX™ TC3xx 调试口保护 secure debug UCB_DBG_ORIG and UCB_DBG_COPY Content 使用Debug Password 保护调试接口设置 项目硬件接口保护方案 hexview Lock using UDE Unlockusing UDE Lock using Lauterbach Unlock using Lauterbach

2301_76563067的博客 6874

IMU标定中的Allen方差

Allen方差 Allen方差是IMU标定任务中的常用工具。IMU标定可以分为确定性误差和随机误差,确定性误差包括:scale、bias、misalignment。随机误差则主要是高斯白噪声和bias随机游走。Allen方差主要用于标定随机误差。 Allen方差获取 Allen方差的计算方法如下: 保持IMU静止, 采集N个数据点。 将N个点定长分组,常用的交叠式Allen方差如下(n=3): 对于每个子集,计算均值后,计算相邻两组的方差,得到的时间-方差曲线即为Allen方差。 Alle

realjc的博客 5740

【小白指南】如何将deepseek引入WPS\Word\Excel中去?

如何将deepseek引入WPS\Word\Excel中去?纯小白都能看的懂,一步步操作就能行

2503_90498422的博客 1万+

从零开始学习CANoe(六)—— CAPL 测试节点

创建CAPL Test Module的两种方式

蚂蚁小兵 1万+

IMSL CNL 7.0.0 (x86-64)Crack

IMSL CNL 7.0.0 (x86-64)Crack intel的算法库破解文件,这是C++版本的破解文件

vb 6.0并口开发操作实例

vb6并口开发,简体中文,试例内有inpout32.dll动态链接库

数学建模模型总结

这是本人在两年内准备参加数学建模竞赛搜集到的模型资料,已经经过了本人的筛选和整理,希望能够对准备参加数学建模竞赛的学弟学妹们提供一些帮助!

老版QQ原生表情gif动态

这是老版的QQ表情,不过全是动态的的,Windows照片查看器是不会动的,可以右击用浏览器打开查看,再说一遍是老版的,只是分享给宁愿用老版也要动态的人。

上一篇: LeGo-LOAM源码篇:源码分析(2)
下一篇: LeGo-LOAM源码篇:源码分析(4)
wenford2026
博客等级 码龄8年 52粉丝 21原创
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明:

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

余额充值
⌂ 换网址