LIO-SAM代码学习——featrueExtraction.cpp

最新推荐文章于 2024-06-19 17:40:02 发布

hit_Simon_auto

最新推荐文章于 2024-06-19 17:40:02 发布

阅读量372

点赞数

分类专栏： slam 文章标签：学习自动驾驶 c++

本文链接：https://blog.csdn.net/qq_44043868/article/details/123409344

版权

slam 专栏收录该内容

5 篇文章 0 订阅

订阅专栏

featrueExtraction.cpp（特征提取线程）

主函数

初始化特征提取类，并等待响应消息到来。

int main(int argc, char **argv)
{
    ros::init(argc, argv, "lio_sam"); // 初始化ROS节点
    FeatureExtraction FE; // 初始化特征提取类
    ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m");
    ros::spin(); // 等待数据
    return 0;
}

FeatureExtraction 特征提取类

一、成员变量

public:
    ros::Subscriber subLaserCloudInfo; // 雷达点云信息订阅器

    ros::Publisher pubLaserCloudInfo;
    ros::Publisher pubCornerPoints;  // 角点特征发布器
    ros::Publisher pubSurfacePoints; // 平面点特征发布器

    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;

    std::vector<smoothness_t> cloudSmoothness; // 点云顺滑性缓存器(每个元素包含点的曲率和索引)
    float *cloudCurvature;                     // 点云中点的曲率
    int *cloudNeighborPicked;
    int *cloudLabel;

二、构造函数 FeatureExtraction()

初始化订阅者和发布者

订阅者（s）or发布者（p）	作用	节点	数据类型	回调函数
subLaserCloudInfo（s）	接收imageProjection进程处理后发布的有效的激光点云信息(包括每根线的起始和结束点索引、点深度、列索引)	“lio_sam/deskew/cloud_info”	lio_sam::cloud_info	FeatureExtraction::laserCloudInfoHandler
pubLaserCloudInfo（p）	发布经过处理后的特征点云（线特征和面特征）	“lio_sam/feature/cloud_info”	lio_sam::cloud_info
pubCornerPoints（p）	发布经过处理后的线特征点云（角点）	“lio_sam/feature/cloud_corner”	sensor_msgs::PointCloud2
pubSurfacePoints（p）	发布经过处理后的面特征点云（面点）	“lio_sam/feature/cloud_surface”	sensor_msgs::PointCloud2

初始化变量

调用initializationValue()对变量进行初始化。

    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();
    }

三、void initializationValue()

对进程处理过程中所用到的变量和滤波器进行初始化。

    void initializationValue()
    {
        cloudSmoothness.resize(N_SCAN * Horizon_SCAN); // 重置矢量大小为 雷达线数*水平扫描数

        downSizeFilter.setLeafSize(odometrySurfLeafSize, odometrySurfLeafSize, odometrySurfLeafSize); // 设置体素滤波器方格大小

        // 通过reset方法使智能指针指向新的对象
        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]; // 分配空间给指向角点面点标志数组的指针
    }

四、void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr &msgIn)

接收imageProjection进程处理后发布的有效的激光点云信息的订阅者回调函数。
取出消息中的有效点云数据信息，计算每个点云的曲率；判断该点云是否可靠（有无出现和激光线平行或遮挡的情况），若有则进行标记；对可靠点云进行特征提取；发布特征点云信息。

    void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr &msgIn)
    {
        cloudInfo = *msgIn;                                      // 新点云信息数据
        cloudHeader = msgIn->header;                             // 新点云头信息数据的消息头
        pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // 将新点云消息中有效点云部分转换成可处理的点云数据结构（PCL格式，包含XYZI信息）
        // 计算点云中点的曲率
        calculateSmoothness();
        // 标记遮挡点和平行光束点，避免后面进行错误的特征提取
        markOccludedPoints();
        // 特征提取(平面点和角点)
        extractFeatures();
        // 发布特征点信息
        publishFeatureCloud();
    }

五、void calculateSmoothness()

点云曲率计算函数。
此处曲率的定义：此处曲率定义为当前点与其左右10个空间最近邻点（同一线上相邻的点）深度值之差的累加和的平方，为一个判断是否为角点或面点的指标。若为角点，则该点与其相邻点之间的深度差别将较大，曲率较大；若为面点，则该点与其相邻点之间的深度差别将较小，曲率较小。
相邻点标志位cloudNeighborPicked：为了防止特征点过多，会将特征点周围一定范围内的点通过置位标志位标记为相邻点（视为已查找点），在后续特征点寻找过程中将不会查找带有相邻点标记的点，以避免在大致相同位置取出多个特征点。
特征点标志位cloudLabel：用于标记该点为角点或面点或普通非特征点。
在imageProjection进程中处理完点云后，extractedCloud指向的有效点云数据集合在队列中的索引相邻的两个点在空间上也是可观测到的最近邻点。
计算有效点云数据集合当中除首尾5个点以外其他点的曲率，将曲率值和点索引保存到数组cloudSmoothness[]中，并将该点的相邻点标志位和特征点标志位置0。

    void calculateSmoothness()
    {
        int cloudSize = extractedCloud->points.size(); // 取得点云中点的数量
        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[i].value = cloudCurvature[i];
            cloudSmoothness[i].ind = i;
        }
    }

六、void markOccludedPoints()

标记遮挡点和平行光束点，避免后面进行错误的特征提取。
遍历有效点，寻找不可靠的点；
若相邻两点的列索引差较小，且深度差较大，则认为有可能发生遮挡，向深度较大的方向数5个点，标记为已处理点，之后不对该部分点进行处理；
若某个点与其相邻两个点的深度差均较大，则认为该点可能是平行于激光束的点，标记为已处理点，之后不对该点进行处理。

    void markOccludedPoints()
    {
        int cloudSize = extractedCloud->points.size();
        // 标记遮挡点和平行光束点
        for (int i = 5; i < cloudSize - 6; ++i)
        {
            // 获取相邻两点之间的列索引差
            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)
            {
                // 如果深度值差的较大，则将相邻的几个5个点认为是遮挡点，并标记为已选择过，后面不会再对这些点进行特征提取
                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;
                }
            }
            // 获取当前点与左右相邻点之间的深度差
            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;
        }
    }

七、void extractFeatures()

特征提取函数。
首先清空角点云和面点云存储结构；
将每一行数据分成六段，每段起始和结束索引如下：

startpointID = startID + j*(endID - startID)/6；
endpointID = startID + (j+1)*(endID - startID)/6 - 1

确保每一段的起始索引小于结束索引，按行按段依次遍历部分点云中的每一个点，对每一部分遍历前先使用sort()对该部分队列数据根据by_value()基于曲率值进行排序，将曲率大的元素放到队尾，曲率小的元素放到队首，再提取角点和面点，并对面点进行降采样，最后保存到相应的数据结构当中。

提取角点

从每一段的最后一个点（曲率最大的点）开始搜索角点，如果当前点没有被标记为已搜索点，即cloudNeighborPicked[ind] == 0，且曲率值大于角点阈值，且当前段中角点数目小于20，则将当前点标记为角点，把该点加入到角点集合cornerCloud当中，并将段中角点数目largestPickedNum加一；
为了防止提取的角点过于密集，计算当前角点左右五个点与该角点的列索引之差，若差值较小则认为两个点相距较近，则直接标记为已搜索点，不对此类点进行角面点分类。

提取面点

从每一段的第一个点（曲率最小的点）开始搜索面点，如果当前点没有被标记为已搜索点，即cloudNeighborPicked[ind] == 0，且曲率值小于面点阈值，则将当前点标记为面点，并标记当当前点为已搜索点；
为了防止提取的角点过于密集，计算当前角点左右五个点与该角点的列索引之差，若差值较小则认为两个点相距较近，则直接标记为已搜索点，不对此类点进行角面点分类；
将标记为面点的点加入到面点集合surfaceCloudScan当中，对面点集合中的点进行降采样，将的降采样后的点集加入到最终的面点云集合surfaceCloud当中。

    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();

            // 将每根线均分成6段，然后分别对每一段进行特征提取
            for (int j = 0; j < 6; j++)
            {

                // 计算每段的起始和结束点索引
                int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;            // startpointID = startID + j*(endID - startID)/6
                int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1;  // endpointID = startID + (j+1)*(endID - startID)/6 - 1 

                if (sp >= ep)
                    continue;

                // 对每段点云数据依据曲率进行由小到大排序
                std::sort(cloudSmoothness.begin() + sp, cloudSmoothness.begin() + ep, by_value());

                int largestPickedNum = 0; // 当前得到的角点数目
                // 由于sp->ep的点云已经按曲率有小到大排序过，此处从ep开始检索，意味着从曲率最大点开始检索
                for (int k = ep; k >= sp; k--)
                {
                    int ind = cloudSmoothness[k].ind; //读取当前检索点对应的索引
                    if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)
                    {
                        // 当点的曲率超过设定的阈值，则认为是角点，并缓存
                        largestPickedNum++;
                        if (largestPickedNum <= 20) // 每段最多提取20个角点
                        {
                            cloudLabel[ind] = 1;
                            cornerCloud->push_back(extractedCloud->points[ind]);
                        }
                        else
                        {
                            break; // 跳出本次循环，开始处理下一个点
                        }

                        // 对当前检索点的左右各5个相邻点进行列索引判断，如果列索引靠的比较近，则将这些相邻点置为选择过的，这样就可以保证不在这些点处提取角点，避免了角点分布过于密集
                        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;
                        }
                    }
                }

                // 由于sp->ep的点云已经按曲率有小到大排序过，此处从sp开始检索，意味着从曲率最小点开始检索
                for (int k = sp; k <= ep; k++)
                {
                    int ind = cloudSmoothness[k].ind; // 提取点索引
                    if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold)
                    {
                        // 如果当前检索点曲率小于给定的阈值，则对该点进行标记，认为该点为平面点(cloudLabel <= 0为平面点)
                        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;
                        }
                        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]);
                    }
                }
            }

            // 对平面点进行降采样
            surfaceCloudScanDS->clear();
            downSizeFilter.setInputCloud(surfaceCloudScan);
            downSizeFilter.filter(*surfaceCloudScanDS);
            // 保存降采样后的平面点
            *surfaceCloud += *surfaceCloudScanDS;
        }
    }

八、void freeCloudInfoMemory()

清空变量并释放内存。

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

九、void publishFeatureCloud()

调用函数释放内存并发布角点、面点和交点面点原数据点综合信息。

    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); // 发布特征点云信息
    }

数据结构

lio_sam::cloud_info

# Cloud Info
Header header 

int32[] startRingIndex # 有效点云数组第i行起始有效计算数据索引
int32[] endRingIndex # 有效点云数组第i行最后有效计算数据索引

int32[]  pointColInd # point column index in range image 有效点云列索引
float32[] pointRange # point range 有效点云深度信息

int64 imuAvailable # IMU姿态估计信息可用标志位
int64 odomAvailable # IMU里程计位姿估计信息可用标志位

# Attitude for LOAM initialization IMU得到的雷达帧初始姿态估计值
float32 imuRollInit
float32 imuPitchInit
float32 imuYawInit

# Initial guess from imu pre-integration 雷达帧初始位姿估计值
float32 initialGuessX
float32 initialGuessY
float32 initialGuessZ
float32 initialGuessRoll
float32 initialGuessPitch
float32 initialGuessYaw

# Point cloud messages
sensor_msgs/PointCloud2 cloud_deskewed  # original cloud deskewed 偏斜校正后的点云
sensor_msgs/PointCloud2 cloud_corner    # extracted corner feature 角点云
sensor_msgs/PointCloud2 cloud_surface   # extracted surface feature 面点云

sensor_msgs/PointCloud2

std_msgs/Header header
uint32 height
uint32 width
sensor_msgs/PointField[] fields
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense

sensor_msgs/PointField[]

uint8 INT8    = 1
uint8 UINT8   = 2
uint8 INT16   = 3
uint8 UINT16  = 4
uint8 INT32   = 5
uint8 UINT32  = 6
uint8 FLOAT32 = 7
uint8 FLOAT64 = 8

string name      # Name of field
uint32 offset    # Offset from start of point struct
uint8  datatype  # Datatype enumeration, see above
uint32 count     # How many elements in the field

点云库PCL相关知识

pcl::VoxelGrid<T>

PointType类型在utility.h中定义为pcl::PointXYZI的别名，pcl::PointXYZI的成员：float x, y, z, intensity; 表示XYZ信息加上强度信息的类型。
pcl::VoxelGrid为降采样器，此处定义的将采样器可以处理PointType类型的点云。
使用setLeafSize方法来设置降采样是建立体素地图的三维体素方格大小，此处为边长为odometrySurfLeafSize的立方体，降采样会将体素方格中的点集中到体素方格所有点的重心位置。

pcl::VoxelGrid<PointType> downSizeFilter; // 定义处理PointType类型点云的降采样器
downSizeFilter.setLeafSize(odometrySurfLeafSize, odometrySurfLeafSize, odometrySurfLeafSize); // 设置体素方格大小
downSizeFilter.setInputCloud(surfaceCloudScan); // 设置要进行体素滤波的点云
downSizeFilter.filter(*surfaceCloudScanDS); // 对点云进行滤波

pcl::PointCloud<T>

表示T类型点云的PCL类，包括以下成员：

变量	含义
width	对于有序点云，为点云2D图像（将3D点云展开为2D平面形式）的宽度（列数）；对于无序点云，为点云总数
height	对于有序点云，为点云2D图像（将3D点云展开为2D平面形式）的高度（行数）；对于无序点云，该值为1
points	点云数据数组（元素类型为T的vector）
is_dense	点云是否为有限的
sensor_origin_	指定传感器采集姿势（原点/平移）。为可选参数
sensor_orientation_	指定传感器采集姿势（旋转）。为可选参数

hit_Simon_auto

关注

0
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
LIO-SAM代码学习——featrueExtraction.cpp

featrueExtraction.cpp（特征提取线程）主函数int main(int argc, char **argv){ ros::init(argc, argv, "lio_sam"); // 初始化ROS节点 FeatureExtraction FE; // 初始化特征提取类 ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m"); ros::spin(); // 等待数据
复制链接

扫一扫

专栏目录