自动驾驶去地面算法学习(2)—基于坡度的去对面算法实现

小山菌

已于 2024-01-16 17:17:27 修改

阅读量170

点赞数 2

分类专栏：去地面算法开发文章标签：性能优化

于 2023-08-12 22:25:49 首次发布

本文链接：https://blog.csdn.net/xjturmy/article/details/132252919

版权

去地面算法开发专栏收录该内容

6 篇文章 28 订阅

订阅专栏

前言

在实际测试中，选取园区场景进行测试，降采样后点云数量为6W个左右，基于法向量的去地面算法的算法耗时太长，而基于平面拟合的算法对于同时存在坡度和转弯的的地面处理效果极差，因此选择基于坡度的地面过滤算法实现。参考文章链接：
基于坡度的过滤地面算法实现

一基于坡度过滤过滤地面原理

原理在链接中已经给出，添加个人理解进行补充完善
1）将所有点云投影到二维平面上，过滤地面时，并统计每个网格的最低点

2）对于每个栅格，统计这个栅格和周围几个栅格的坡度，然后将均值作为当前中心栅格的坡度，类似于中值滤波
在这里插入图片描述
3）设置坡度，进行地面判断因为存在部分栅格虽然有点，但是不是地面点，比如远处打在墙壁上的点

4）遍历点云，当其在地面栅格内，并且高度小于0.5时过滤。

二各部分代码实现理解

1）计算栅格化主要参数

void lidar_pipline::buildGrid(pcl::PointCloud<pcl::PointXYZ>::Ptr orgin_pointclouds,
                              int &row_num_, int &col_num_)
{
    // 计算所有点云边界
    float grid_max_x_ = -100;
    float grid_min_x_ = 100;
    float grid_max_y_ = -100;
    float grid_min_y_ = 100;

    for (int i = 0; i < orgin_pointclouds->points.size(); i++)
    {
        if (orgin_pointclouds->points[i].x < grid_min_x_)
        {
            grid_min_x_ = orgin_pointclouds->points[i].x;
        }
        if (orgin_pointclouds->points[i].y < grid_min_y_)
        {
            grid_min_y_ = orgin_pointclouds->points[i].y;
        }
        if (orgin_pointclouds->points[i].x > grid_max_x_)
        {
            grid_max_x_ = orgin_pointclouds->points[i].x;
        }
        if (orgin_pointclouds->points[i].y > grid_max_y_)
        {
            grid_max_y_ = orgin_pointclouds->points[i].y;
        }
    }

    // 计算栅格边界
    grid_scale_ = 3;                             // 栅格尺寸边长
    grid_max_x_ = grid_max_x_ + grid_scale_ * 2; // 扩大格网边界，避免在格网边界计算
    grid_min_x_ = grid_min_x_ - grid_scale_ * 2;
    grid_max_y_ = grid_max_y_ + grid_scale_ * 2;
    grid_min_y_ = grid_min_y_ - grid_scale_ * 2;

    row_num_ = int((grid_max_x_ - grid_min_x_) / grid_scale_ + 1); // 格网行数
    col_num_ = int((grid_max_y_ - grid_min_y_) / grid_scale_ + 1); // 格网列数

    printf("grid row size is %d, col size is %d\n", row_num_, col_num_); // 体素网格法降采样
}

2）统计每个栅格内的点云，并且将其存储在map中

// 根据栅格尺寸将点云进行栅格化处理
std::map<size_t, pcl::PointCloud<pcl::PointXYZ>> lidar_pipline::mapPointClouds(
    pcl::PointCloud<pcl::PointXYZ>::Ptr orgin_pointclouds)
{

    // 遍历原始点云，放入二维格网对应的哈希表，并记录有点格网的索引号
    int row = 0;
    int col = 0;
    int map_index = 0;
    std::map<size_t, size_t> get_obs_map;
    int get_obs_num = 0;
    std::map<size_t, pcl::PointCloud<pcl::PointXYZ>> map_pointclouds;
    for (size_t index = 0; index < orgin_pointclouds->points.size(); index++)
    {
        row = int((orgin_pointclouds->points[index].x - grid_min_x_) / grid_scale_) + 1;
        col = int((orgin_pointclouds->points[index].y - grid_min_y_) / grid_scale_) + 1;
        map_index = (row - 1) * col_num_ + col;
        map_pointclouds[map_index].push_back(orgin_pointclouds->points[index]);
        get_obs_map[map_index] = 1;
    }
    
    return map_pointclouds;
}

3）计算地面的高程，将整个高程计算过程省略，采用最低点，

实际测试过程中，在测试通道的远处，点云采集的效果差，会把两侧的通道壁误识别为地面带你，另外通道两侧存在排水渠等，导致采用参考链接中的方式会存在误识别。

// 计算地面和顶部的高程
void lidar_pipline::getGroundHigh(
    std::map<size_t, pcl::PointCloud<pcl::PointXYZ>> &map_pointclouds,
    Eigen::MatrixXd &ground_high,
    Eigen::MatrixXd &roof_high)
{
    int map_get_points_num = 0;
    int map_not_get_points_num = 0;
    float sum = 0;
    int map_index = 0;
    vector<float> z_min_line;

    for (int i = 0; i < row_num_; i++) // 遍历格网，计算坡度值
    {
        for (int j = 0; j < col_num_; j++)
        {

            map_index = (i - 1) * col_num_ + j;
            if (map_pointclouds[map_index].size() > 0)
            {
                // 统计存在点云的数量（显示网格内的点云）
                map_get_points_num = map_get_points_num + 1;
                for (int k = 0; k < map_pointclouds[map_index].size(); k++)
                {
                    // 所有点云的z值存储在一起
                    z_min_line.push_back(map_pointclouds[map_index].points[k].z);
                }

                // 地面矩阵，最低值
                sort(z_min_line.begin(), z_min_line.end());

                // 底部矩阵，最低值
                ground_high(i, j) = z_min_line[0];

                // 清空容器
                z_min_line.clear();
                sum = 0;
            }
            else
            {
                map_not_get_points_num = map_not_get_points_num + 1;
            }
        }
    }
    printf("get point grid num is %d, do not get point grid num is%d \n",
           map_get_points_num, map_not_get_points_num); // 体素网格法降采样

}

4）地面栅格判断

将四周坡度求均值的方式舍弃，当栅格的高度低于地面时，认为是地面栅格，因为会存在将远处的通道顶部误识别为，远处的地面会无法识别到点云，顶部存在点云，导致将顶部误识别

void lidar_pipline::firstJudgeGround(
    std::map<size_t, pcl::PointCloud<pcl::PointXYZ>> &map_pointclouds,
    Eigen::MatrixXd &ground_high,
    Eigen::MatrixXd &roof_high,
    Eigen::MatrixXd &first_ground,
    Eigen::MatrixXd &first_roof)
{

    int count2 = 0;
    // 搜索邻域格网时，中心格网的行号的改变值，以遍历八邻域格网
    int areaGridx, areaGridy = 0;
    int sx[8] = {-1, -1, -1, 0, 0, 1, 1, 1};
    int sy[8] = {-1, 0, 1, -1, 1, -1, 0, 1};
    double totalHigh = 0, averageHigh = 0, pointNum = 0, highmax = -10000, highmin = 10000, highdiff = 0, distance = grid_scale_;
    double averageSlope = 0;
    int grid_num = 0;
    int grid_num_roof = 0;
    int map_index = 0;
    int count_diff = 0;
    vector<float> hight_temp;
    float highdiff_all = 0;

    for (int i = 0; i < row_num_; i++) // 遍历格网，计算坡度值
    {
        for (int j = 0; j < col_num_; j++)
        {
            // 网格内有点云
            map_index = (i - 1) * col_num_ + j;
            if (map_pointclouds[map_index].size() > 0)
            {

                // 默认所有零点一下都是，
                if (ground_high(i, j) < 0)
                {
                    first_ground(i, j) = 1;
                }

                if (roof_high(i, j) > 0)
                {
                    first_roof(i, j) = 1;
                }
            }

            averageSlope = 0;
            grid_num = 0;
        }
    }
}

5）二次地面过滤

在行驶区域以外，会存在孤立的点，这部分点云不是地面点，因此认为地面应该最少是连续的。

void lidar_pipline::secondJudgeGround(
    Eigen::MatrixXd &first_ground,
    Eigen::MatrixXd &second_ground)
{
    int IS_GROUND = 1;
    // 矩阵初始化
    int count3 = 0;
    int first_ground_map = 0;

    int second_ground_map = 0;
    int grid_num = 0;
    int areaGridx, areaGridy = 0;
    int sx[8] = {-1, -1, -1, 0, 0, 1, 1, 1};
    int sy[8] = {-1, 0, 1, -1, 1, -1, 0, 1};
    double totalHigh = 0, averageHigh = 0, pointNum = 0;
    double highmax = -10000, highmin = 10000;
    double highdiff = 0, distance = grid_scale_, averageSlope = 0;

    for (int i = 0; i < row_num_; i++) // 遍历格网，计算坡度值
    {
        for (int j = 0; j < col_num_; j++)
        {

            if (first_ground(i, j) == IS_GROUND)
            {

                first_ground_map = first_ground_map + 1;
                for (int k = 0; k < 8; k++)
                {
                    areaGridx = i + sx[k]; // 邻域格网的行列号
                    areaGridy = j + sy[k];

                    if (first_ground(areaGridx, areaGridy) == IS_GROUND)
                    {
                        grid_num = grid_num + 1;
                    }
                }

                if (grid_num > 1)
                {
                    second_ground(i, j) = 1;
                    second_ground_map = second_ground_map + 1;
                }
            }
            grid_num = 0;
        }
    }

    printf("the first ground is %d, second ground map size is %d\n",
           first_ground_map, second_ground_map);
}

6）地面点过滤

确保是地面点云，同时单个栅格内的点云的坡度小于阈值，认为该点是地面点

void lidar_pipline::filterGround(
    pcl::PointCloud<pcl::PointXYZ>::Ptr orgin_pointclouds,
    Eigen::MatrixXd &first_ground,
    Eigen::MatrixXd &ground_high,
    pcl::PointCloud<pcl::PointXYZ>::Ptr ground_pointclouds,
    pcl::PointCloud<pcl::PointXYZ>::Ptr upground_pointclouds)
{

    // 过滤地面点
    int IS_GROUND = 1;
    int row = 0;
    int col = 0;
    for (int index = 0; index < orgin_pointclouds->points.size(); index++) // 遍历格网，计算坡度值
    {
        row = int((orgin_pointclouds->points[index].x - grid_min_x_) / grid_scale_) + 1;
        col = int((orgin_pointclouds->points[index].y - grid_min_y_) / grid_scale_) + 1;
        if (first_ground(row, col) == IS_GROUND &&
            (((orgin_pointclouds->points[index].z - ground_high(row, col)) / grid_scale_) < ground_slope_threshold_))
        {
            ground_pointclouds->points.push_back(orgin_pointclouds->points[index]);
        }
        else
        {
            upground_pointclouds->points.push_back(orgin_pointclouds->points[index]);
        }
    }
    printf("the ground points is %ld, upground points is %ld\n",
           ground_pointclouds->points.size(), upground_pointclouds->points.size());
    // 参数重置
    // grid_max_x = -100, grid_min_x = 100;
    // grid_max_y_ = -100, grid_min_y_ = 100;

    // 过滤顶部点云
}