3D激光SLAM:LeGO-LOAM---两步优化的帧间里程计及代码分析

简介: **LeGO-LOAM**的全称是 Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain其中LeGO就是轻量级和利用地面优化,轻量级的实现就是通过两步的优化方式,利用地面优化的部分也在两步优化的第一步中。和原始LOAM一样,通过前后两帧点云来估计两帧之间的运动,从而累加得到前端里程计的输出,和上述方法使用线面约束同时优化六自由度帧间位姿不同,LeGO-LOAM的前端分成两个步骤,每个步骤估计三自由度的变量。通过这种方式进行帧间里程计的运算,可以提供运算效率,使得可以在嵌入式平台

前言

LeGO-LOAM的全称是 Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

其中LeGO就是轻量级和利用地面优化,轻量级的实现就是通过两步的优化方式,利用地面优化的部分也在两步优化的第一步中。

和原始LOAM一样,通过前后两帧点云来估计两帧之间的运动,从而累加得到前端里程计的输出,和上述方法使用线面约束同时优化六自由度帧间位姿不同,LeGO-LOAM的前端分成两个步骤,每个步骤估计三自由度的变量。

通过这种方式进行帧间里程计的运算,可以提供运算效率,使得可以在嵌入式平台中运行

在里程计部分,LeGO-LOAM充分利用了前面存储的标签信息,在原本LOAM的里程计部分,其使用的思路更像是ICP,单纯寻找距离最近的点,而并没有考虑其合理性。在这里,LeGO-LOAM通过标签增加了一次筛选,标签相同的点才会用于优化,通过这种方法,能够大幅提高匹配的准确率。

此外,在优化部分,LOAM使用的是直接将所有参数放在一起,进行LM优化,而在LeGO-LOAM中,作者将LM优化分两步进行,每次优化三个参数,在确保准确性的前提下,减小了35%的计算开销。

利用地面点优化

地面点更符合面特征的性质,因此地面点的优化问题就使用点到面的约束来构建

地面点的约束对x,y,yaw这三个自由度是不能估测的。当这个三个自由度的值发生变化,点到面的残差不会发生显著变化,
地面点的优化,只对pitch、roll及z进行约束和优化

在这里插入图片描述
在这里插入图片描述
白色的为当前帧,绿色的为上一帧,可以看到当前帧发生yaw的旋转,和发生x、y的平移的时候,当前帧的面点到上一帧的距离都没有变,所有不能把这三个自由度的优化放到面点特征上来。

当z轴发生移动,或者发生pitch和roll角的变化,当前帧的面特征点,到上一帧的面特征点的距离即发生变化。

利用角点优化

第一步优化完pitch、roll和z之后,剩下还有三个自由度的变量需要进行估计,
需要提取角点进行优化,由于多线激光雷达提取的角点通常是垂直的边缘特征,因此对,x、y及yaw有着比较好的能观性,通过角点的优化结合上地面点的结果可以得到六自由度的帧间优化结果。

将上一步分优化的z、pitch和roll的三自由度量,和剩下没有估计的三个自由度进行角点的约束优化。

代码部分

LeGO-LOAM的帧间里程计部分在FeatureAssociation.cpp中 。
可以直接找到runFeatureAssociation的函数

        adjustDistortion();//去除畸变

        calculateSmoothness();//计算曲率

        markOccludedPoints();//剔除离散点

        extractFeatures();//提取特征

        publishCloud(); // cloud for visualization
    
        /**
        2. Feature Association
        */
        if (!systemInitedLM) {
            checkSystemInitialization();
            return;
        }

        updateInitialGuess();

        updateTransformation();//帧间里程计核心部分

        integrateTransformation();

        publishOdometry();

        publishCloudsLast(); // cloud to mapOptimization

在这个函数中进行了

  • 去除畸变
  • 计算曲率
  • 剔除离散点
  • 提取特征
  • 帧间里程计核心部分
  • 发布里程计
  • 发布点云

其中帧间里程计核心部分在 updateTransformation() 函数中

    void updateTransformation(){

        if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)
            return;

        //地面点帧间里程计估计优化
        for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {
            laserCloudOri->clear();
            coeffSel->clear();

            findCorrespondingSurfFeatures(iterCount1);//通过kdtree 寻找对应的点

            if (laserCloudOri->points.size() < 10)
                continue;
            if (calculateTransformationSurf(iterCount1) == false)//进行优化估计
                break;
        }

        //角点帧间里程计估计优化
        for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {

            laserCloudOri->clear();
            coeffSel->clear();

            findCorrespondingCornerFeatures(iterCount2);//通过kdtree 寻找对应的点

            if (laserCloudOri->points.size() < 10)
                continue;
            if (calculateTransformationCorner(iterCount2) == false)//进行优化估计
                break;
        }
    }

从这个函数中可以明显看出分成了两部分

  • 第一个for循环是 地面点帧间里程计估计优化
  • 第二个for循环是 角点帧间里程计估计优化

在这里插入图片描述
从这里可以看出surfPointsFlat中的点都是地面点

然后在findCorrespondingSurfFeatures的函数中,进行当前帧的地面点与上一帧中的地面点进行匹配。
其过程和ALOAM中的原理一致。

在findCorrespondingCornerFeatures的函数中,进行当前帧的角点与上一帧中的角点进行匹配。
其过程和ALOAM中的原理一致。

当前帧角点i,映射到上一帧点云中,并找到距离最近的点j,在与j相同的scan中,寻找次近邻点m。
利用映射后的点i到j和m的距离,进行约束

通过两步优化的位姿变换通过里程计数据的方式发送出来可以进行可视化的操作
具体代码在 publishOdometry() 函数中

当前帧角点i,映射到上一帧点云中,并找到距离最近的点j,并在相同scan中找到次近邻点l,在相邻的scan中再找到近邻点m。
利用映射后的点i到j、l、m平面的距离,进行约束。

//发布前端里程计
    void publishOdometry(){
        geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformSum[2], -transformSum[0], -transformSum[1]);

        laserOdometry.header.stamp = cloudHeader.stamp;
        laserOdometry.pose.pose.orientation.x = -geoQuat.y;
        laserOdometry.pose.pose.orientation.y = -geoQuat.z;
        laserOdometry.pose.pose.orientation.z = geoQuat.x;
        laserOdometry.pose.pose.orientation.w = geoQuat.w;
        laserOdometry.pose.pose.position.x = transformSum[3];
        laserOdometry.pose.pose.position.y = transformSum[4];
        laserOdometry.pose.pose.position.z = transformSum[5];
        pubLaserOdometry.publish(laserOdometry);

        laserOdometryTrans.stamp_ = cloudHeader.stamp;
        laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
        laserOdometryTrans.setOrigin(tf::Vector3(transformSum[3], transformSum[4], transformSum[5]));
        tfBroadcaster.sendTransform(laserOdometryTrans);
    }

其topic的名称为:
在这里插入图片描述

gazebo测试

在这里插入图片描述
前端的里程计在Z轴方向已经发生了漂移。
但是地图没有飘,这个是后端进行优化的结果,将前端漂移的部分优化了回来

相关文章
|
存储 传感器 编解码
3D激光SLAM:LeGO-LOAM论文解读---完整篇
![在这里插入图片描述](https://img-blog.csdnimg.cn/348d0b4467a24296a22413207566c67e.png) 论文的标题是:**LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain** - 标题给出的应用场景是 **可变地形** - 重点是 **轻量级** 并 利用 **地面优化** - 本质依然是一个 **激光雷达里程计和建图**
3D激光SLAM:LeGO-LOAM论文解读---完整篇
|
机器学习/深度学习 PyTorch TensorFlow
TensorRT 模型加速——输入、输出、部署流程
本文首先简要介绍 Tensor RT 的输入、输出以及部署流程,了解 Tensor RT 在部署模型中起到的作用。然后介绍 Tensor RT 模型导入流程,针对不同的深度学习框架,使用不同的方法导入模型。
4253 1
|
XML Android开发 数据格式
Android实现ProgressBar旋转菊花加载的动画
Android实现ProgressBar旋转菊花加载的动画 在一些常见到的加载中需要显示一个加载动画,如旋转的菊花这样的动画效果,开发中有两种方案,一种是直接让设计师给出一个gif图片,然后写入到布局,另外一种,使用Android动画,把一张静止的图片动起来,本例用后者实现。
4297 0
|
机器学习/深度学习 数据可视化 测试技术
YOLO26分割如何涨点系列篇(裂缝分割crack-seg) | 一种新颖的大核局部-全局-局部(LGL)模块,有效平衡图像信息低与高层语义差异大的问题,实现高效涨点
本文提出大核局部-全局-局部(LGL)模块,与YOLO26的C3k2深度融合,有效解决图像低层细节与高层语义差异大的问题。在crack-seg分割任务中,mAP50从0.68提升至0.69,召回率R达0.691,显著增强小目标分割性能。(239字)
396 0
YOLO26分割如何涨点系列篇(裂缝分割crack-seg) | 一种新颖的大核局部-全局-局部(LGL)模块,有效平衡图像信息低与高层语义差异大的问题,实现高效涨点
|
12月前
|
数据采集 运维 监控
金融行业RPA案例全解析:银行、证券、保险落地实践与价值数据
凌晨两点,银行、证券、保险、基金从业者仍在与数据搏斗。RPA“数字员工”正颠覆这一现状,通过自动化处理开户、清算、理赔、净值计算等高重复任务,大幅提升效率与合规性。实在智能的实在Agent以AI能力破解非结构化数据难题,助力金融行业实现从“人海战术”到“智能协同”的跨越,释放人力聚焦高价值服务,推动数字化转型迈向新阶段。
1041 1
|
传感器 算法 安全
具有飞行约束的无人机MPC模型预测控制研究(Matlab代码实现)
具有飞行约束的无人机MPC模型预测控制研究(Matlab代码实现)
778 2
|
8月前
|
机器学习/深度学习 人工智能 JSON
别被AI概念绕晕:一次分清迁移学习、微调与蒸馏
本文用职场比喻厘清大模型落地三大易混概念:迁移学习是“借力思维”(复用预训练权重);微调是“岗前培训”,提升垂直场景效果;知识蒸馏是“师徒传承”,压缩模型、提速降本。三者目标不同,选型需结合效果、延迟、数据与预算——理清本质,方能精准落地。
895 1
|
算法 数据可视化 定位技术
基于PCL库的通过ICP匹配多幅点云方法
基于PCL库的通过ICP匹配多幅点云方法
基于PCL库的通过ICP匹配多幅点云方法
|
NoSQL Redis 数据安全/隐私保护
Redis6入门到实战------ 二、Redis安装
这篇文章详细介绍了Redis 6的安装过程,包括下载、解压、编译、安装、配置以及启动Redis服务器的步骤。还涵盖了如何设置Redis以在后台运行,如何为Redis设置密码保护,以及如何配置Redis服务以实现开机自启动。
Redis6入门到实战------ 二、Redis安装

热门文章

最新文章