LVI-SAM LIS featureExtraction代码阅读_lvi-sam与r2live

对于当下多传感器融合框架，LVI-SAM、R2LIVE和R3LIVE成为团队研究的热点和重点，今天写一下featureExtraction.cpp的代码阅读。

目录

头文件

结构体

类

问题

头文件

#include "utility.h" // 包含了本系统中所需要的各种初始定义，位于本地文件夹下
#include "lvi_sam/cloud_info.h" // 点云的基本信息，也包含了imu 和 odom的成员变量

这个头文件包括了两个，一个是utility，另一个是cloud_info，这个在后面用的比较多，它是作者团队自己编写的。

结构体

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

使用两个结构体，第一个是用来表示平滑参数的（看的其他的文章，这里的smoothness有这么个翻译的方式）。另一个使用在sort函数里面作为第三个参数进行传递的，就是返回从小到大的排序

类

它继承了ParamServer类，下面说一下它的输入和输出，还有一些变量

ros::Subscriber subLaserCloudInfo; // 输入：点云信息
 
    // 输出：点云信息
    ros::Publisher pubLaserCloudInfo; // 用来完成发布不同的点云信息，激光帧提取特征之后的点云信息
    ros::Publisher pubCornerPoints; // 激光帧提取到的角点点云
    ros::Publisher pubSurfacePoints; // 激光帧提取到的平面点点云
 
    pcl::PointCloud<PointType>::Ptr extractedCloud; // pcl格式的提取点云 、 角点 、 平面点
    pcl::PointCloud<PointType>::Ptr cornerCloud;
    pcl::PointCloud<PointType>::Ptr surfaceCloud;
 
    pcl::VoxelGrid<PointType> downSizeFilter;
 
    //用来存储当前激光帧的全部信息，包括所有的历史数据
    lvi_sam::cloud_info cloudInfo;
    std_msgs::Header cloudHeader; // Header是std_msgs包下用来传递数据的点云信息
 
    //用来存储激光帧点云的曲率
    std::vector<smoothness_t> cloudSmoothness; // 平滑度 这个待确定
    float *cloudCurvature; // 曲率
    
    //有待确定，一个用来表示是否提取，一个用来表示提取的类型：角点还是平面点
    int *cloudNeighborPicked;
    int *cloudLabel;

上面的变量不详细地解释，ros：：表示是在ros作用域下面，pcl::表示是在pcl作用域下面，这个pcl是point cloud library的缩写，就是把点云搞成统一的格式。然后voxelgrid后面的downsizefilter，这个就是体素滤波，就是我把正方体里面的所有点云用它的重心来表示，这样数据量就大大减少。cloudheader里面存储的是头部信息，但是这个没太明白。下面就是平滑度，曲率啥的这个不需要看。但是曲率的计算这个cpp文件里面没看到。然后下面的话cloudNeighborPicked表示这个点是否选取，当值为1的时候说明这个点是遮挡点或者平行光束点，意思就是不再用这个点了。然后cloudLabel这个值，当它为1的时候，判定为角点。当它的值为-1的时候，判定为平面点。这个是从下面的函数里面看出来的，具体哪个函数，下面进行解释。

//构造函数，用来初始化
    //它们初始化了订阅者和发布者的信息，并且给上文所定义的各个变量进行初始化。
    FeatureExtraction()
    {
        //订阅当前激光帧运动畸变校正后的点云信息
        subLaserCloudInfo = nh.subscribe<lvi_sam::cloud_info>(PROJECT_NAME + "/lidar/deskew/cloud_info", 5, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
        // ros::Subscriber subLaserCloudInfo; 它的定义是在ros作用域下面的，那么它获取的数据最后需要在ros里面进行展示
 
        //发布当前激光帧提取特征之后的点云信息
        pubLaserCloudInfo = nh.advertise<lvi_sam::cloud_info> (PROJECT_NAME + "/lidar/feature/cloud_info", 5);
        //pubLaserCloudInfo = nh.advertice<lvi_sam::cloud_info> (PROJECT_NAME + "/lidar/feature/cloud_info", 5); 后面是发布的路径 sub订阅 pub发布 5是传给订阅者的最大传出信息数
        //发布当前激光帧的角点点云
        pubCornerPoints = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/feature/cloud_corner", 5);
        //发布当前激光帧的平面点点云
        pubSurfacePoints = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/feature/cloud_surface", 5);
        
        //调用初始化函数
        initializationValue();
    }

上面是构造函数，第一行订阅的时候，用到了handler函数，这个在下面进行解释，然后就是这个文件的输入输出，pub就是输出，sub就是输入。然后最后初始化。接下来讲一下初始化。

    // 初始化各个变量信息
    // 平滑度重新设定大小
    // 体素滤波的小方块重新设置大小
    // 提取点云 角点 平面点重新设置
    // 曲率 、 周围点是否被提取 、 是不是角点label这些内容也重新构建数组
    // 3.23 HuangY
    void initializationValue()
    {
        cloudSmoothness.resize(N_SCAN*Horizon_SCAN); // 这个resize就是重新划定大小的，这个SCAN一看就知道是雷达的扫描线
 
        // 体素滤波：就是简单理解成 我放一个正方体 处在这个正方体里面的点云用正方体的重心来表示即可 这样实现了降采样
        downSizeFilter.setLeafSize(odometrySurfLeafSize, odometrySurfLeafSize, odometrySurfLeafSize); // 设置体素大小的
 
        extractedCloud.reset(new pcl::PointCloud<PointType>()); // 这个reset函数就是重新设置里面的参数，括号里面的内容就是前面变量的类型，这个对齐即可
        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]; // 这个我认为是判断点云的周围几个点有没有被提取，如果使用过了（也就是提取过了）赋值为1， 没使用过就为0
        cloudLabel = new int[N_SCAN*Horizon_SCAN]; // 这个表示这个点是否是角点的
    }

这个就是初始化，里面的步骤在上面就已经详细地解释了，就是先初始化各个变量信息，然后重新设置大小，然后各种操作。

    void laserCloudInfoHandler(const lvi_sam::cloud_infoConstPtr& msgIn) // 在构造函数的第一个sub变量中使用，handler是个句柄，但是看见某些文章也说它是回调函数
    {
        // lvi_sam作用域下的点云信息mgsIn
        cloudInfo = *msgIn; // new cloud info 生成一个新的点云
        cloudHeader = msgIn->header; // new cloud header 在这里msgIn是一个引用，所以其成员header需要用->来表示
        pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction 提取出来有效点云，deskewed去歪斜
 
        calculateSmoothness(); // 直接函数名的意思，计算平滑度，看下面的函数主体
 
        markOccludedPoints();
 
        extractFeatures();
 
        publishFeatureCloud();
    }

这个函数就是我说的构造函数的第一行里面使用的参数里面的函数，这个的话就是下面有四个函数，这些函数就构成了这个cpp的功能，进行特征提取。对于这个特征提取的话，我现在的理解就是先计算平滑参数，然后标记遮挡点和平行光束点（因为这两类点影响雷达点云）。然后提取特征，就是提取角点和平面点，最后发布信息。
 

    // 这个地方就是在计算点的平滑参数的，要细看lio-sam
    void calculateSmoothness()
    {
        int cloudSize = extractedCloud->points.size(); // 先获取点云的大小
        //遍历所有的有效点云（extractedCloud）
        for (int i = 5; i < cloudSize - 5; i++)
        {
            //用当前激光点前后5个点计算当前点的曲率，因为是计算前后5个点云，所以在这里i从5开始
            // diffRange是这个点的pointRange 和 前后5个点的pointRange的差值
            // 这里的pointRange还不是很明白，可能是点云的深度？？？点的范围，我觉得可能是深度，毕竟这里是float浮点数，肯定不是个范围，是个数值 3.25
            // 平滑性采用的方法为将当前点的距离与前后各5点计算误差累计和；显然误差累计和较大，则此点属于跳跃点，或边缘点； 通过这里，这个部分还得看lio-sam里面的内容
            // 这里的pointRange，表示深度，在下面的函数里面，它赋值给了深度！ 3.25解决
            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;
        }
    }

这个就是计算平滑参数的，这里有个疑问，就是这个一维索引来存储点云到底是怎么存储的？？？感觉很迷。然后这里的pointRange的理解也不是很好，我通过下面的代码，发现是深度。但是在判断平行光束的时候，要是深度的话。。。我不理解怎么判断了。。。还有这个curvature的计算，也是很迷，我不明白为什么是相加再相乘以后就直接dx*dx + dy*dy +dz*dz了

    //用来标记两种不同的点，分别是被遮挡的 和 被标记的。
    void markOccludedPoints() // Occluded 遮挡的
    {
        int cloudSize = extractedCloud->points.size(); // 通用操作，先获取大小
        // mark occluded points and parallel beam points 标记遮挡点 和 平行光束点
        for (int i = 5; i < cloudSize - 6; ++i)
        {
            // occluded points 这部分来标记遮挡点，从i = 5就知道要前后5个来进行操作。这里对为什么是前后5个 而不是6个 7个表示疑问 3.25 还是得读论文lio-sam
            float depth1 = cloudInfo.pointRange[i]; // 这里的pointRange 推测，应该就是深度
            float depth2 = cloudInfo.pointRange[i+1];
            int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i])); // 这里用pointColInd来计算columnDiff,然后进入下面的判断
 
            if (columnDiff < 10)
            {
                // 10 pixel diff in range image
                // 标记成1的 就是屏蔽的点
                // 参考地址：https://www.daimajiaoliu.com/daima/4ed329fe7900408
                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
            // pointRange就是深度值，对于一维数组的索引的话 可以是左右 也可以是前后 这个问题暂留
            float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));
            float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));
 
            // 平行光束，即激光打上去之后，物体是和激光在统一平面上，即它的深度无法知道，这个就叫平行光束
            // 这里的判断条件，就是看第i个点，左右两个i-1 和 i+1他们之间的左右距离进行判断
            // 感觉这里的pointRange可以是深度，就是它的范围值，来计算的。这个pointRange暂时没搞清楚
            if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
                cloudNeighborPicked[i] = 1;
            // 到这里基本结束这个遮挡点 和 平行光束点的判断。赋值为1的NeighborPicked感觉后面应该是不会使用的，再进行验证  3.25
 
        }
    }

这里的话就是来标记遮挡点和平行光束点的。上面注释的很详细不过多展开。

    //特征提取，最核心的函数
    void extractFeatures()
    {
        //首先清除原来的信息，并创建指针，用来存放平面点 和 降采样以后的平面点
        cornerCloud->clear();
        surfaceCloud->clear();
        // clear()就是清楚里面的变量的值，并且resize一下。
        // cornerCloud->clear();
        // surfaceCloud->clear();
 
        pcl::PointCloud<PointType>::Ptr surfaceCloudScan(new pcl::PointCloud<PointType>()); // 平面点云扫描
        pcl::PointCloud<PointType>::Ptr surfaceCloudScanDS(new pcl::PointCloud<PointType>()); // 平面点云扫描 然后DownSample一下
 
        for (int i = 0; i < N_SCAN; i++)
        {
            //N_SCAN 是在utility.h中定义的变量，这个是用来存储雷达中的数据：扫描线
            //因此第一个for循环的作用是遍历所有的扫描线。
            surfaceCloudScan->clear(); // 先清除surfaceCloudScan里面的值
 
            for (int j = 0; j < 6; j++)
            {
                //这里的6是一个常量，是作者用来分段的数目，将每一个SCAN分为六段来进行分析
 
                //用线性插值对SCAN进行等分，取得sp和ep
                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; // 就结束下面的内容，然后j++。所以需要sp < ep 才能执行下面的内容
 
                std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value()); // 排序 by_value()表示这个sort需要从小到大进行排序
 
                int largestPickedNum = 0;
                for (int k = ep; k >= sp; k--)
                {
                    int ind = cloudSmoothness[k].ind; // 是一个标签，为了避免重复访问点云，就是index
                    if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)
                    {
                        //判断条件：当前激光点还未被处理，且曲率大于边的阈值，则认为是角点 neighborpicked见上面的那个函数，等于0是好的点，等于1是遮挡点和平行光束点中的一个
                        //这里的edgeThreshold默认值为0.1
                        largestPickedNum++;
                        if (largestPickedNum <= 20)
                        {
                            //在每一段中只提取出来20个点，然后将其标记为角点，加入角点点云
                            cloudLabel[ind] = 1; // 推测这里的cloudLabel = 1就表示这个点是角点
                            cornerCloud->push_back(extractedCloud->points[ind]);
                            // 还有这里的push_back()函数有专门地写它，可以自行转到定义去看
                            // cornerCloud->push_back(extractedCloud->points[ind]);
                            // 这里的变化的量是ind，是点的index
                        }
                        else
                        {
                            break;
                        }
 
                        cloudNeighborPicked[ind] = 1; // 在这里 这个点经过上面的if以后，进行赋值，让它 == 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) // 这个10 在上面的遮挡点的if里面 可以去看
                                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前后5个点，然后是不是遮挡点的，因为根据里面的if的条件，初步感觉是在判断遮挡点的，如果没有break，则赋值为1，和上面的遮挡点判断的那个函数效果相同
                    }
                }
 
                for (int k = sp; k <= ep; k++)
                {
                    int ind = cloudSmoothness[k].ind; // 提取index
                    if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold)
                    {
                        // 判断条件：当前激光点还未被处理，且曲率小于平面阈值，这里的surfThreshold应该也是0.1
                        cloudLabel[ind] = -1; // 不是角点，赋值成-1，根据293行的if条件，可以知道，这个为平面点，平面点：赋值成-1
                        cloudNeighborPicked[ind] = 1; // 然后这个neighborpicked赋值成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和上面的两个for效果一样，就是在判断是否是遮挡点，然后赋值成1，就不再进行操作了
                    }
                }
                // 这个for循环是用来进行平面点云的添加的
                for (int k = sp; k <= ep; k++)
                {
                    if (cloudLabel[k] <= 0)
                    {
                        surfaceCloudScan->push_back(extractedCloud->points[k]);
                    }
                }
                // 总结：角点：ep -> sp进行循环 从大到小
                // 平面点:sp -> ep进行循环 从小到大 然后if条件里面的参数要看清楚
            }
 
            // 一个降采样的操作，把surfaceCloudScan -> surfaceCloudScanDS -> surfaceCLoud
            surfaceCloudScanDS->clear();
            downSizeFilter.setInputCloud(surfaceCloudScan); // 降采样
            downSizeFilter.filter(*surfaceCloudScanDS);
 
            *surfaceCloud += *surfaceCloudScanDS;
        }
    }

然后这个就是featureExtraction的主要部分，就是提取角点和平面点的。这里不明白的是sp和ep的取值方法，看到别人说的是线性插值，可能还需要回归lio-sam论文的阅读了。然后对平滑参数进行排序，这里的平滑参数，好吧，读懂代码了，平滑参数的值他就是曲率，曲率的计算不太明白。好了，这个地方没问题了。然后接着对角点进行判断，从大到小进行for循环。里面的内容不过多展开。然后是平面点的提取，这里使用了两个循环，我觉得是不是可以合成一个呢，因为第三个for循环，他判断cloudlabel<0就push进surfacecloud里面了。最后就是降采样，这三行函数希望自己牢牢记在心里！

    void freeCloudInfoMemory() // 清理点云信息
    {
        // cloudinfo是lvi_sam：：作用域下的
        // 对startRingIndex endRingIndex pointColInd pointRange进行先clear 再 shrink to fit函数操作
        cloudInfo.startRingIndex.clear();
        cloudInfo.startRingIndex.shrink_to_fit(); // 这里的shrink_to_fit()函数的作用就是看这个容器的大小，如果你本来装10个的，但是你申请了100个的空间，那么这个函数执行以后，你的空间大小变成了10个
        cloudInfo.endRingIndex.clear();
        cloudInfo.endRingIndex.shrink_to_fit();
        cloudInfo.pointColInd.clear();
        cloudInfo.pointColInd.shrink_to_fit();
        cloudInfo.pointRange.clear();
        cloudInfo.pointRange.shrink_to_fit();
    }
 
    void publishFeatureCloud() // 发布点云信息
    {
        //先清理点云信息，再保存好新提取到的点云信息，然后传送给图优化函数mapOptimization
        // free cloud info memory
        freeCloudInfoMemory();
        // save newly extracted features 保存了上面提取的特征
        cloudInfo.cloud_corner  = publishCloud(&pubCornerPoints,  cornerCloud,  cloudHeader.stamp, "base_link"); // 这个肯定就是publishCloud以后返回了一个值
        cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, "base_link");
        // publish to mapOptimization 这些给了图优化，现在的重中之重，待解决：图优化到底输入了些啥
        pubLaserCloudInfo.publish(cloudInfo);
    }

这个没啥好说的，解释一下shrink to fit这个就是你申请的空间大了，系统自动给你缩小空间的。

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lidar"); // 这个是ros的初始化，但是有时候节点的名字会被launch给覆盖，这个不需要过多纠结
 
    FeatureExtraction FE; // 这个是这个类的实体
 
    ROS_INFO("\033[1;32m----> Lidar Feature Extraction Started.\033[0m"); // 这个是发消息的，使用roslaunch的时候会在terminal里面看见它
   
    ros::spin();
    // 这句话的意思是循环且监听反馈函数（callback）。 懂了，这个函数就是写在这里调用callback的，具体哪个还需要再进一步深究
    // 循环就是指程序运行到这里，就会一直在这里循环了。监听反馈函数的意思是，如果这个节点有callback函数，那写一句ros::spin()在这里，
    // 就可以在有对应消息到来的时候，运行callback函数里面的内容。
    // 就目前而言，以我愚见，我觉得写这句话适用于写在程序的末尾（因为写在这句话后面的代码不会被执行），适用于订阅节点，且订阅速度没有限制的情况。
 
    return 0;
}

然后这个主函数的话，一般套路，先ros::init，再创建个类的实体，然后ros上面发布个信息，里面的函数的话就是创建实体的时候运行完了。最后这个ros::spin()也是第一次见，然后查阅完资料以后恍然大悟。

到此这部分结束，对现有的问题进行一个总结：

问题

1、一维索引怎么存储点云的

2、pointRange的理解，平行光束点判断的那个部分

3、曲率的计算

4、发布给图优化的那部分也没太懂