地面点检测算法（附C++代码）

Satisfying

已于 2024-07-10 11:17:33 修改

阅读量387

点赞数 9

分类专栏：数学基础算法源码文章标签：自动驾驶人工智能算法

于 2024-03-14 10:13:01 首次发布

本文链接：https://blog.csdn.net/weixin_40583722/article/details/136700190

版权

数学基础同时被 2 个专栏收录

9 篇文章 27 订阅

订阅专栏

算法源码

4 篇文章 0 订阅

订阅专栏

文章目录

一、直接法（角度阈值法）
二、RANSAC

一、直接法（角度阈值法）

1.1 传统方法

在这里插入图片描述

基于角度 $\theta=arctan(\frac{||BC||}{||AC||})=arctan(\frac{|dz|}{\sqrt{dx^2+dy^2}})$ 做阈值检测，当 $\theta<5°$ 时认为这两点为地面点。
在这里插入图片描述

代码实战 | 用LeGO-LOAM实现地面提取

1.2 优化1：安装高度先验

加入雷达安装高度先验信息约束，雷达安装高度约为1.8m，那么只对 $z < - 1.5 m$ 的点进行地面点检测：
在这里插入图片描述

1.3 优化2：地面线最远先验

加入雷达最远地面线先验信息约束，地面线最远到不了100m，那么只对 $d i s < 100 m$ 的点进行地面点检测：
在这里插入图片描述

// points为有序点云：逐个角度存储1~128线数据
void GroundRemoval(std::vector<LidarPointInfo>& points) {
    size_t N_points = points.size();
    int flag;
    for (size_t j = 0; j < N_points - 1; j++) {
        if ((j + 1) % 128 != 0) {
            const size_t lower_ind = j;
            const size_t upper_ind = j + 1;

            float diff_x = points[upper_ind].x - points[lower_ind].x;
            float diff_y = points[upper_ind].y - points[lower_ind].y;
            float diff_z = points[upper_ind].z - points[lower_ind].z;
            float diff_xy = sqrt(pow(diff_x, 2) + pow(diff_y, 2));

            ((atan2(abs(diff_z), diff_xy) * 180 / PI < 5) && (points[lower_ind].z < -1.5) && (points[lower_ind].dis < 100)) ? flag = 1 : flag = 0;
            points[lower_ind].ins = flag;  // 地面点和非地面点的intensity不同，可根据颜色区分开
            points[upper_ind].ins = flag;  
        }
    }
}

// 或者如下
void GroundRemoval(std::vector<LidarPointInfo>& points, std::vector<std::vector<int>> inds_mesh) {
    for (size_t col = 0; col < ANGLE_NUM; ++col) {
        for (size_t row = 0; row < LINE_NUM - 1; ++row) {
            const size_t lower_ind = inds_mesh[row][col];
            const size_t upper_ind = inds_mesh[row + 1][col];

            float diff_x = points[upper_ind].x - points[lower_ind].x;
            float diff_y = points[upper_ind].y - points[lower_ind].y;
            float diff_z = points[upper_ind].z - points[lower_ind].z;
            float diff_xy = sqrt(pow(diff_x, 2) + pow(diff_y, 2));

            int flag;
            ((atan2(abs(diff_z), diff_xy) * 180 / PI < 5) && (points[lower_ind].z < -1.5) && (points[lower_ind].dis < 100)) ? flag = 1 : flag = 0;
            points[lower_ind].ins = flag;  // 地面点和非地面点的intensity不同，可根据颜色区分开
            points[upper_ind].ins = flag;  
        }
    }
}

1.4 优化3：地面厚度

地面是有一定厚度的，给一个厚度阈值。

// 优化后的算法参数：THR_ANGLE=3; THR_HEIGHT=-1.5; THR_RANGE=100; DELTA_THICK=0.08; 
void GroundRemoval(std::vector<LidarPointInfo>& points, std::vector<std::vector<int>> inds_mesh, float& mean_z) {
    float sum_z = 0;
    int cnt = 0;
        
    // 角度阈值初筛
    for (size_t col = 0; col < ANGLE_NUM; ++col) {
        for (size_t row = 0; row < LINE_NUM - 1; ++row) {
            const size_t lower_ind = inds_mesh[row][col];
            const size_t upper_ind = inds_mesh[row + 1][col];

            float diff_x = points[upper_ind].x - points[lower_ind].x;
            float diff_y = points[upper_ind].y - points[lower_ind].y;
            float diff_z = points[upper_ind].z - points[lower_ind].z;
            float diff_xy = sqrt(pow(diff_x, 2) + pow(diff_y, 2));

            int flag;
            ((atan2(abs(diff_z), diff_xy) * 180 / PI < THR_ANGLE) && (points[lower_ind].z < THR_HEIGHT) && (points[lower_ind].dis < THR_RANGE)) ? flag = 1 : flag = 0;
            points[lower_ind].ins = flag;  // 地面点和非地面点的intensity不同，可根据颜色区分开
            if (flag == 1) {
                sum_z += points[lower_ind].z;
                cnt += 1;
            }
        }
    }

    // 地面厚度优化
    mean_z = sum_z / cnt;
    for (size_t col = 0; col < ANGLE_NUM; ++col) {
        for (size_t row = 0; row < LINE_NUM - 1; ++row) {
            const size_t ind = inds_mesh[row][col];
            if ((points[ind].z < THR_HEIGHT) && (points[ind].dis < THR_RANGE) && (points[ind].ins == 0) && (points[ind].z <= mean_z + DELTA_THICK)) {
                points[ind].ins = 1;
            }
        }
    }
}

在这里插入图片描述

1.5 优化4：剔除野值z

// 计算地面点z均值时，只使用其中80%的数据，即DEL_RATIO=0.1
void GroundRemoval(std::vector<LidarPointInfo>& points, std::vector<std::vector<int>> inds_mesh) {
    // 角度阈值初筛
    vector <float> ground_z_lis;
    ground_z_lis.reserve(LINE_NUM * ANGLE_NUM);
    for (size_t col = 0; col < ANGLE_NUM; ++col) {
        for (size_t row = 0; row < LINE_NUM - 1; ++row) {
           const size_t lower_ind = inds_mesh[row][col];
           const size_t upper_ind = inds_mesh[row + 1][col];

           float diff_x = points[upper_ind].x - points[lower_ind].x;
           float diff_y = points[upper_ind].y - points[lower_ind].y;
           float diff_z = points[upper_ind].z - points[lower_ind].z;
           float diff_xy = sqrt(pow(diff_x, 2) + pow(diff_y, 2));

           if ((atan2(abs(diff_z), diff_xy) * 180 / PI < THR_ANGLE) && (points[lower_ind].z < THR_HEIGHT) && (points[lower_ind].dis < THR_RANGE)) {
               points[lower_ind].ins = 1;  // 地面点和非地面点的intensity不同，可根据颜色区分开
               ground_z_lis.emplace_back(points[lower_ind].z);
           }
        }
    }

    // 地面厚度优化（剔除ground_z_lis的野值）
    int start_ind = ceil(ground_z_lis.size() * DEL_RATIO);
    int end_ind = floor(ground_z_lis.size() * (1 - DEL_RATIO));
    float sum_z = 0;
    int cnt = 0;
    for (size_t i = start_ind; i <= end_ind; ++i) {
        sum_z += ground_z_lis[i];
        cnt += 1;
    }
    float mean_z = sum_z / cnt;
    for (size_t col = 0; col < ANGLE_NUM; ++col) {
        for (size_t row = 0; row < LINE_NUM - 1; ++row) {
            const size_t ind = inds_mesh[row][col];
            if ((points[ind].z < THR_HEIGHT) && (points[ind].dis < THR_RANGE) && (points[ind].ins == 0) && (points[ind].z <= mean_z + DELTA_THICK)) {
                points[ind].ins = 1;
            }
        }
    }
}

二、RANSAC

2.1 传统方法

思路：先根据先验信息滤除掉不可能是地面点的部分点云，再从剩下的候选地面点中随机选择3个点，根据三个点坐标计算平面的法线向量，将法线向量及已知点坐标代入点法式方程即可得拟合的平面方程表达式，将剩余候选地面点代入方程计算每个点到平面的距离，统计满足内点数量点的个数。循环迭代，取内点数量最多的一次迭代的平面方程模型为最优模型。

#define LIDAR_HEIGHT 1.8
#define N_POINT_FIT 3
#define MAX_ITER_NUM 1e4
#define DELTA_DIS 0.06
#define INLINER_RATIO 0.95

void RANSAC(std::vector<LidarPointInfo>& points) {
    // 地面点初筛
    std::vector<int> idx;
    idx.reserve(points.size());
    for (size_t i = 0; i < points.size(); ++i) {
        if (points[i].z < -LIDAR_HEIGHT) {  // 先滤除不可能是地面点的，只从z小于安装高度负值的点中选
            idx.emplace_back(i);
        }
    }
    std::vector<LidarPointInfo> points_new;
    points_new.resize(idx.size());
    for (size_t i = 0; i < idx.size(); ++i) {
        points_new[i] = points[idx[i]];
    }
        
    // 初始化
    float A = 0; 
    float B = 0; 
    float C = 0;
    float X1 = 0;
    float Y1 = 0;
    float Z1 = 0;
    float inliner_ratio = 0;
    float max_inliner_ratio = 0;
    std::vector<int> inliers_res = {};

    int iter = 0;
    while (iter < MAX_ITER_NUM & inliner_ratio < INLINER_RATIO) {
        std::vector<int> inliers_idx = {};
        inliers_idx.reserve(points_new.size());

        // 随机抽3个点
        std::vector<int> random_vec;
        std::vector<int> rand_nums;
        for (size_t i = 0; i < points_new.size(); ++i) {
            random_vec.emplace_back(i);
        }
        random_shuffle(random_vec.begin(), random_vec.end());
        rand_nums.assign(random_vec.begin(), random_vec.begin() + N_POINT_FIT);

        // 求平面方程参数
        float x1 = points_new[rand_nums[0]].x; float x2 = points_new[rand_nums[1]].x; float x3 = points_new[rand_nums[2]].x;
        float y1 = points_new[rand_nums[0]].y; float y2 = points_new[rand_nums[1]].y; float y3 = points_new[rand_nums[2]].y;
        float z1 = points_new[rand_nums[0]].z; float z2 = points_new[rand_nums[1]].z; float z3 = points_new[rand_nums[2]].z;
        float x_11 = x1 - x2; float y_11 = y1 - y2; float z_11 = z1 - z2;  // 法向量就是两个向量的叉积
        float x_12 = x1 - x3; float y_12 = y1 - y3; float z_12 = z1 - z3;
        float a = y_11 * z_12 - z_11 * y_12;  // 叉乘计算公式
        float b = z_11 * x_12 - x_11 * z_12;
        float c = x_11 * y_12 - y_11 * x_12;

        // 逐点判断是否是内点
        for (size_t i = 0; i < points_new.size(); ++i) {
            LidarPointInfo point = points_new[i];
            float dis_to_surface = fabs(-(a * (point.x - x1) + b * (point.y - y1) + c * (point.z - z1))) / sqrt(a * a + b * b + c * c);  // 法向量未归一化，计算点到面的距离时需要除以这个系数
            if (dis_to_surface <= DELTA_DIS) {
                inliers_idx.emplace_back(i);
            }
        }

        // 更新最优模型
        inliner_ratio = static_cast<float>(inliers_idx.size()) / random_vec.size();  // 两个整数相除结果一定是整数，相当于整除，因此必须要对其中一个做强制类型转换
        if (inliner_ratio > max_inliner_ratio) {
            A = a;
            B = b;
            C = c;
            X1 = x1;
            Y1 = y1;
            Z1 = z1;
            inliers_res = inliers_idx;
            max_inliner_ratio = inliner_ratio;
        }

        iter += 1;
    }
    std::cout << "迭代次数=" << iter << "，内点概率=" << inliner_ratio << "，最大概率=" << max_inliner_ratio << std::endl;


    // 将地面点标记为红色
    for (size_t i = 0; i < LINE_NUM * ANGLE_NUM; ++i) {
        float dis = fabs(-(A * (points[i].x - X1) + B * (points[i].y - Y1) + C * (points[i].z - Z1))) / sqrt(A * A + B * B + C * C);
        if (dis <= DELTA_DIS) {
            points[i].ins = 1;
        }
    }
}

在这里插入图片描述

2.2 优化思路

加快拟合速度：先过滤掉不可能是地面点的点云；（2.1节中已经使用了）
墙面可能被误检：优化思路1可以解决一部分；另外雷达和地面通常是平行的，所以法向量和(0, 0, 1)的夹角是很小的，这个思路也可以解决。
斜坡失效：分段拟合。

2.3 迭代次数推导

每次迭代计算得到的内点概率为：
$t=\frac{n_{inliers}}{n_{inliers}+n_{outliers}}$
那么每次迭代使用的点数为N的情况下，至少有一个外点被选择的概率为：
$1-t^N$
则k次迭代中每次都至少有一个外点被选择的概率为：
$1-t^N)^k$
那么k次迭代至少一次能够全部选择正确N个点的概率为：
$P=1-(1-t^N)^k$
则迭代次数为：
$k=\frac{log(1-P)}{log(1-t^N)}$

2.4 加快速度

（1）先初筛；

（2）先下采样；

（3）并行计算。

2.5 LS优化参数

2.5.1 思路

RANSAC拟合之后，可以使用最小二乘法再次优化平面方程参数A, B, C, D：
在这里插入图片描述

最小二乘法——拟合平面方程(深度相机外参标定、地面标定)_最小二乘法拟合平面-CSDN博客

2.5.2 代码

def least_square(self, points):
     A = np.c_[points[:,0], points[:,1], np.ones(points.shape[0])]  # 构造系数矩阵
     b = points[:,2]  # 构造矩阵b
     X = np.dot(np.dot(np.linalg.inv(np.dot(A.T, A)), A.T), b)  # 计算参数向量w
     a, b, c = X[0], X[1], -1
     d = X[2]  # z = X[0]*x + X[1]*y + X[2] => X[0]x+X[1]y-z+X[2]=0
     n = np.array([a , b ,c])
     return n, d

实现Ransac随机采样算法分割地面点云_激光雷达点云障碍物检测

RANSAC 激光雷达地面检测（1）

RANSAC 激光雷达地面检测（2）

计算机视觉CS131：专题4-拟合（最小二乘、RANSAC、霍夫变换）

RANSAC算法详解(附Python拟合直线模型代码)

点云地面检测算法_点云识别算法楼高-CSDN博客

（本文完整的pdf请关注公众号“张张学算法”，并回复“024”获取~）

本文由mdnice多平台发布