前言
针对第一版中算法耗时过长,达到了30ms,需要对各个环节的耗时情况分析,减少不必要的算法逻辑。
一 算法主要耗时点分析
1.1 极值点搜索
输入点云数量为6万个点,在本机电脑上测试,在进行栅格最小值点计算时,两种方法
方法一:给定每个栅格内的最低点,然后先遍历所有栅格,并在每个栅格内进行最小值搜索
通过分析可以知道,耗时19.489ms
for (int i = 0; i < RowNum; i++) // 遍历格网,计算坡度值
{
for (int j = 0; j < ColNum; j++)
{
temp_index = (i - 1) * ColNum + j;
if (lidar_pipline_pointclouds[temp_index].size() > 0)
{
count = count + 1; // 统计存在点云的数量(显示网格内的点云)
for (int k = 0; k < lidar_pipline_pointclouds[temp_index].size(); k++)
{
if (lidar_pipline_pointclouds[temp_index].points[k].z < Z_min)
{
Z_min = lidar_pipline_pointclouds[temp_index].points[k].z;
high(i, j) = Z_min;
}
}
}
}
第二种:先将所有点存储在一个容器内,然后用sort排序,耗时140.815ms
for (int i = 0; i < RowNum; i++) // 遍历格网,计算坡度值
{
for (int j = 0; j < ColNum; j++)
{
temp_index = (i - 1) * ColNum + j;
if (lidar_pipline_pointclouds[temp_index].size() > 0)
{
count = count + 1; // 统计存在点云的数量(显示网格内的点云)
for (int k = 0; k < lidar_pipline_pointclouds[temp_index].size(); k++)
{
z_min_line.push_back(lidar_pipline_pointclouds[temp_index].points[k].z);
}
sort(z_min_line.begin(), z_min_line.end());
high(i, j) = z_min_line.begin();
}
}
}
小结,第二种方法明显的此一举,在每个栅格内都进行了排序,耗时增加到7倍
1.2 栅格搜索方式
第一种:就是通过两个for循环来搜索
第二种:减少循环的次数,采用一个for循环进行网格搜索
int grid_index_size = (row_scale - 1) * col_scale + col_scale;
for (int i = 0; i < grid_index_size; i++) // 遍历格网,计算坡度值
{
for (int k = 0; k < lidar_pipline_pointclouds[temp_index].size(); k++)
{
if (lidar_pipline_pointclouds[temp_index].points[k].z < Z_min)
{
Z_min = lidar_pipline_pointclouds[temp_index].points[k].z;
high(i, j) = Z_min;
}
}
}
第一种方法耗时17.581ms,第二种方法耗时16.4ms,大致可以减少1ms左右,两个for循环时间复杂度是O(N2),一个是时间复杂度是O(N)
1.3 在将点云进行栅格化的同时,记录下栅格中的最低点
实际上在操作过程中是先将点云存储在map中,然后在计算每个map中的最低点,这个步骤是多余的。
第一种方法:分成两步处理,耗时20.965ms
第二种方法:对于点云所在位置,用一个vector记录,而每个栅格点云的最低点,同样用一个vector记录
// 最大值和最小值容器设置
int row = 0; // 点云所在行
int col = 0; // 点云所在列
float row_dis = 0;
float col_dis = 0;
int grid_index = 0;
std::vector<int> grid_min_value(grid_index_size, INT_MAX);
std::vector<int> point_grid_index(clouds_in->points.size(), 0);
for (int index = 0; index < clouds_in->points.size(); index++)
{
row_dis = clouds_in->points[index].x - grid_params_.min_x;
row = int(row_dis / grid_dimension) + 1;
col_dis = clouds_in->points[index].y - grid_params_.min_y;
col = int(col_dis / grid_dimension) + 1;
grid_index = (row - 1) * grid_params_.col_scale + col;
point_grid_index[index] = grid_index;
if (clouds_in->points[index].z < grid_min_value[grid_index])
{
grid_min_value[grid_index] = clouds_in->points[index].z;
}
}
第二种方法耗时,4.7ms
1.4采用提取器进行点云提取
第一种方法:判断为地面点云后,直接进行赋值操作,耗时2.474ms
第二种方法:判断万成之后采用点云索引保存,然后用提取器进行提起,耗时0.566ms
pcl::PointCloud<PointType>::Ptr under_roof_clouds(new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>::Ptr roof_clouds(new pcl::PointCloud<PointType>);
pcl::ExtractIndices<PointType> roof_extract;
roof_extract.setInputCloud(src_pointcloud);
pcl::PointIndices indices;
bool is_roof = false;
float z_min_limit = 0.3;
for (int index = 0; index < src_pointcloud->points.size(); index++) // 遍历格网,计算坡度值
{
is_roof = ifRoofPointclouds(src_pointcloud->points[index].z,
grid_max_value[point_grid_index[index]],
z_min_limit);
if (is_roof)
{
indices.indices.push_back(index);
}
}
roof_extract.setIndices(boost::make_shared<pcl::PointIndices>(indices));
roof_extract.setNegative(true); // false to under roof the indices
roof_extract.filter(*under_roof_clouds);
//将判断条件单独放一个函数
bool RemoveGroundBySlope::ifRoofPointclouds(
const float points_z,
const int grid_max_val,
const float z_min_limit)
{
// 栅格不存在点云,栅格存在点云,但是这个栅格不是最高点
if (grid_max_val < 0)
{
ROS_DEBUG_STREAM("grid do not have point !");
return false;
}
// 是否符合坡度要求
float z_dis = points_z - grid_max_val;
float z_slop = fabs(z_dis / grid_params_.grid_dimension);
if (z_slop < grid_params_.slope_threshold && points_z < z_min_limit)
{
return true;
}
else
{
return false;
}
}
1.5采用多线程进行最后一步的提取工作和一开始的栅格极值点极端
考虑到每一步实际上只是做了一个判断,采用多线程并不能很好的降低耗时,参考如下多线程不一定提高效率
二 个人总结
- 算法优化目前整体耗时在6-7ms左右,基本满足需要
- 测试过程中的代码并没有很好的保存下来,因此每个部分记录的代码比较乱以后再做优化,切个分支单独记录
- 最后做了代码整体格式的优化,和第一版有较大出入,很多变量都不同,这里主要记录思路和效果
- 后续依然后优化空间,主要减少误识别的情况,有时间单独对效果进行优化分析