【PCL】点云栅格化2

//
// Created by ethan on 18-6-6.
//
#include <iostream>
#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/console/print.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/cloud_viewer.h>
#include <cmath>
#include <pcl/filters/statistical_outlier_removal.h>
#define _USE_MATH_DEFINES
class Grid
{
public:
    bool  road;
    float h_mean;
    float h_square;
    pcl::PointCloud<pcl::PointXYZ>::Ptr grid_cloud {new pcl::PointCloud<pcl::PointXYZ>};
};
inline float abs(float &a)
{
    if (a<0)
    {a=-a;}
    return a;
}
int
main(int argc,char** argv)
{

    ros::init(argc,argv,"resterization");
    ros::NodeHandle nh;
   
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in  (new pcl::PointCloud<pcl::PointXYZ>);

    if (pcl::io::loadPCDFile<pcl::PointXYZ> ("/home/ethan/bags/pcl_2_bag/cloud9.pcd", *cloud_in) == -1)
    {
        PCL_ERROR ("Couldn't read file cloud9.pcd ^.^\n");
        return (-1);
    }

    int R=150;//半径上分割数
    int TH=72;//角度上的分割数
    float grid_size_r=0.1;//半径上的分辨率
    float grid_size_th=2*M_PI/TH;//角度上的分辨率

    int j=0;
    int i=0;
    double threshold;

    ros::param::get("R",R);
    ros::param::param<int>("R", R, 150);
    ros::param::get("TH",TH);
    ros::param::param<int>("TH", TH, 72);

    grid_size_r=50.0/R;
    grid_size_th=2*M_PI/TH;
    ros::param::get("threshold",threshold);
    ros::param::param<double>("threshold", threshold, 0.00010);

    Grid grid[R][TH];
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_final (new pcl::PointCloud<pcl::PointXYZ>);
    float r=0;
    float th;
    for (int m=0;m<cloud_in->size();m++)//这些点一个一个看
    {
        r=pow(cloud_in->points[m].x,2)+pow(cloud_in->points[m].y,2);//点到原点的距离
        r=pow(r,0.5);
        th=acos(cloud_in->points[m].x/r);//点对应的向量的角度（以x轴正方向为零角度）
        if(cloud_in->points[m].y<0)
        {th=2*M_PI-th;}

        for( i=0;i<R;i++)
        {

            if (((r>i*grid_size_r)&&(r<(i+1)*grid_size_r)))//如果r在某范围内
            {
                for( j=0;j<TH;j++)
                {

                    if((th>j*grid_size_th)&&(th<(j+1)*grid_size_th))//如果th在某范围内
                    {

                        grid[i][j].grid_cloud->points.push_back(cloud_in->points[m]);

                    }
                }
            }
        }
    pcl::visualization::CloudViewer viewer("resterization");//可视化窗口
    viewer.showCloud(cloud_final);//看一下其中某个栅格
    while(!viewer.wasStopped())
    {    }
}

上篇文章中栅格是按照棋盘格的形式划分的，这次是按照蜘蛛网划分的，更加符合车载激光雷达的形状。