Lego_Loam--源码分析

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 阅读详情

0 整体框架分析

翻看 LEGO-Loam 的代码目录,首先进入到launch 文件中,看到:

  <!--- LeGO-LOAM -->    
    <node pkg="lego_loam" type="imageProjection"    name="imageProjection"    output="screen"/>
    <node pkg="lego_loam" type="featureAssociation" name="featureAssociation" output="screen"/>
    <node pkg="lego_loam" type="mapOptmization"     name="mapOptmization"     output="screen"/>
    <node pkg="lego_loam" type="transformFusion"    name="transformFusion"    output="screen"/>

luanch 文件启动了四个 node ,所以整个程序执行了四个 node 的基础上完成了整体功能的实现。

然后再打开 CmakeList.txt 可以看到这些 node 的一些依赖

add_executable(imageProjection src/imageProjection.cpp)
add_dependencies(imageProjection ${catkin_EXPORTED_TARGETS} cloud_msgs_gencpp)
target_link_libraries(imageProjection ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})

add_executable(featureAssociation src/featureAssociation.cpp)
add_dependencies(featureAssociation ${catkin_EXPORTED_TARGETS} cloud_msgs_gencpp)
target_link_libraries(featureAssociation ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})

add_executable(mapOptmization 
    src/mapOptmization.cpp
    src/Scancontext.cpp
)
target_link_libraries(mapOptmization 
    ${catkin_LIBRARIES} 
    ${PCL_LIBRARIES} 
    ${OpenCV_LIBRARIES} 
    gtsam
    Eigen3::Eigen
)

add_executable(transformFusion src/transformFusion.cpp)
target_link_libraries(transformFusion ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})

简单截取了以上部分,每一个节点之间链接了那些库可以看的很清楚。接下来的内容,按照这四个 node 的顺序 逐步对整个代码进行分析

1 ImageProjection – 将激光点云处理成图像

在这个node 中,cpp 文件实现了 激光点云特征提取,标号,分割等功能

第一步,先打开main 函数:

int main(int argc, char **argv)
{

    ros::init(argc, argv, "lego_loam");

    ImageProjection IP;

    ROS_INFO("\033[1;32m---->\033[0m Image Projection Started.");

    ros::spin();
    return 0;
}

在 main 函数中,只是实例化了一个对象 IP ,所以 一旦创建了一个对象,就进入到了 Class 中去执行构造函数。

      ImageProjection() : nh("~")
    {
		// 订阅原始的激光点云
        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 1, &ImageProjection::cloudHandler, this);
		
        // 转换成图片的点云
        pubFullCloud = nh.advertise<sensor_msgs::PointCloud2>("/full_cloud_projected", 1);
         // 转换成图片的并带有距离信息的点云
        pubFullInfoCloud = nh.advertise<sensor_msgs::PointCloud2>("/full_cloud_info", 1);
		
        // 发布提取的地面特征
        pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2>("/ground_cloud", 1);
        
        // 发布已经分割的点云
        pubSegmentedCloud = nh.advertise<sensor_msgs::PointCloud2>("/segmented_cloud", 1);
         // 具有几何信息的分割点云
        pubSegmentedCloudPure = nh.advertise<sensor_msgs::PointCloud2>("/segmented_cloud_pure", 1);
        pubSegmentedCloudInfo = nh.advertise<cloud_msgs::cloud_info>("/segmented_cloud_info", 1);
          
         // 含有异常信息的点云
        pubOutlierCloud = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud", 1);

        nanPoint.x = std::numeric_limits<float>::quiet_NaN();
        nanPoint.y = std::numeric_limits<float>::quiet_NaN();
        nanPoint.z = std::numeric_limits<float>::quiet_NaN();
        nanPoint.intensity = -1;

        allocateMemory();
        resetParameters();
    }

class 中的构造函数中,订阅了原始的激光点云数据(subLaserCloud)然后就进入到了 回调函数(ImageProjection::cloudHandler)对点云数据进行处理。

 // 订阅激光雷达点云信息之后的回调函数
    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
    {

        // 1. 使用 pcl库
        copyPointCloud(laserCloudMsg);

        // 2. 找到开始时刻和结束时刻的方向角度
        findStartEndAngle();

        // 3.将点云信息投影到 16 * 1800 分辨率的图像上(点云阵列上)
        projectPointCloud();

        // 4. 根据上下线束 俯仰角 判断是否是 地面  (角度小于10度 为地面)
        groundRemoval();

        // 5. 点云分割 首先对点云进行聚类标记 然后通过聚类完成的标签 对点云分块存储
        cloudSegmentation();
剥洋葱
        // 6. Publish all clouds 发布各类型的点云
        publishCloud();

        // 7. 重启
        resetParameters();
    }

回调函数中 按照以上 7 个逻辑步骤 对点云进行处理 个人认为这个回调函数写的相当nice 主要的逻辑步骤通过一个个封装好的函数呈现出来。按照总分的结构下面将逐步对这几个步骤进行分析。

1.1 ROS 消息转PCL

​ 由于激光雷达点云消息在传递过程中使用的是ROS 类型的消息,所以在处理的过错中统一需要对消息类型转换,通常的办法就是使用 PCL 库

void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
    {
        // 1. 读取ROS点云转换为PCL点云
        cloudHeader = laserCloudMsg->header;
        cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
        pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);

        //2.移除无效的点云 Remove Nan points
        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);

        // 3. have "ring" channel in the cloud or not
        // 如果点云有"ring"通过,则保存为laserCloudInRing
        // 判断是不是使用了 velodyne 的激光雷达
        if (useCloudRing == true)
        {
            pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
            if (laserCloudInRing->is_dense == false)
            {
                ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
                ros::shutdown();
            }
        }
    }

​ 这个函数主体内容 分为三个步骤,其中的第三步 注意区分 自己使用的激光雷达是否存在 CloudRing 没有这个的激光雷达 接下来的步骤都不会去执行,所以在实际自己跑这个框架的时候一定要注意到这个地方

1.2 寻找方向角

   void findStartEndAngle()
    {
        // 1.开始点和结束点的航向角 (负号表示顺时针旋转)   
        segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
        
        // 加 2 * M_PI 表示已经转转了一圈
        segMsg.endOrientation = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,
                                       laserCloudIn->points[laserCloudIn->points.size() - 1].x) +
                                2 * M_PI;
        // 2.保证 所有角度 落在 [M_PI , 3M_PI] 上 
        
        if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI)
        {
            segMsg.endOrientation -= 2 * M_PI;
        }
        else if (segMsg.endOrientation - segMsg.startOrientation < M_PI)
            segMsg.endOrientation += 2 * M_PI;
        segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;
    }

​ 这个函数在 LOAM 代码里面也存在。首先通过一个反正切函数 找到一帧激光点云起始时刻和结束时刻的航向角 第二步的作用 我个人认为是将一帧激光点云的航向角度限制在【pi , 3pi 】 之间,方便后续计算出一帧激光点云的时间。

1.3 激光点云图片化

    // 3.点云消息处理成图像方式的阵列 将一帧点云变成 16* 1800 的图片
    //  rangeMat.at<float>(rowIdn, columnIdn) = range; 关键的代码  
    void projectPointCloud()
    {
        // range image projection
        float verticalAngle, horizonAngle, range;
        size_t rowIdn, columnIdn, index, cloudSize;
        PointType thisPoint;

        cloudSize = laserCloudIn->points.size();
        
        // 遍历整个点云 
        for (size_t i = 0; i < cloudSize; ++i)
        {

            // 提取点云中 x y z 坐标数值
            thisPoint.x = laserCloudIn->points[i].x;
            thisPoint.y = laserCloudIn->points[i].y;
            thisPoint.z = laserCloudIn->points[i].z;
            // find the row and column index in the iamge for this point
            // 判断是不是使用了 velodyne 的雷达
            if (useCloudRing == true)
            {   
                // 提取激光雷达线束到 rowIdn 
                rowIdn = laserCloudInRing->points[i].ring;
            }
            //  是其他的雷达 就通过俯仰角 确定当前的激光点是来自哪个线束 index 
            else
            {
                verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
                // idx = (theta +15.1)/2 
                rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
            }
            if (rowIdn < 0 || rowIdn >= N_SCAN)
                continue
LeGO-LOAM代码解析---基础知识 在学习LeGO-LOAM之前,先介绍一下基础知识: 位姿是旋转和平移的统称。在三维世界中,物体的位姿变换既要考虑物体的位置的变化,也要考虑姿态的变化。 举个例子,你在宿舍里,以宿舍某个点取个固定坐标系,例如东北天坐标系。现在你要去食堂吃饭,无容置疑在这个坐标系中你的位置肯定发生变化,此外还有一个“东东”也在不断发生变化,也就是你脸的朝向(PS:假设是刚体变换)。假设在起点,你脸的朝向正东面,左手在北面,右手在南面,在行进过程中,显然你自身的姿态肯定不断发生变化。 我 阅读详情

相关推荐

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

mapOptmization.cpp,主要内容包括回环检测、scan-to-map位姿优化、因子图。

slamml 1386

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

Xilinx ERNIC RoCEv2 FPGA实现与QP队列深度解析

本文深入解析Xilinx ERNIC RoCEv2在FPGA上的实现,重点剖析QP队列对的管理机制与深度优化策略。通过Verilog代码示例和工程实战,详细介绍了如何实现高吞吐量、低延迟的RDMA硬件卸载方案,为金融交易和数据中心应用提供性能优化指导。

solidity8miner的博客 884

避坑指南:PVE下N卡直通错误代码43的终极解决思路与dmesg日志分析

本文详细解析了PVE环境下NVIDIA显卡直通时遇到的错误代码43问题,提供了从dmesg日志分析到BIOS配置、PVE宿主机优化及虚拟机设置的完整解决方案。特别针对GTX1060等显卡,分享了驱动版本选择、ROM修补及高级排错技巧,帮助用户彻底解决这一常见难题。

weixin_28693683的博客 297

Win10更新后黑屏只剩鼠标?火绒误杀explorer.exe的5种自救方案(附详细步骤)

本文详细介绍了Win10更新后出现黑屏只剩鼠标的5种专业修复方案,重点解决火绒误杀explorer.exe的问题。从基础的任务管理器操作到安全模式下的系统修复,再到注册表编辑和系统还原,提供了全面的解决方案,帮助用户快速恢复系统正常运行。

weixin_29306571的博客 653

Unity2020版本使用Kinect的修复(附粒子人像互动demo)

Unity2020版本使用Kinect的修复附粒子人像互动demo工程版本及说明工程demo说明演示效果工程地址 工程版本及说明 Unity2020.1.3f1c1 因为想用(学习)jpのK神做的一些粒子效果,由于他使用的是2020的新技术故需要把Kinect插件导入进2020版本,但2020版本已经不支持GUIText及GUITex这些操作了 因此需要修复成UGUI。 工程demo说明 打开工程后直接进入这个场景即可,如果只是想看粒子效果则需要把KinectController关掉,然后开启Beta O

qq_42685027的博客 5374

Qualcomm 常用NV列表

^ NV ITEM ^ 功能^^ |NV10 |mode prefer,找网模式|新平台使用NV80000| ^NV74 NV74=0关闭自动接听电话^ |NV240 |NV240=FALSE|EVDO出现网络的问题,此项必须设定为FALSE| |NV255 |CDMA规避网络列表(SID/NID号)| |NV256 |

sdrgtywretyeruet的专栏 5782

基于 Simulink 的三相四线制整流器在三相不平衡负载下的中点电位平衡控制实战教程。

本文介绍了三相四线制整流器在不平衡负载下的中点电位平衡控制方法。首先分析了中点电位漂移的物理机理和控制策略(零序电压注入)。接着详细讲解了Simulink建模步骤:搭建三相四线制主电路、设计平衡控制算法、实现PWM调制。通过对比有无平衡控制的仿真结果,验证了控制方法的有效性。最后提供了工程建议:零序注入需限幅、考虑中性线寄生电感影响、采用稳健的PI参数。该方案能有效抑制中点电位波动,保障三电平系统在不平衡工况下的稳定运行。

MHD0815的博客 66

Simulink仿真模块 -Scope

如果您将 Scope 模块放在具有锁定链接的库或者锁定的库中,当尝试打开示波器窗口时,Simulink 会显示错误。要显示某个库模块中的内部数据,请为库模块添加一个输出端口,然后将此端口连接到您模型中的 Scope 模块。当您在示波器内可视化多个基于帧的信号时,部分帧大小为 1 的信号采样可能不会显示。当在示波器内可视化多个基于帧的信号时,部分帧大小为 1 的信号采样可能不会显示。如果在仿真开始时波形视图关闭,则当在仿真后打开波形视图时,波形视图将显示附加的输入信号的仿真结果。

jk_101的博客 3793

使用pyqt5实现可勾选的测试用例界面

使用pyqt5实现可勾选的测试用例界面

天赐好车的博客 630

TMS_WebGMaps_1.9.8.0_Full_Source

TMS_WebGMaps_1.9.8.0_Full_Source 全源码 delphi cbuilder

老版QQ原生表情gif动态

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

廖雪峰Python教程[完整版]

廖雪峰的python教程整理的,已经排好版了,而且还是完整的,适合小白

Probability and Statistics - 4th edition

Probability and Statistics - 4th edition

上一篇: 高翔--视觉SLAM 14讲 --3.6.2实际坐标变换的例子
下一篇: 如何理解旋转矩阵怎么来的
Pepper_Doc
博客等级 码龄6年 626粉丝 12原创
评论 3
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明:

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

余额充值
⌂ 换网址