LVI：激光雷达子系统的特征提取梳理

一、节点关系

回顾节点关系图：

上一篇文章详细分析了imageProjection节点，该节点订阅了原始点云话题、imu原始测量话题、以及VIS的里程计话题，发布了预处理（过滤无效点、有序化、去畸变）之后的点云话题cloud_deskewed和cloud_info（其中cloud_deskewed话题是普通的PointCloud2的消息类型，cloud_info话题是自定义格式的消息类型）。

观察节点关系图可以看到cloud_deskewed话题被可视化节点lvi_sam_rviz和VIS视觉特征节点visual_feature订阅，因此本文我们按照cloud_info话题的走向，分析特征提取节点featureExtraction。

deskewed/cloud_info话题只被特征提取节点订阅，并且特征提取节点只有feature/cloud_info一个话题被其他节点订阅。

二、自定义格式消息类型cloud_info

# Cloud Info
Header header   ## 标头


int32[] startRingIndex  ## 点云的第i条扫描线（ring）上的第一个可计算曲率的点
int32[] endRingIndex    ## 点云的第i条扫描线（ring）上的最后一个可计算曲率的点


int32[]  pointColInd # point column index in range image ## 点云中每个点在投影图片中的列序号
float32[] pointRange # point range ## 点云中每个点与LiDAR的距离，即投影图片的像素值


int64 imuAvailable  ## imu的旋转测量是否对齐到LiDAR，若对齐说明imu旋转测量可用
int64 odomAvailable ## 是否有与当前帧最近的两个相邻帧之间的位姿变换可用


# Attitude for lidar odometry initialization
## 可用的imu旋转测量，作为LIS帧间位姿变换的预测值
float32 imuRollInit
float32 imuPitchInit
float32 imuYawInit


# Odometry
## 可用的相邻帧位姿变换，同样作为LIS帧间位姿变换的预测值
float32 odomX
float32 odomY
float32 odomZ
float32 odomRoll
float32 odomPitch
float32 odomYaw


# Odometry reset ID
## 从里程计获得的位姿变换协方差的取整（四舍五入），可以用于计算该位姿变换值的可信度
int64 odomResetId


# 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 面点组成的点云

三、主函数

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


     与imageProjection相似，核心工作都由FeatureExtraction类的构造函数完成
    FeatureExtraction FE;


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


    return 0;
}

四、FeatureExtraction类

4.1 父类ParamServer类

在上一篇文章中已经说明过ParamServer类了，其主要作用就是设置好各项参数，并留有imu测量数据坐标变换到LiDAR的接口。

4.2 FeatureExtraction类使用到的结构体

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

4.3 FeatureExtraction类成员概览

4.4 FeatureExtraction类的几个优秀函数

我们重点看一些比较优秀的做法，这也许沿袭自LeGO-LOAM和LIO-SAM，我们不必计较其来源。

4.4.1 calculateSmoothness() 计算曲率

LOAM计算曲率的方式：

LVI-SAM计算曲率的方式：

void calculateSmoothness()
    {
        int cloudSize = extractedCloud->points.size();
        for (int i = 5; i < cloudSize - 5; i++)
        {
             采用距离来计算曲率，这是对之前imageProjection节点计算过的距离信息再次利用，提高信息利用率和计算效率。
            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;
        }
    }

4.4.2 markOccludedPoints() 遮挡点标记

图(a)为平行光束，图(b)为遮挡点。这两种情况都会导致本来是平面，但是误以为是曲率较高的角点，因此在计算曲率之前，需要先将这些点标记上，防止被误提取。

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];
        float depth2 = cloudInfo.pointRange[i+1];
        int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));


         相近点：在有序点云中顺序相邻，并且在距离图像上的列序号之差小于10
        if (columnDiff < 10){
            // 10 pixel diff in range image
             两个相近的点与LiDAR的距离之差大于0.3m，则认为它们是互相遮挡的点
             标记后景点，防止把遮挡点当做角点
            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
         理论上物体距离越远时，点云才应该越稀疏，相邻点距离较远
         但是对于平行于激光光束的表面来说，即使距离激光雷达很近，依然会导致两个相邻点很远
         所以当相邻点距离之差大于0.02倍距离时，认为它是平行于激光光束的平面，排除掉该点
        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;
    }
}

4.4.3 freeCloudInfoMemory() 释放自定义点云内存

cloud_info是LVI-SAM自定义格式的点云，包含了大量的数据内容，需要占用大量的内存空间。

imageProjection节点把原始的无序点云，进行了去畸变和有序化，并且设置了大量的索引用来方便遍历查找特征点，还保留了距离信息用来筛除一些特殊点云点。

这些数据对于featureExtraction节点是必要的，但是对于后续的过程没有意义，而且重新创建一个cloud_info实例是不方便的，因此在发布话题之前，lvi-sam对cloudInfo进行了一次“瘦身”，以提高后面的运行效率。

void freeCloudInfoMemory()
{
    cloudInfo.startRingIndex.clear();
    cloudInfo.startRingIndex.shrink_to_fit(); 释放数组的capacity
    cloudInfo.endRingIndex.clear();
    cloudInfo.endRingIndex.shrink_to_fit();
    cloudInfo.pointColInd.clear();
    cloudInfo.pointColInd.shrink_to_fit();
    cloudInfo.pointRange.clear();
    cloudInfo.pointRange.shrink_to_fit();
}

这里用到了shrink_to_fit()函数来释放数组的capacity，因为仅仅使用clear()函数的话只会改变数组的size，具体何时释放内存我们并不知道，而使用shrink_to_fit()函数可以明确地、主动地释放数组内存。

4.5 FeatureExtraction类的构造函数

最后我们重新看一遍FeatureExtraction类的构造函数，完成本篇的总结。

FeatureExtraction()
{
     订阅去畸变、有序化的自定义格式点云cloudInfo，回调函数负责去除干扰点，根据曲率提取角点和平面点
    subLaserCloudInfo = nh.subscribe(PROJECT_NAME + "/lidar/deskew/cloud_info", 5, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());


     发布注册了角点点云和面点点云，并且“瘦身”之后的cloudInfo
    pubLaserCloudInfo = nh.advertise (PROJECT_NAME + "/lidar/feature/cloud_info", 5);
     发布角点点云和面点点云
    pubCornerPoints = nh.advertise(PROJECT_NAME + "/lidar/feature/cloud_corner", 5);
    pubSurfacePoints = nh.advertise(PROJECT_NAME + "/lidar/feature/cloud_surface", 5);
    
     重置所有参数，设置体素化滤波器分辨率
    initializationValue();
}

本文仅做学术分享，如有侵权，请联系删文。

3D视觉精品课程推荐：

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论+源码+实践]

10.单目深度估计方法：算法梳理与代码实现

11.自动驾驶中的深度学习模型部署实战

12.相机模型与标定(单目+双目+鱼眼）

13.重磅！四旋翼飞行器：算法与实战

14.ROS2从入门到精通：理论与实战

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~