二维点云栅格化优化
基本思路:按照点云中的xyz坐标值,直接判断放到相应的栅格中,因为定义每个栅格的大小和多少是知道的,比如我的x值是11.5,每个栅格设置的大小是1m,那我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]);}
}
}
}
}
}
里面所用到的一些变量,在上次的代码中已经提及,定义是相同的。之前的代码使用的3个for循环,时间复杂度是O(
N
3
N^3
N3),这次只用了一个for循环,时间复杂度为O(N)。
看一下实际运行的效果:
优化前:
优化后:
时间还是提升很多倍的。以后再继续出其他的栅格化算法。
算法后记现存问题!!!
首先,投影算法是没有问题的,在后续开发中,在激光雷达线束少的情况下,会出现断层的情况,所谓的断层就是在比如在车前20米的情况下,线束少,所以打到地面上的点云可能都没有,但是这个时候出现个悬空的障碍物的时候如果不是很厚,比如说一个平板厚5厘米,那么我们在过滤地面的时候,这个5厘米,比如上面的0.05,在我们设置的时候就很可能过滤掉,这种情况下这个障碍物我们就会被滤掉,在聚类的情况下也不会有结果.在实际使用的情况下出现了这个问题.
上图中红色的部分为障碍物,但是未被识别出来,因为它下面(也就是地面)是没有点云的,它的厚度在5cm里面内,所以就会当成地面处理,它实际举例地面的高度出不多20mm,这个bug还没被解决,希望大家帮忙出出主意~~