LIO-SLAM 代码详细注释二featureExtraction.cpp

最新推荐文章于 2022-12-07 10:01:23 发布
最新推荐文章于 2022-12-07 10:01:23 发布
阅读量384 收藏 2
点赞数
分类专栏： LIO-SLAM 文章标签： 自动驾驶
版权声明：本文为博主原创文章，遵循 CC 4.0 BY 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_44023934/article/details/113752217
版权

featureExtraction.cpp

#include "utility.h"
#include "lio_sam/cloud_info.h"

struct smoothness_t{ 
    float value; //曲率
    size_t ind;  //序号
};

struct by_value{ //曲率值对比
    bool operator()(smoothness_t const &left, smoothness_t const &right) { 
        return left.value < right.value;
    }
};

class FeatureExtraction : public ParamServer
{

public:
    ros::Subscriber subLaserCloudInfo;
    ros::Publisher  pubLaserCloudInfo;
    ros::Publisher  pubCornerPoints;//发布角点特征
    ros::Publisher  pubSurfacePoints;//发布面特征
//pcl点云指针，提取、角点、面
    pcl::PointCloud<PointType>::Ptr extractedCloud;//有效点
    pcl::PointCloud<PointType>::Ptr cornerCloud;   //存角点
    pcl::PointCloud<PointType>::Ptr surfaceCloud;  //存面

    pcl::VoxelGrid<PointType> downSizeFilter;

    lio_sam::cloud_info cloudInfo;
    std_msgs::Header cloudHeader;                 //发布topic的时间戳

    std::vector<smoothness_t> cloudSmoothness;    //存储每个点的曲率和索引号
    float *cloudCurvature;                        //存储每个点的曲率
    int *cloudNeighborPicked;                     //不进行特征提取的索引
    int *cloudLabel;                              //标记面的索引

    FeatureExtraction()
    {
        su`bLaserCloudInfo = 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();
    }

    void initializationValue()
    {
        cloudSmoothness.resize(N_SCAN*Horizon_SCAN);
         //降采样参数0.2
        downSizeFilter.setLeafSize(odometrySurfLeafSize, odometrySurfLeafSize, odometrySurfLeafSize);

        extractedCloud.reset(new pcl::PointCloud<PointType>());
        cornerCloud.reset(new pcl::PointCloud<PointType>());
        surfaceCloud.reset(new pcl::PointCloud<PointType>());

        cloudCurvature = new float[N_SCAN*Horizon_SCAN];
        cloudNeighborPicked = new int[N_SCAN*Horizon_SCAN];
        cloudLabel = new int[N_SCAN*Horizon_SCAN];
    }
//将订阅的点云信息传到cloudInfo 
    void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
    {
        cloudInfo = *msgIn; // new cloud info
        cloudHeader = msgIn->header; // new cloud header
        pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction，转成ros信息点云
        //计算曲率
        calculateSmoothness();
        //标记遮挡和平行点
        markOccludedPoints();
        //提取角点和面特帧
        extractFeatures();
        //发布特征点云
        publishFeatureCloud();
    }
//计算曲率 10个点
    void calculateSmoothness()
    {
        int cloudSize = extractedCloud->points.size();
        //序号从第五个开始，前后五个，共10个

        for (int i = 5; i < cloudSize - 5; i++)
        {   //但前点与附近10点的距离差和
            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;
        }
    }
//标记不提取的点云特征，遮挡平行点
    void markOccludedPoints()
    {
        int cloudSize = extractedCloud->points.size();
        // mark occluded points and parallel beam points
        for (int i = 5; i < cloudSize - 6; ++i)//与后一个点插值，所以减去6
        {
            // occluded points
            float depth1 = cloudInfo.pointRange[i];
            float depth2 = cloudInfo.pointRange[i+1];
            //列索引间的距离
            int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));

            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;
                }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平行线的 情况，左右两点与该点的深度差，确定该点是否会被选择成为特征点
            float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));
            float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));
            //与前后点的距离大于深度的百分之2,视为远离群点、斜坡点、凹凸面点和空旷区域点，设置为筛选过、弃用
            //因为噪声点会影响与该障碍物平面的匹配
            if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
                cloudNeighborPicked[i] = 1;
        }
    }
//进行角点与面点的提取，角点直接保存，面需要经过降采样之后保存
    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();
            //六次循环，每次都要找2个角点（平滑差）和四个面（平滑好）
            for (int j = 0; j < 6; j++)
            {
                //startRingIndex从4到20 共16次
                //假如起始0,终点是360度，将一帧云平均分乘6份，每一份60度
                //ep是end_index sp 是start_index  如：startRingIndex[i] =1  endRingIndex[i]=60
                //sp1 : 1     sp2 : 11    sp3 : 21        sp6 : 51
                //ep1 : 10    ep2 : 20    ep3 : 30........ep6 : 60

                //最后的点的曲率最大，如果满足条件就是角点    （6-j）/6                               j/6
                int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;
                ///                                       (5 - j)/6                              （j + 1)/6         -1
                int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1;

                if (sp >= ep)
                    continue;
                 //cloudSmoothness.包括曲率和序号，将曲率从小到达排序
                std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());

                int largestPickedNum = 0;

                //从后往前读取，越前面曲率越低越不满足边缘要求
                //进行角点提取
                for (int k = ep; k >= sp; k--)
                {
                    int ind = cloudSmoothness[k].ind;                           
                    // edgeThreshold: 1.0
                    // surfThreshold: 0.1
                    //        可提取特帧                              曲率                    
                    if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)//边缘
                    {
                        largestPickedNum++;
                        //每一段最多选取20个点
                        if (largestPickedNum <= 20){
                            cloudLabel[ind] = 1;//因为是角点所以不是面 ，标记为1指的角点，-1指的是面
                            cornerCloud->push_back(extractedCloud->points[ind]);
                        } else {
                            break;
                        }
                         //已经标记了角点，就不再重复标记
                        cloudNeighborPicked[ind] = 1;
                        //同时避免了特征点聚集，所以对前后的五个角点不再提取特征
                        for (int l = 1; l <= 5; l++)//前5
                        {                                            //一帧云与光心距离
                            int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
                            if (columnDiff > 10)
                                break;
                            cloudNeighborPicked[ind + l] = 1;
                        }
                        for (int l = -1; l >= -5; l--)//后5
                        {
                            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;  //不再重复提取特征

                        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 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++)
                {
                    if (cloudLabel[k] <= 0){
                        surfaceCloudScan->push_back(extractedCloud->points[k]);
                    }
                }
            }
            //对面进行pcl库降采样，临时保存在surfaceCloudScanDS
            surfaceCloudScanDS->clear();
            downSizeFilter.setInputCloud(surfaceCloudScan);
            downSizeFilter.filter(*surfaceCloudScanDS);
           //保存临时值
            *surfaceCloud += *surfaceCloudScanDS;
        }
    }

    void freeCloudInfoMemory()
    {
        cloudInfo.startRingIndex.clear();
        cloudInfo.endRingIndex.clear();
        cloudInfo.pointColInd.clear();
        cloudInfo.pointRange.clear();
    }

    void publishFeatureCloud()
    {
        // free cloud info memory
        freeCloudInfoMemory();
        // save newly extracted features
        cloudInfo.cloud_corner  = publishCloud(&pubCornerPoints,  cornerCloud,  cloudHeader.stamp, lidarFrame);
        cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame);
        // publish to mapOptimization
        pubLaserCloudInfo.publish(cloudInfo);
    }
};


int main(int argc, char** argv)
{
    ros::init(argc, argv, "lio_sam");

    FeatureExtraction FE;

    ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m");
   
    ros::spin();

    return 0;
}

参考博文：
https://bbs.csdn.net/topics/398260808
https://zhuanlan.zhihu.com/p/182408931
https://zhuanlan.zhihu.com/p/57351961
https://zhuanlan.zhihu.com/p/111388877

不加辣先生
关注
专栏目录
LIO-SAM源码阅读笔记(三)---featureExtraction.cpp
Zlp19970106的博客
09-13 983
文章目录
LIO-SAM代码解析——featureExtraction.cpp
weixin_45626706的博客
01-30 581
lio-sam
LIO-SAM源码解析（六）：featureExtraction.cpp
人无远虑，必有近忧
12-03 645
对每条线上大曲率进行排序，每条线分为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
qq_44305240的博客
09-24 350
LIO-SAM代码阅读——featureExtraction.cpp
《视觉slam十四讲》第七讲feature_extraction.cpp程序解释
lcmsir的博客
01-30 1962
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) 由于在书中直接省略了，
第七讲 feature_extraction.cpp
qq_40213457的博客
07-09 1071
#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代码（中文注释）
02-24
**LIO-SAM（LiDAR-Inertial ...通过阅读和理解LIO-SAM的源代码，你可以深入学习如何在实际项目中实现LiDAR和IMU的融合，以及如何构建高效、精确的SLAM系统。这对于无人车、无人机等移动机器人平台的开发至关重要。
LIO-SAM代码阅读详细注释版
11-16
代码注释提供了理解LIO-SAM算法流程和实现细节的重要线索。注释可能涵盖关键函数的作用、参数的意义以及优化策略。 需要注意的是，由于LIO-SAM的代码可能存在未解的问题或不稳定性，因此在实际应用时，需要根据...
LIO-SAM-DetailedNote:LIO-SAM源码详细注释，3D SLAM融合激光、IMU、GPS
05-08
LIO-SAM源码详细注释，3D SLAM融合激光、IMU、GPS，因子图优化。 LIO-SAM的代码十分轻量，只有四个cpp文件，很值得读一读呢。 关于LIO-SAM的论文解读，网上已经有很多文章啦，同系列的LOAM、A-LOAM、LEGO-LOAM等，在...
LIO-SAM代码解析-徐胜攀.pdf
最新发布
06-23
对LIO-SAM算法代码进行了深入分析，对各模块功能和联系进行梳理，对重要计算过程进行详细解读。读完本文档将可以清晰准确地了解LIO-SAM算法思想和实现。
LIO-SAM探秘第三章之代码解析（二） --- featureExtraction.cpp
李想的博客
11-09 2811
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 657
这一块代码和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和mapOptmization.cpp
qq_42731705的博客
11-13 939
本文是LIO-SAM论文与代码阅读笔记系列文章的一部分，所有博客如下：LIO-SAM论文与代码阅读笔记（一）论文阅读LIO-SAM论文与代码阅读笔记（一）论文阅读LIO-SAM论文与代码阅读笔记（一）论文阅读。
lio-sam之FeatureExtraction.cpp
weixin_49227107的博客
08-22 562
该cpp里就一个回调函数lasercloudinfohandler。
LIO-SAM源码解析-特征提取featureExtraction.cpp
chennuo0125的博客
12-19 1918
节点数据传递 从上图可以看出来，特征提取节点(红色)/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
LIO-SAM 源码阅读--featureExtraction.cpp(2)
chrishuimin的博客
12-07 148
SLAM
lio-sam算法整理＜2＞
共同学习
09-27 813
msg/cloud_info.msg cloud_info.msg 可以看到，这里面定夜猫子点云,imu的初始化,以及ringindex的相关msg. 在对应的CMake里面也加入 这个相当于是自定义的了，因此也需要有 msg_generation相关.在package.xml也要有对应的 src/imageProjection.cpp 这个代码开始定义了自定义的 结构体 VelodynePointXYZIRT 因为正常的pointcloud2里面没有这个类型的，因此这里进行了自定义. （后面
从零开始搭二维激光SLAM --- 使用单线雷达实现LIO-SAM中的特征点提取
李想的博客
12-03 6882
我们这篇文章将对LIO-SAM中特征点提取的部分进行二维激光雷达下的实现. LIO-SAM的项目地址为: https://github.com/TixiaoShan/LIO-SAM LIO-SAM的特征点提取部分与LOAM基本相同,只不过在算曲率值时的具体计算方式稍有不同. 1 特征点提取后的效果 我们首先看一下激光原始数据的样子,如下图所示: 接下来,再看一下经过我们进行特征点提取之后的数据点的样子,如下图所示: 可以看到,经过特征点提取之后的数据点呈现出不太直观的样子,但是分布还是停均匀的. 接下来
激光SLAM系统Fast LOAM (Lidar Odometry And Mapping)源码解析
秃头队长
12-22 2768
github地址：Fast LOAM (Lidar Odometry And Mapping) Fast LOAM提供了mapping和localization的两个节点，目前只使用其定位部分，以velodyne为例分析一下源码。 <node pkg="floam" type="floam_odom_estimation_node" name="floam_odom_estimation_node" output="screen"/> <node pkg="floam" t
FAST-LIO-SLAM
10-04
FAST-LIO-SLAM是一种激光SLAM算法，它在FAST-LIO的基础上引入了GNSS先验因子，以提高定位的精度和鲁棒性。在FAST-LIO-SLAM中，红色表示没有GNSS先验因子的FAST-LIO Map，蓝色表示包含GNSS先验因子的FAST-LIO-SAM。这种算法相比于其他经典的激光SLAM算法（如LOAM、LeGO-LOAM等），在解决定位问题上具有更好的性能和更少的局限性。
评论 2
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值