R2LIVE代码学习记录（3）：对雷达特征提取

main读取配置参数声明订阅、发布话题

int main(int argc, char **argv)
{
    
    ros::init(argc, argv, "feature_ext");
    ros::NodeHandle n;
    // 从launch读取配置参数
    n.param<double>("feature_extraction/blind", blind, 0.01);                    // 距离最小值，用于剔除重复点，当当前点与后一个点距离的平方小于该阈值，则剔除
    n.param<double>("feature_extraction/inf_bound", inf_bound, 10);              // 无穷值阈值，超出该阈值则认为是无穷值
    n.param<int>("feature_extraction/N_SCANS", N_SCANS, 1);                      // 扫描线数
    n.param<int>("feature_extraction/group_size", group_size, 8);                // 激光点云特征提取中，用于确实是否分组，点数大于该阈值则分组
    n.param<double>("feature_extraction/disA", disA, 0.01);                      // 平面点判断算法中，用于增大点计算过程中的搜索范围，相乘的系数
    n.param<double>("feature_extraction/disB", disB, 0.1);                       // 平面点判断算法中，用于增大点计算过程中的搜索范围，相加的稀疏
    n.param<double>("feature_extraction/p2l_ratio", p2l_ratio, 400);             // 用于判断是否为平面点的阈值，如果小于该阈值，则该点是待判断点
    n.param<double>("feature_extraction/limit_maxmid", limit_maxmid, 9);         // 检测平面点中的阈值
    n.param<double>("feature_extraction/limit_midmin", limit_midmin, 16);        // 检测平面点中的阈值
    n.param<double>("feature_extraction/limit_maxmin", limit_maxmin, 3.24);      // 检测平面点中的阈值
    n.param<double>("feature_extraction/jump_up_limit", jump_up_limit, 175.0);   // 角度上限
    n.param<double>("feature_extraction/jump_down_limit", jump_down_limit, 5.0); // 角度下限
    n.param<double>("feature_extraction/cos160", cos160, 160.0);
    n.param<double>("feature_extraction/edgea", edgea, 3);    // 有跨越的边判断算法中，距离阈值，相乘的系数
    n.param<double>("feature_extraction/edgeb", edgeb, 0.05); // 有跨越的边判断算法中，距离阈值，相加的稀疏
    n.param<double>("feature_extraction/smallp_intersect", smallp_intersect, 170.0);
    n.param<double>("feature_extraction/smallp_ratio", smallp_ratio, 1.2);
    n.param<int>("feature_extraction/point_filter_num", point_filter_num, 4);
    n.param<int>("feature_extraction/point_step", g_point_step, 3);
    n.param<int>("feature_extraction/using_raw_point", g_if_using_raw_point, 1);

    jump_up_limit = cos(jump_up_limit / 180 * M_PI);
    jump_down_limit = cos(jump_down_limit / 180 * M_PI);
    cos160 = cos(cos160 / 180 * M_PI);
    smallp_intersect = cos(smallp_intersect / 180 * M_PI);

    ros::Subscriber sub_points = n.subscribe("/velodyne_points", 1000, velo16_handler, ros::TransportHints().tcpNoDelay());
    pub_full = n.advertise<sensor_msgs::PointCloud2>("/laser_cloud", 100);       // 所有激光点云
    pub_surf = n.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);  // 平坦的平面点
    pub_corn = n.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100); // 锐利的边缘点

    ros::spin();
    return 0;
}

回调函数：

int orders[16] = {
    0, 2, 4, 6, 8, 10, 12, 14, 1, 3, 5, 7, 9, 11, 13, 15};

void velo16_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
    
    pcl::PointCloud<PointType> pl_orig;
    pcl::fromROSMsg(*msg, pl_orig);
    uint plsize = pl_orig.size();

    vector<pcl::PointCloud<PointType>> pl_buff(N_SCANS);
    vector<vector<orgtype>> typess(N_SCANS);
    pcl::PointCloud<PointType> pl_corn, pl_surf, pl_full;

    int scanID;
    int last_stat = -1;
    int idx = 0;

    for (int i = 0; i < N_SCANS; i++)
    {
    
        pl_buff[i].resize(plsize);
        typess[i].resize(plsize);
    }

    for (uint i = 0; i < plsize; i++)
    {
    
        PointType &ap = pl_orig[i];
        double leng = sqrt(ap.x * ap.x + ap.y * ap.y);
        if (leng < blind)
        {
    
            continue;
        }

        double ang = atan(ap.z / leng) * rad2deg;
        scanID = int((ang + 15) / 2 + 0.5);

        if (scanID >= N_SCANS || scanID < 0)
        {
    
            continue;
        }

        if (orders[scanID] <= last_stat)
        {
    
            idx++;
        }

        pl_buff[scanID][idx].x = ap.x;
        pl_buff[scanID][idx].y = ap.y;
        pl_buff[scanID][idx].z = ap.z;
        pl_buff[scanID][idx].intensity = ap.intensity;
        typess[scanID][idx].range = leng;
        last_stat = orders[scanID];
    }

    idx++;

    for (int j = 0; j < N_SCANS; j++)
    {
    
        pcl::PointCloud<PointType> &pl = pl_buff[j];
        vector<orgtype> &types = typess[j];
        pl.erase(pl.begin() + idx, pl.end());
        types.erase(types.begin() + idx, types.end());
        plsize = idx - 1;
        for (uint i = 0; i < plsize; i++)
        {
    
            vx = pl[i].x - pl[i + 1].x;
            vy = pl[i].y - pl[i + 1].y;
            vz = pl[i].z - pl[i + 1].z;
            types[i].dista = vx * vx + vy * vy + vz * vz;
        }

        types[plsize].range = sqrt(pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y);

        give_feature(pl, types, pl_corn, pl_surf);
    }

    pub_func(pl_orig, pub_full, msg->header.stamp);
    pub_func(pl_surf, pub_surf, msg->header.stamp);
    pub_func(pl_corn, pub_corn, msg->header.stamp);
}