LIO-SAM代码解析——featureExtraction.cpp

最新推荐文章于 2023-01-30 18:50:07 发布
最新推荐文章于 2023-01-30 18:50:07 发布
阅读量379 收藏 4
点赞数 2
分类专栏： lio-sam 文章标签： 计算机视觉 人工智能
原文链接：https://blog.csdn.net/iwanderu/article/details/123080537
版权

目录

在这里插入图片描述传感器输入： IMU，Point Cloud， GPS（可选）
输出 ： IMU 频率的odometry

imageProjection.cpp: 接受IMU，PointCloud以及IMU预积分输出的IMU odometry（系统刚初始化时没有IMU odometry）。

  • 主要功能：
    1. 基于IMU odometry得到系统的初始位姿
    2. 将点云投影到cv::mat中，做相应的预处理
    3. 对原始点云数据做运动补偿（点云的去畸变补偿在代码中只应用于旋转部分，注释掉了平移部分）

featureExtraction.cpp :接收来自imageProjection处理完的lidar帧数据，对应的点云是去畸变的，也就是在同一个坐标系下。对点云进行面点，角点特征的分类。

  • 主要功能：
    1. 提取点云边缘特征和面特征

mapOptimization.cpp:

  • 主要功能：
    1. 将提取到点云特征与地图中的边缘特征和面特征进行配准
    2. 配准后得到当前帧在地图中的位姿
    3. 图优化： 将lidar的帧间约束，回环的约束，（GPS因子）添加到因子图中

imuPreintegration.cpp:一开始并没有工作，只有收到lidar odometry后才会工作

  • 主要功能：
    1. 图优化：lidar odometry和IMU的帧间约束添加到因子图中
    2. 估计IMU零偏

featureExtraction.cpp

主函数只存在1个类FeatureExtraction FE

1. FeatureExtraction类

FeatureExtraction()
    {
        // 订阅当前激光帧运动畸变校正后的点云信息
        subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
        // 发布当前激光帧提取特征之后的点云信息
        pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/feature/cloud_info", 1);
        // 发布当前帧的角点点云
        pubCornerPoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_corner", 1);
        // 发布当前帧的面点点云
        pubSurfacePoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_surface", 1);
        // 初始化
        initializationValue();
    }

1.1. laserCloudInfoHandler

// 点云特征提取
    void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
    {
        cloudInfo = *msgIn; // new cloud info
        cloudHeader = msgIn->header; // new cloud header
        // 把提取出的有效的点转成pcl格式
        pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction
        // 计算当前激光帧点云中每个点的曲率
        calculateSmoothness();
        // 标记属于遮挡、平行两种情况的点，不做特征提取，同LOAM论文图4的情况
        markOccludedPoints();
        // 提取特征，同aloam，没有区分明显和不明显的角点/面点
        extractFeatures();
        // 发布角点、面点点云，发布带特征点云数据的当前激光帧点云信息
        publishFeatureCloud();
    }
1.1.1. calculateSmoothness()
// 计算曲率
    void calculateSmoothness()
    {
        // 遍历当前激光帧运动畸变校正后的有效点云
        int cloudSize = extractedCloud->points.size();
        for (int i = 5; i < cloudSize - 5; i++)
        {
            // 计算当前点和周围十个点的距离差，平坦位置处曲率较小，角点处曲率较大
            float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4]
                            + cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2]
                            + cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10
                            + cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2]
                            + cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4]
                            + cloudInfo.pointRange[i+5];            
            // 距离差的平方做曲率
            cloudCurvature[i] = diffRange*diffRange;//diffX * diffX + diffY * diffY + diffZ * diffZ;
            // 下面两个值赋成初始值
            cloudNeighborPicked[i] = 0;
            cloudLabel[i] = 0;
            // cloudSmoothness for sorting
            // 存储该点曲率值、激光点一维索引
            cloudSmoothness[i].value = cloudCurvature[i];
            cloudSmoothness[i].ind = i;
        }
    }
1.1.2. markOccludedPoints()
// 选出(剔除)与激光方向平行或被遮挡的激光点，同LOAM论文图4的情况
    void markOccludedPoints()
    {
        int cloudSize = extractedCloud->points.size();
        // mark occluded points and parallel beam points
        for (int i = 5; i < cloudSize - 6; ++i)
        {
            // occluded points
            // 取出相邻两个点距离信息
            float depth1 = cloudInfo.pointRange[i]; // 存储了点到lidar中心的距离
            float depth2 = cloudInfo.pointRange[i+1];
            // 计算两个有效点之间的列id差
            int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));
            // 1. 判断是否存在遮挡
            // 只有比较靠近才有意义
            if (columnDiff < 10){
                // 10 pixel diff in range image
                // 前者深度更大，说明后者把前者遮挡了，所以前者标记为1后续就不用了
                if (depth1 - depth2 > 0.3){  // 如果左边的点更远，那么左边的五个点都是无效点
                    cloudNeighborPicked[i - 5] = 1;
                    cloudNeighborPicked[i - 4] = 1;
                    cloudNeighborPicked[i - 3] = 1;
                    cloudNeighborPicked[i - 2] = 1;
                    cloudNeighborPicked[i - 1] = 1;
                    cloudNeighborPicked[i] = 1;
                // 后者深度更大，说明前者把后者遮挡了，所以后者标记为1后续就不用了
                }else if (depth2 - depth1 > 0.3){  // 此处同理，如果右边的边更远，那么右边的五个点设置为无效点
                    cloudNeighborPicked[i + 1] = 1;
                    cloudNeighborPicked[i + 2] = 1;
                    cloudNeighborPicked[i + 3] = 1;
                    cloudNeighborPicked[i + 4] = 1;
                    cloudNeighborPicked[i + 5] = 1;
                    cloudNeighborPicked[i + 6] = 1;
                }
            }

            // parallel beam
            // 2.判断当前点是否在与激光帧平行的平面上
            // 用前后相邻点判断当前点所在平面是否与激光束方向平行          
            float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));
            float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));
            // 如果两个点距离比较大，就很可能是平行的点，也很可能失去观测
            if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
                cloudNeighborPicked[i] = 1;
        }
    }
1.1.3. extractFeatures()
// 提取特征
    void extractFeatures()
    {
        cornerCloud->clear();
        surfaceCloud->clear();

        pcl::PointCloud<PointType>::Ptr surfaceCloudScan(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr surfaceCloudScanDS(new pcl::PointCloud<PointType>());

        for (int i = 0; i < N_SCAN; i++)
        {
            surfaceCloudScan->clear();
            // 把每一份scan等分成6份，每份分别提取特征点
            for (int j = 0; j < 6; j++)
            {
                // 根据之前得到的每个scan的起始点和结束id来均分
                // 每段点云的起始、结束索引；startRingIndex为扫描线起始第5个激光点在一维数组中的索引
                int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;
                int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1;
                //  这种情况就不正常
                if (sp >= ep)
                    continue;
                // 按曲率排序
                std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());

                int largestPickedNum = 0;
                // 按照曲率从大到小遍历
                for (int k = ep; k >= sp; k--)
                {
                    // 找到这个点对应的原先的idx
                    int ind = cloudSmoothness[k].ind;
                    // 如果没有被认为是遮挡点，且曲率大于边缘点阈值，则认为是角点
                    if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)
                    {
                        largestPickedNum++;
                        // 每段最多找20个角点
                        // 如果单条扫描线扫描一周是1800个点，则划分6段，每段300个点，从中提取20个角点
                        if (largestPickedNum <= 20){
                            // 标记为角点
                            cloudLabel[ind] = 1;
                            // 加入角点点云
                            cornerCloud->push_back(extractedCloud->points[ind]);
                        } else {
                            break;
                        }
                        // 标记已被处理
                        cloudNeighborPicked[ind] = 1;
                        // 同一条扫描线上后5个点标记一下，不再处理，避免特征聚集
                        for (int l = 1; l <= 5; l++)
                        {
                            int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
                            if (columnDiff > 10)
                                break;
                            cloudNeighborPicked[ind + l] = 1;
                        }
                        // 同一条扫描线上前5个点标记一下，不再处理，避免特征聚集
                        for (int l = -1; l >= -5; l--)
                        {
                            int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));
                            if (columnDiff > 10)
                                break;
                            cloudNeighborPicked[ind + l] = 1;
                        }
                    }
                }
                // 按照曲率从小到大遍历
                for (int k = sp; k <= ep; k++)
                {
                    // 激光点的索引
                    int ind = cloudSmoothness[k].ind;
                    // 当前激光点还未被处理，且曲率小于阈值，则认为是平面点
                    if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold)
                    {
                        // 标记为平面点
                        cloudLabel[ind] = -1;
                        // 标记已被处理
                        cloudNeighborPicked[ind] = 1;
                        // 同一条扫描线上后5个点标记一下，不再处理，避免特征聚集
                        for (int l = 1; l <= 5; l++) {
                            int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
                            if (columnDiff > 10)
                                break;
                            cloudNeighborPicked[ind + l] = 1;
                        }
                        // 同一条扫描线上前5个点标记一下，不再处理，避免特征聚集
                        for (int l = -1; l >= -5; l--) {
                            int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));
                            if (columnDiff > 10)
                                break;
                            cloudNeighborPicked[ind + l] = 1;
                        }
                    }
                }
                // 平面点和未被处理的点，都认为是平面点，加入平面点云集合
                for (int k = sp; k <= ep; k++)
                {
                    // 注意这里小于等于0,也就是说不是角点的都认为是面点了
                    if (cloudLabel[k] <= 0){
                        surfaceCloudScan->push_back(extractedCloud->points[k]);
                    }
                }
            }

            surfaceCloudScanDS->clear();
            // 因为面点太多，所以做一个下采样
            downSizeFilter.setInputCloud(surfaceCloudScan);
            downSizeFilter.filter(*surfaceCloudScanDS);
            // 加入平面点云集合
            *surfaceCloud += *surfaceCloudScanDS;
        }
    }
1.1.4. publishFeatureCloud()
void publishFeatureCloud()
    {
        // free cloud info memory
        freeCloudInfoMemory();
        // save newly extracted features
        // 发布角点、面点点云，用于rviz展示
        cloudInfo.cloud_corner  = publishCloud(&pubCornerPoints,  cornerCloud,  cloudHeader.stamp, lidarFrame);
        cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame);
        // publish to mapOptimization
        // 发布当前激光帧点云信息，加入了角点、面点点云数据，发布给mapOptimization
        pubLaserCloudInfo.publish(cloudInfo);
    }
每天都在努力学习SLAM的小黑
关注
  • 2
    点赞
  • 踩
  • 4
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
Ubuntu 16.04 lio_sam运行环境配置
zbr794866300的博客
11-17 1914
搭建成功环境: ubuntu16.04 pcl1.9.0 gtsam 4.0.2 一 pcl1.9.0安装 电脑原来安装的是1.7的版本,网上的一些人说,1.7版本最后不能呢个成功运行,需要1.8以上的版本,所以先卸载之前的pcl版本,然后重新安装1.9. (1)卸载不满足条件的pcl版本 sudo apt-get remove libpcl1.7 sudo apt-get remove libpcl-* (2)PCL依赖库安装 sudo apt-get update sudo apt
LIO-SAM代码解析-徐胜攀.pdf
最新发布
06-23
对LIO-SAM算法代码进行了深入分析，对各模块功能和联系进行梳理，对重要计算过程进行详细解读。读完本文档将可以清晰准确地了解LIO-SAM算法思想和实现。
关于LIO-SAM编译时出现错误 /usr/bin/ld: 找不到 -lBoost::timer
wwtx的博客
12-30 2744
关于LIO-SAM编译时出现错误： 下载LIO-SAM代码 进入LIO-SAM文件夹，通过以下代码编译： mkdir build cd build cmake .. make 报错： /usr/bin/ld: 找不到 -lBoost::timer collect2: error: ld returned 1 exit status CMakeFiles/lio_sam_imuPreintegration.dir/build.make:759: recipe for target 'devel/lib/li
LIO-SAM featureExtraction
xiaoma_bk的博客
05-19 285
featureExtraction1、FeatureExtraction1.1 类成员变量1.2 构造1.3 激光数据回调calculateSmoothness标记遮挡点 markOccludedPointsextractFeaturespublishFeatureCloud 1、FeatureExtraction 1.1 类成员变量 1、订阅 激光数据 发布 点云数据+角特征+面特征 2、提取点云 角点云 面点云 3、将采样 ros::Subscriber subLaserCloudInfo; ros
LIO-SAM代码阅读——featureExtraction.cpp
qq_44305240的博客
09-24 281
LIO-SAM代码阅读——featureExtraction.cpp
LIO-SAM源码解读（二）：FeatureExtraction
JaydenQ的博客
09-09 564
写在前面 FeatureExtraction 点云特征提取 功能简介: 对经过运动畸变矫正之后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点（用曲率的大小进行判定）。 订阅： 1、订阅当前激光帧运动畸变矫正后的点云信息，来自ImageProjection。 发布： 1.发布当前激光帧提取特征之后的点云信息，包括的历史数据有：运动畸变矫正，点云数据，初始位姿，姿态角，有效点云数据，角点点云，平面点点云等，发布给MapOptimization； 2.发布当前激光帧提取的角点点云，用rviz展示； 3.
LIO-SAM代码（中文注释）
02-24
**LIO-SAM（LiDAR-Inertial Odometry with Simultaneous Localization and Mapping）**是一种先进的定位与建图算法，特别适用于具有激光雷达（LiDAR）和惯性测量单元（IMU）传感器的机器人系统。这个算法结合了两者...
LIO-SAM代码阅读详细注释版
11-16
本文将围绕LIO-SAM的代码进行详细解释，以帮助读者理解其核心思想和实现细节。 1. **系统框架** LIO-SAM采用EKF（扩展卡尔曼滤波）框架，结合IMU的连续运动模型和LiDAR的离散扫描数据，对传感器数据进行融合，以...
lio-sam-kitti
05-14
《LIO-SAM与KITTI数据集：激光雷达惯导集成SLAM的实践与解析》 在自动驾驶、机器人导航和地图构建等领域，定位与建图（Simultaneous Localization and Mapping, SLAM）技术扮演着至关重要的角色。LIO-SAM（LiDAR-...
LIO-SAM:LIO-SAM
03-18
LIO-SAM实时激光雷达惯性测距软件包。我们强烈建议用户彻底阅读本文档，并首先使用提供的数据集对软件包进行测试。有关该方法演示的视频可在找到。菜单（必须读取）（必须读取）系统架构我们设计了一个系统，该系统...
LIO-SAM代码流程详解
liaolizhou的博客
03-18 2887
在阅读源码的过程中，往往会遇到不知道每个回调函数是在什么时候对数据进行处理，又是如何将处理的信息传递给其他的回调函数。因此，本文旨在根据每一个类给出代码的流程图，让我们在阅读源码的过程中能后更加清晰明了的认识到这一个类在做什么，以及做了什么。
LIO-SAM源码解析（六）：featureExtraction.cpp
人无远虑，必有近忧
12-03 431
对每条线上大曲率进行排序，每条线分为6段，每段最多提取20个。大于阈值就是角点（默认0.1），并且标记该点周围其他点已经被提取了。小于阈值同理，当成平面点。不过平面点要经过一个降采样过程（角点不用，可能是平面点比较多）。角点以"lio_sam/feature/cloud_corner"发布，平面点以"lio_sam/feature/cloud_surface"发布，然后把二者合在一起，在lio_sam/deskew/cloud_info的基础上，填充cloud_corner和cloud_surface字段，
LIO-SAM探秘第三章之代码解析（二） --- featureExtraction.cpp
李想的博客
11-09 2726
1 main() int main(int argc, char** argv) { ros::init(argc, argv, "lio_sam"); FeatureExtraction FE; // \033[1;32m，\033[0m 终端显示成绿色 ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m"); ros::spin(); // 一个线程 retur
LIO-sam源码分析：featureExtraction.cpp
非晚非晚的博客
02-09 621
这一块代码和LeGO的思想是一致的，而且还略去了scan-to-scan的内容，整体更加简介。 构造函数–》回调函数FeatureExtraction()： 订阅： lio_sam/deskew/cloud_info 发布： lio_sam/feature/cloud_info lio_sam/feature/cloud_corner lio_sam/feature/cloud_surface 函数首先会进入回调函数：laserCloudInfoHandler void laserCloudI.
LIO-SAM源码解析-特征提取featureExtraction.cpp
chennuo0125的博客
12-19 1859
节点数据传递 从上图可以看出来，特征提取节点(红色)/lio_sam_featureExtraction只订阅图像投影节点发布的话题(蓝色)/lio_sam_deskew/cloud_info，并且只发布话题(绿色)/lio_sam/feature/cloud_info． 订阅 /lio_sam_deskew/cloud_info 做了什么？ void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr &msgIn) { clo
第七讲 feature_extraction.cpp
qq_40213457的博客
07-09 1053
#include &lt;iostream&gt;#include &lt;opencv2/core/core.hpp&gt;#include &lt;opencv2/features2d/features2d.hpp&gt;#include &lt;opencv2/highgui/highgui.hpp&gt; using namespace std;using namespace cv; in...
lio-sam之FeatureExtraction.cpp
weixin_49227107的博客
08-22 540
该cpp里就一个回调函数lasercloudinfohandler。
LIO-SAM代码解析——imageProjection.cpp
weixin_45626706的博客
01-30 998
lio-sam
《视觉slam十四讲》第七讲feature_extraction.cpp程序解释
lcmsir的博客
01-30 1941
Ptr orb = ORB::create(int nfeatures=500, float scaleFactor=1.2f, int nlevels=8, int edgeThreshold=31, int firstLevel=0, int WTA_K=2, int scoreType=ORB::HARRIS_SCORE, int patchSize=31) 由于在书中直接省略了，
lio-sam代码阅读
11-17
了解lio-sam如何解析SCSI命令、调用底层存储设备和返回响应可以帮助理解其工作原理和性能优化方法。 4. 性能优化技巧：lio-sam的设计目标之一是提高性能。代码中可能包含一些性能优化技巧，如批量处理、IO调度和...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值