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

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

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

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

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

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

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

    //// 发布注册了角点点云和面点点云，并且“瘦身”之后的cloudInfo
    pubLaserCloudInfo = nh.advertise<lvi_sam::cloud_info> (PROJECT_NAME + "/lidar/feature/cloud_info", 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();
}