激光雷达点云－－点云栅格化(二)

二维点云栅格化优化

  　基本思路：按照点云中的xyz坐标值，直接判断放到相应的栅格中，因为定义每个栅格的大小和多少是知道的，比如我的x值是11.5，每个栅格设置的大小是１m，那我x所在的行就是第12行。在上次的代码中已经提及过。
直接上代码：

for (int count = 0; count < temp_cloud->points.size(); count++) {
//        cout<<"x:"<<temp_cloud->points[count].x<<endl;
//        cout<<"y:"<<temp_cloud->points[count].y<<endl;
//        cout<<count<<endl;
        int x_grid = floor(double(temp_cloud->points[count].x) / step);//格子边上的点算在下限上。
        int y_grid = floor(double(temp_cloud->points[count].y + (row * step)/2) / step);
        if ((x_grid < column && x_grid >= 0) && (y_grid < row && y_grid >= 0)) {
            grid[x_grid][y_grid].grid_inliers->indices.push_back(count);
            grid[x_grid][y_grid].grid_cloud->points.push_back(temp_cloud->points[count]);
            if (grid[x_grid][y_grid].low_emp) {
                grid[x_grid][y_grid].min_height = temp_cloud->points[count].z;
                grid[x_grid][y_grid].low_emp = false;
            } else {
                if (temp_cloud->points[count].z <grid[x_grid][y_grid].min_height)
                { grid[x_grid][y_grid].min_height = temp_cloud->points[count].z; }
            }
        }
    }
    //点云的处理.
       for(int i=0;i<row;i++)
    {
        for(int j=0;j<column;j++)
        {
            int grid_num = grid[i][j].grid_cloud->points.size();
            if(grid[i][j].min_height< 2)//雷达安装位置为0
            {
                for(int k=0;k<grid_num;k++)
                {
                    if(grid[i][j].grid_cloud->points[k].z>(grid[i][j].min_height+0.05)) //过滤地面     ????有问题
                            if((right < grid[i][j].grid_cloud->points[k].y )&&(grid[i][j].grid_cloud->points[k].y < left))
                            {all_piece->push_back(grid[i][j].grid_cloud->points[k]);}
                    }
                }
            }
        }
    }

里面所用到的一些变量，在上次的代码中已经提及，定义是相同的。之前的代码使用的３个for循环，时间复杂度是O($N^3$),这次只用了一个for循环，时间复杂度为O(N)。
看一下实际运行的效果：
优化前：

优化后：

时间还是提升很多倍的。以后再继续出其他的栅格化算法。

 


算法后记现存问题!!!


  首先,投影算法是没有问题的,在后续开发中,在激光雷达线束少的情况下,会出现断层的情况,所谓的断层就是在比如在车前20米的情况下,线束少,所以打到地面上的点云可能都没有,但是这个时候出现个悬空的障碍物的时候如果不是很厚,比如说一个平板厚5厘米,那么我们在过滤地面的时候,这个5厘米,比如上面的0.05,在我们设置的时候就很可能过滤掉,这种情况下这个障碍物我们就会被滤掉,在聚类的情况下也不会有结果.在实际使用的情况下出现了这个问题.

上图中红色的部分为障碍物,但是未被识别出来,因为它下面(也就是地面)是没有点云的,它的厚度在5cm里面内,所以就会当成地面处理,它实际举例地面的高度出不多20mm,这个bug还没被解决,希望大家帮忙出出主意~~