R2live code learning record (3): radar feature extraction

main Read configuration parameter declaration subscription 、 Post topics

int main(int argc, char **argv)
{
    
    ros::init(argc, argv, "feature_ext");
    ros::NodeHandle n;
    //  from launch Read configuration parameters 
    n.param<double>("feature_extraction/blind", blind, 0.01);                    //  Minimum distance , Used to eliminate duplicate points , When the square of the distance between the current point and the next point is less than the threshold , Then remove 
    n.param<double>("feature_extraction/inf_bound", inf_bound, 10);              //  Infinite threshold , Beyond this threshold, it is considered infinite 
    n.param<int>("feature_extraction/N_SCANS", N_SCANS, 1);                      //  Number of scan lines 
    n.param<int>("feature_extraction/group_size", group_size, 8);                //  Laser point cloud feature extraction , Used to confirm whether to group , If the number of points is greater than the threshold, the Group 
    n.param<double>("feature_extraction/disA", disA, 0.01);                      //  Plane point judgment algorithm , It is used to increase the search scope in the process of point calculation , The coefficient of multiplication 
    n.param<double>("feature_extraction/disB", disB, 0.1);                       //  Plane point judgment algorithm , It is used to increase the search scope in the process of point calculation , Additive sparseness 
    n.param<double>("feature_extraction/p2l_ratio", p2l_ratio, 400);             //  Threshold used to judge whether it is a plane point , If less than this threshold , Then this point is the point to be judged 
    n.param<double>("feature_extraction/limit_maxmid", limit_maxmid, 9);         //  Detect the threshold in the plane point 
    n.param<double>("feature_extraction/limit_midmin", limit_midmin, 16);        //  Detect the threshold in the plane point 
    n.param<double>("feature_extraction/limit_maxmin", limit_maxmin, 3.24);      //  Detect the threshold in the plane point 
    n.param<double>("feature_extraction/jump_up_limit", jump_up_limit, 175.0);   //  Upper angle limit 
    n.param<double>("feature_extraction/jump_down_limit", jump_down_limit, 5.0); //  Lower limit of angle 
    n.param<double>("feature_extraction/cos160", cos160, 160.0);
    n.param<double>("feature_extraction/edgea", edgea, 3);    //  In the edge judgment algorithm with crossing , Distance threshold , The coefficient of multiplication 
    n.param<double>("feature_extraction/edgeb", edgeb, 0.05); //  In the edge judgment algorithm with crossing , Distance threshold , Additive sparseness 
    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);       //  All laser point clouds 
    pub_surf = n.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);  //  Flat plane point 
    pub_corn = n.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100); //  Sharp edge points 

    ros::spin();
    return 0;
}

Callback function ：

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