激光雷达点云---点云二维栅格化处理

激光点云栅格化处理

 　　激光点云地图存储的是传感器对环境的原始扫描点云,优点是保留信息完整,缺点是计算量大、不能直接用于导航避障;特征地图存储的是环境中的特殊几何特征,如电线杆、路标、障碍物边缘等,其计算量小但保留信息过少需进行过滤后才能进行使用。

 　　激光点云栅格化核心思想是将激光雷达所扫描到的区域用网格进行处理，每个栅格点云代表空间的一小块区域，内含一部分点云，点云栅格化处理分为二维栅格化和三维栅格化，二维其实就是将三维点云进行一个投影。不考虑ｚ值的变化。
这里我们先讲一下二维栅格化的处理：
 　 我们假设地面相对平坦，即地面扫描点的 z 轴方向的波动较小，通过将扫描区域进行栅格划分，将扫描点云投影到 xy 的栅格平面，通过统计栅格中 z 轴方面的最高点和最低点的差值(即，极差)，判断栅格中的点是否为地面点或障碍物点。
核心代码如下，最后会附上完整的。

class Grid　//先定义一个类，或者结构体也行。
    {
    public:
        typename pcl::PointCloud<PointT>::Ptr grid_cloud {new pcl::PointCloud<PointT>()};//这里我是定义了一个模板，可以自己将其定义成普通的共享指针类型
        pcl::PointIndices::Ptr grid_inliers {new pcl::PointIndices};	//用于储存每个栅格内点云对应原始点云的索引值。
        bool low_emp=true;	//相当于flag，判断当前点云中是否有ｚ最小值。
        float min_height;　　//z最小值
    };
    grid_size = 1;     　　//每个网格的大小
    int column = 60;　　//行数
    int row = 40;			//列数
     vector<int>save_vec;　//最终处理的点云的索引值。
    Grid grid[column][row]; //定义点云的大小
    

  　上边一块使我们自己定义每个栅格内含的数据格式，定义了60* 40 *1大小的网格，这个可以自己定义，以后我们的处理中可能会遇到很多需要用到的数据，如点云落到栅格中的概率，或者栅格的属性、分类等，这些都可以自行添加。

 for(int count=0;count<cloud->points.size();count++)    //从第一个点云开始遍历所有的点云数量。
  {
    for(int i=0;i<column;i++)　//定位point该在哪一行，从第０行开始，让i＝０是因为我之前已经用直通滤波将x范围设置在[0,60]。
      {
       if((cloud->points[count].x > i*grid_size)&&(cloud->points[count].x < (i+1)*grid_size))　//看是否在第ｉ行
          {
           for(int j=-row/2;j<row/2;j++)　//确定好行后，找point所在的列。
             {
              if((cloud->points[count].y > j*grid_size)&&(cloud->points[count].y < (j+1)*grid_size))　//定位所在的列比如是不是在[-20,-19.5]之间，这里实际上是将最边缘的方块作为grid[0][0]。
                {
                　 grid[i][j+row/2].grid_inliers->indices.push_back(count);　//将符合条件的点的索引送给栅格中点的索引。
               　  grid[i][j+row/2].grid_cloud->points.push_back(cloud->points[count]);　//将符合条件的点的索引送给栅格中点的索引。
                   if(grid[i][j+row/2].low_emp)　//下面语句是设置方格中点的最小值，进入网格中的每一个点都会与min_height进行一个比较。
                     {
                        grid[i][j+row/2].min_height = cloud->points[count].z;
                        grid[i][j+row/2].low_emp=false;
                     }
                      else{
                       if(cloud->points[count].z<grid[i][j+row/2].min_height)
                      {grid[i][j+row/2].min_height = cloud->points[count].z;}
                     }
                 }
             }
          }
      }
  }

  　以上代码块的作用其实就是将原始点云做网格化处理，先定位行，后定位列。我这里的范围是x:[0-60],这个可以自行设定,下图为栅格的一个简单的示意图，其中右下角的红色方格为grid[0][0],也就是我们所限定的Y的范围中负向最大的值。

    //--------GRID-------//
    for(int i=0;i<column;i++)　//这里从行开始遍历
    {
        for(int j=0;j<row;j++)   //从列开始遍历。
        {
            int grid_num = grid[i][j].grid_cloud->points.size();
            if(grid[i][j].min_height<0)　//这里是判断每个格子里面的最低点是不是比雷达的安装位置低。
            {
                for(int k=0;k<grid_num;k++)　//遍历格子中的点云。
                {
                    if( (grid[i][j].grid_cloud->points[k].z>(grid[i][j].min_height+0.14)) )//这里加的值是你想保留住的范围点云。　
                    {      //0.20
                        if(grid[i][j].grid_cloud->points[k].z<(grid[i][j].min_height+1.6))　//同样是范围值。
                        {     //2.2
                            if((right+0.4< grid[i][j].grid_cloud->points[k].y )&&(grid[i][j].grid_cloud->points[k].y<left-0.4)){　//y向你想保存的范围。
                                save_vec.push_back(grid[i][j].grid_inliers->indices[k]);//将点云的索引保存到vector.
                         }
                     }
                 }
              }
            }
        }
    }
    typename pcl::PointCloud<PointT>::Ptr all_piece(new pcl::PointCloud<PointT>());//可以使用你自己的点云类型进行定义。
    pcl::copyPointCloud(*cloud, save_vec , *all_piece)；//将点云按照索引存储成一个新的点云。

  　上边一段代码说白了是你想用分格后网格来做些什么，这里是一个简单的入门，我是限制了一个左右，高度的一个范围，而且上面代码中的min_height的值一般都是地面的高度(负数)，上边+0.14就是把地面滤掉了。这就是利用点云栅格化过滤地面的一个简单的用法。这类功能行函数还需要自己多开发。

以上。

  　其实这个栅格化的代码有些复杂，现在有个想法是按照点云中的xyz坐标值，直接判断放到相应的栅格中，因为定义每个栅格的大小和多少是知道的，比如我的x值是11.5，每个栅格设置的大小是１m，那我x所在的行就是第12行，不用这么麻烦的判断了，后续优化代码写完后会放出来。

下面是整个的代码，不可以直接使用，有些参数还需要自己定义。

class Grid
    {
    public:
        typename pcl::PointCloud<PointT>::Ptr grid_cloud {new pcl::PointCloud<PointT>()};
        pcl::PointIndices::Ptr grid_inliers {new pcl::PointIndices};
        bool low_emp=true;
        float min_height;
    };
    vector<int>save_vec;
    Grid grid[column][row];

    for(int count=0;count<cloud->points.size();count++)
    {
        for(int i=0;i<column;i++)
        {
            if((cloud->points[count].x > i*grid_size)&&(cloud->points[count].x < (i+1)*grid_size))
            {
                for(int j=-row/2;j<row/2;j++)
                {
                    if((cloud->points[count].y > j*grid_size)&&(cloud->points[count].y < (j+1)*grid_size))
                    {
                        grid[i][j+row/2].grid_inliers->indices.push_back(count);
                        grid[i][j+row/2].grid_cloud->points.push_back(cloud->points[count]);
                        if(grid[i][j+row/2].low_emp)
                        {
                            grid[i][j+row/2].min_height = cloud->points[count].z;
                            grid[i][j+row/2].low_emp=false;
                        }
                        else{
                            if(cloud->points[count].z<grid[i][j+row/2].min_height)
                            {grid[i][j+row/2].min_height = cloud->points[count].z;}
                        }
                    }
                }
            }
        }

    }
    //--------GRID-------//
    for(int i=0;i<column;i++)
    {
        for(int j=0;j<row;j++)
        {
            int grid_num = grid[i][j].grid_cloud->points.size();
            if(grid[i][j].min_height<0)
            {
                for(int k=0;k<grid_num;k++)
                {
                    if( (grid[i][j].grid_cloud->points[k].z>(grid[i][j].min_height+0.14)) )
                    {      //0.20
                        if(grid[i][j].grid_cloud->points[k].z<(grid[i][j].min_height+1.6))
                        {     //2.2
                            if((right+0.4< grid[i][j].grid_cloud->points[k].y )&&(grid[i][j].grid_cloud->points[k].y<left-0.4)){
                                save_vec.push_back(grid[i][j].grid_inliers->indices[k]);
                            }
                        }
                    }
                }
            }
        }
    }
    typename pcl::PointCloud<PointT>::Ptr all_piece(new pcl::PointCloud<PointT>());
    pcl::copyPointCloud(*cloud, save_vec , *all_piece)；