自动驾驶去地面算法学习(3)—基于坡度的去地面算法耗时优化

前言

针对第一版中算法耗时过长，达到了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采用多线程进行最后一步的提取工作和一开始的栅格极值点极端

考虑到每一步实际上只是做了一个判断，采用多线程并不能很好的降低耗时，参考如下多线程不一定提高效率

二 个人总结

1. 算法优化目前整体耗时在6-7ms左右，基本满足需要
2. 测试过程中的代码并没有很好的保存下来，因此每个部分记录的代码比较乱以后再做优化，切个分支单独记录
3. 最后做了代码整体格式的优化，和第一版有较大出入，很多变量都不同，这里主要记录思路和效果
4. 后续依然后优化空间，主要减少误识别的情况，有时间单独对效果进行优化分析