地面点滤除算法存档

基于栅格化点云高度差的地面点滤除算法

统计每个栅格中的最高点和最低点，并计算最高点和最低点的高度差，若高度差大于某一阈值，则将该栅格内的点标记为物体点，若高度差小于该阈值，则将该栅格内的点标记为地面点。
栅格最低点（ransac拟合的最低点）

ransac传送门：【简书】随机抽样一致性算法（RANSAC）详解 + 面试手写RANSAC

输入：

data // 一组观测数据
model // 适应于数据的模型
n // 适应于模型的最少数据个数，如果是直线的话，n=2
k // 算法的迭代次数
t // 用于决定数据是否应用于模型的阈值
d // 判定模型是否适用于数据集的数据个数，人为设定

过程：

有一个模型适应于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。
用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点。
如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。
然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。
最后，通过估计局内点与模型的错误率来评估模型。

这个过程被重复执行固定的次数，每次产生的模型要么因为局内点太少而被舍弃，要么因为比现有的模型更好而被选用

输出

best_model // 跟数据最匹配的模型参数（如果没有好的模型，返回null）
best_consensus_set // 估计出模型的数据点
best_error // 跟数据相关的估计出的模型错误

伪代码：

iterations = 0
best_model = null
best_consensus_set = null
best_error = 无穷大
while(iterations < k)
  maybe_inliers = 从数据集中随机选择n个点
  maybe_model = 适合于maybe_inliers的模型参数
  consensus_set = maybe_inliers

  for(每个数据集中不属于maybe_inliers的点)
    if(如果点适合于maybe_model，且错误小于t)
      将点添加到consensus_set
  if(consensus_set中的元素数目大于d)
    已经找到好的模型，现在测试该模型到底有多好
    better_model = 适合于consensus_set中所有点的模型参数
    this_error = better_model究竟如何适合这些点的度量
    if(this_error < best_error)
      我们发现了比以前更好的模型，保存该模型直到更好的模型出现
      best_model = better_model
      best_error = this_error
      best_consensus_set = consensus_set
  iteration++

return best_model, best_consensus_set, best_error

C++存档

#include "ground_filter.h"
#include <pcl/common/common.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <unordered_map>
#include <vector>

GroundFilter::GroundFilter(float grid_size, float height_threshold)
    : grid_size_(grid_size), height_threshold_(height_threshold) {}

void GroundFilter::getBoundingBox(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                                 Eigen::Vector4f& min_pt,
                                 Eigen::Vector4f& max_pt) {
    pcl::getMinMax3D(*cloud, min_pt, max_pt);
}

void GroundFilter::filterByHeight(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_cloud,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& ground_cloud,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& non_ground_cloud) {
    
    // 获取点云边界
    Eigen::Vector4f min_pt, max_pt;
    getBoundingBox(input_cloud, min_pt, max_pt);
    
    // 创建栅格索引映射
    std::unordered_map<size_t, std::vector<size_t>> grid_map;
    
    // 将点分配到栅格
    for (size_t i = 0; i < input_cloud->points.size(); ++i) {
        const auto& pt = input_cloud->points[i];
        int grid_x = static_cast<int>((pt.x - min_pt[0]) / grid_size_);
        int grid_y = static_cast<int>((pt.y - min_pt[1]) / grid_size_);
        size_t grid_index = static_cast<size_t>(grid_x) * 1000000 + static_cast<size_t>(grid_y);
        grid_map[grid_index].push_back(i);
    }
    
    // 处理每个栅格
    std::vector<bool> is_ground(input_cloud->points.size(), false);
    for (const auto& grid : grid_map) {
        if (grid.second.empty()) continue;
        
        // 找到栅格中的最高点和最低点
        float min_z = std::numeric_limits<float>::max();
        float max_z = -std::numeric_limits<float>::max();
        
        for (const auto& idx : grid.second) {
            const float& z = input_cloud->points[idx].z;
            min_z = std::min(min_z, z);
            max_z = std::max(max_z, z);
        }
        
        // 根据高度差判断是否为地面点
        float height_diff = max_z - min_z;
        bool is_ground_grid = height_diff < height_threshold_;
        
        // 标记点
        for (const auto& idx : grid.second) {
            is_ground[idx] = is_ground_grid;
        }
        
        std::cout << "Grid index: " << grid.first
                  << ", Points: " << grid.second.size()
                  << ", Height diff: " << height_diff
                  << ", Is ground: " << is_ground_grid << std::endl;
    }
    
    // 分离地面点和非地面点
    ground_cloud->clear();
    non_ground_cloud->clear();
    
    for (size_t i = 0; i < input_cloud->points.size(); ++i) {
        if (is_ground[i]) {
            ground_cloud->points.push_back(input_cloud->points[i]);
        } else {
            non_ground_cloud->points.push_back(input_cloud->points[i]);
        }
    }
    
    ground_cloud->height = 1;
    ground_cloud->width = ground_cloud->points.size();
    non_ground_cloud->height = 1;
    non_ground_cloud->width = non_ground_cloud->points.size();
}

void GroundFilter::filterByRansac(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_cloud,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& ground_cloud,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& non_ground_cloud) {
    
    // 获取点云边界
    Eigen::Vector4f min_pt, max_pt;
    getBoundingBox(input_cloud, min_pt, max_pt);
    
    // 创建栅格索引映射
    std::unordered_map<size_t, std::vector<size_t>> grid_map;
    
    // 将点分配到栅格
    for (size_t i = 0; i < input_cloud->points.size(); ++i) {
        const auto& pt = input_cloud->points[i];
        int grid_x = static_cast<int>((pt.x - min_pt[0]) / grid_size_);
        int grid_y = static_cast<int>((pt.y - min_pt[1]) / grid_size_);
        size_t grid_index = static_cast<size_t>(grid_x) * 1000000 + static_cast<size_t>(grid_y);
        grid_map[grid_index].push_back(i);
    }
    
    // RANSAC分割参数
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(height_threshold_);
    seg.setMaxIterations(100);
    
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    
    // 处理每个栅格
    std::vector<bool> is_ground(input_cloud->points.size(), false);
    for (const auto& grid : grid_map) {
        if (grid.second.size() < 3) continue; // 需要至少3个点来拟合平面
        
        // 创建当前栅格的点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr grid_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        for (const auto& idx : grid.second) {
            grid_cloud->points.push_back(input_cloud->points[idx]);
        }
        grid_cloud->height = 1;
        grid_cloud->width = grid_cloud->points.size();
        
        // 执行RANSAC平面拟合
        seg.setInputCloud(grid_cloud);
        seg.segment(*inliers, *coefficients);
        
        if (inliers->indices.size() > 0) {
            // 计算平面法向量与垂直方向的夹角
            float normal_angle = std::acos(std::abs(coefficients->values[2])) * 180.0f / M_PI;
            
            // 如果平面接近水平（小于30度），则认为是地面
            if (normal_angle < 30.0f) {
                for (size_t i = 0; i < inliers->indices.size(); ++i) {
                    size_t grid_point_idx = grid.second[inliers->indices[i]];
                    is_ground[grid_point_idx] = true;
                }
            }
        }
        
        std::cout << "Grid index: " << grid.first
                  << ", Points: " << grid.second.size()
                  << ", Ground points: " << inliers->indices.size() << std::endl;
    }
    
    // 分离地面点和非地面点
    ground_cloud->clear();
    non_ground_cloud->clear();
    
    for (size_t i = 0; i < input_cloud->points.size(); ++i) {
        if (is_ground[i]) {
            ground_cloud->points.push_back(input_cloud->points[i]);
        } else {
            non_ground_cloud->points.push_back(input_cloud->points[i]);
        }
    }
    
    ground_cloud->height = 1;
    ground_cloud->width = ground_cloud->points.size();
    non_ground_cloud->height = 1;
    non_ground_cloud->width = non_ground_cloud->points.size();
}

range image分割

PY示例：【点云精配准】Iterative Closest Point（ICP）

def depth_clustering(projected_image,
                     angle_tollerance = np.radians(8),
                     min_cluster_size = 100,
                     max_cluster_size = 25000,
                     h_res=np.radians(360/2048.),
                     v_res=np.radians(26.9/65)):

    depth = projected_image[:, :, 4]
    depth_shape = depth.shape

    # 初始 label , label matrix
    
    label_matrix = np.zeros(depth_shape)

    # 对每个点进行检测，同时聚类
    for i in range(depth_shape[0]):
        
            if label_matrix[i, j] == 0:
                if depth[i, j] == 0:
                    continue  # 地面点跳过计算
                one_label_BFS(i, j, label, label_matrix,
                              depth, angle_tollerance,
                              h_res, v_res, projected_image)
                label += 1


    # 过滤掉点数量较多或较少的类别
    not_real_obstacle = 0
    for lx in range(1,label):
        cluster_num = np.sum(label_matrix == lx)
        # 最大数量限制会把一些墙体也过滤掉，这是否是需要的？
        # if cluster_num < min_cluster_size :
        if cluster_num < min_cluster_size or cluster_num > max_cluster_size:
            # 把过滤掉的类别全部设置为地面
            label_matrix[label_matrix == lx] = 0
            not_real_obstacle += 1
    # print('After filtered  left ', label - not_real_obstacle, ' real clusters .')

    return label_matrix

# @profile
def one_label_BFS(i, j, label, label_matrix,
                  depth, angle_tollerance,
                  hres, vres, projected):
    # 不管是广度优先还是深度优先，都是要搜索到所有与它关联的节点；
    # 它是怎么保证最多搜索次数只有2N？
    # TODO 用set 更合适，因为会添加进去很多重复的点，可以用set的自动去重；
    
    while len(Q) > 0:
        
        Q.pop(0) # 默认是丢弃最后一个元素， 要放在这里，不然会死循环
        if label_matrix[r, c] > 0:
            # we have already labeled this point.No need to deal it.
            continue
        # label 赋值
        #label_matrix[r, c] = label

        # 深度值为0，不纳入计算
        if depth[r,c] < 0.0001:
            continue

        for rn, cn in neighbourhood(r, c):
            if rn >= 65 or rn < 0 or cn < 0 or cn >= 2049:
                continue  # 超出范围的点，不考虑
            if label_matrix[rn, cn] > 0:
                # we have already labeled this point.No need to add it.
                continue
            # d1 = np.max((depth[r,c], depth[rn, cn]))
            # d2 = np.min((depth[r,c], depth[rn, cn]))
            #d1 = depth[r, c]
            d2 = depth[rn, cn]
   
            if d1 < d2:   
                #d1,d2 = d2,d1   # 这样更快


            # TODO  两种方案，使用固定分辨率，或者实时计算分辨率，好像差别不大，因为不是压缩的深度图，所以差别不大
            # plan 1
            if r != rn:
                res = vres
            elif c != cn:
                res = hres

            # plan 2
            # if r != rn:
            #     # y_img_2 = -np.arcsin(z_lidar/d) # 得到垂直方向角度
            #     res0 = np.arcsin(projected[r, c, 2] / np.sqrt(projected[r, c, 0] ** 2 + projected[r, c, 1] ** 2 + projected[r, c, 2] ** 2))
            #     res1 = np.arcsin(projected[rn, cn, 2] / np.sqrt(projected[rn, cn, 0] ** 2 + projected[rn, cn, 1] ** 2 + projected[rn, cn, 2] ** 2))
            #     res00 = np.arcsin(projected[r, c, 2] / depth[r,c])
            #     res11 = np.arcsin(projected[rn, cn, 2] / depth[rn, cn])
            #     print(res0,res00,res1,res11)
            #     res = abs(res0 - res1)
            # elif c != cn:
            #     # np.arctan2(-y_lidar, x_lidar)
            #     res0 = np.arctan2(projected[r, c, 1] ,  projected[r, c, 0] )
            #     res1 = np.arctan2(projected[rn, cn, 1] ,  projected[rn, cn, 0] )
            #     res = abs(res0 - res1)


            theta = np.arctan2(d2 * np.sin(res), d1 - d2 * np.cos(res))
            if theta > angle_tollerance:
                Q.append((rn, cn))




def neighbourhood(r, c):
    # 某个点的上下左右四个点的坐标
    return ( ( r-1, c ),
             ( r, c-1 ),
             ( r+1, c ),
             ( r, c+1 )  )

需要改成C++

DipG-Seg复现

传送门DipG-SegGithub

先配置环境，安装ROS Melodic: 参考如下链接

Ubuntu18.04 ROS Melodic 安装必成功流程+避坑指南（超详细）

Ubuntu 20.04 【ROS】在 Ubuntu 20.04 安装 ROS 的详细教程

E: 无法定位软件包 ros-noetic-desktop-full 或者E: 无法定位软件包 ros-noetic-message-generation（换源没用，版本正确，已解决）

ROS安装问题： “Command ‘roscore‘ not found, but can be installed with: sudo apt install python-roslaunch“

ps. 防火墙问题一概可用clash的TUN解决。

Opencv按照Github上链接的步骤一一解决。

Ubuntu 20.04遇到python的一些版本问题，如carkin_make操作。


catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python3

顺便需要修改一下C++支持的版本(在CMakeLists.txt的project()后面添加)

set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)

最后make完记得刷新一下


source devel/setup.bash

分割效果经实际验证极快。能到100Hz。不过目前测试是在KITTI数据集内集成的，暂未测试山峦地形。

简单学习一下算法流程：

参考博客：CSDN

1.数据投影

双图投影：将 LiDAR 的 3D 点云投影为两幅圆柱图像：

d‑image（range-image）：记录水平距离；

z‑image：记录高度信息。

当多个点投影到同一个像素上时，后面的投影点将被忽略，但是它们在点云中的索引将被保存起来用于后续的标签投票操作。

图像修复：点云投影会存在空洞或缺失点，DipG‑Seg 会对空洞像素填充，通过邻域插值或其他简易修复方式，保证图像质量：

一级图像修复：当顶行数据缺失时，若其下邻居行存在数据，则将下邻居行数据复制给它；当底行数据缺失时，若其上邻居行存在数据，则将上邻居行数据复制给它；当中间行数据缺失时，若上下邻居行均存在有效数据，则将其均值复制给它。

二级图像修复：二级图像修复过程中会遍历当前像素的同列上step行数据（prev）和下step行数据（next），遍历过程中若prev和next代表的像素值偏差在阈值范围内，则记录下当前的两个数据；待完成整个上邻域和下邻域的邻居点遍历后，当前像素的值设为邻域内值的平均值，否则的话依然设置为无效数据

参数自适应：分辨率设置可根据激光束数（16/32/64）动态调整，例如标准使用 (64×360)，也提供轻量版本如 (64×360) 或更小。

2. 特征提取

在两张图上计算四种特征图：

STDZ（z方向高度标准差）：用33矩形像素作为领域捕捉局部高度波动，用于区分不平整区域。

edge图：edge图中像素表示该像素是否属于边缘像素，若属于边缘像素，则z图像的水平相邻像素变化较大，而非边缘像素的相邻像素变化较小。其实该图像所反映的物体意义即，沿着传感器y坐标系方向，找出高度变化较大的部分数据，该部分数据必然是边缘数据，如果是正常地面数据，那么同一高度情况下沿传感器y轴的像素值不会发生较大的变动

Slope 特征：改进自传统只垂直两行的斜率测量，结合 LiDAR 安装角度进行补偿。存放的是坡度斜率像素。

HSV（Horizontal Slope Variation）：测量水平方向上斜率的变化，代表地面平坦程度。计算slope图像同一行中两个连续像素之间的斜率差，很容易得到 HSV 图像。

当上述4个像素图生成完成后，对slope图和HSV图像进行3*3局部邻域均值操作，可以缓解局部范围内的传感器观测误差造成的坡度和水平斜率变化问题，这种均值操作，如果真的遇到障碍物数据，那像素值虽然会被降低，但依然明显高于观测数据产生的数据变化

预分割

对slope图和HSV图像进行3*3局部邻域均值操作，可以缓解局部范围内的传感器观测误差造成的坡度和水平斜率变化问题，这种均值操作，如果真的遇到障碍物数据，那像素值虽然会被降低，但依然明显高于观测数据产生的数据变化。

预分割生成possibility图像，possibility图像中，非地面像素置为1，地面像素置为0。由下向上，由左向右逐元素遍历各个像素，获取各元素对应的edge图、slope图、HSV图和STDZ图像素值，首先对高程方向的沿传感器y轴方向的高度偏差进行判断，若该偏差满足阈值（说明该像素可能属于地面像素，此时若满足斜坡梯度下限或者y方向斜坡角度偏差下限或者最低高程阈值下限中的一个则属于地面像素，若都不满足则说明该像素为非地面像素）；若偏差不满足阈值（并且不满足斜坡梯度上限或者y方向斜坡角度偏差上限或者最低高程阈值上限中的一个则属于非地面像素）。也就是所谓的利用低阈值从非边缘像素中寻找地面像素，利用高阈值从边缘像素中重新确认非地面像素。

精细优化

对预分割的HC图进行闭运算。和MVK卷积（所谓的多数票核心卷积，就是比较3*3邻域内的9个像素，对9个像素进行投票。当邻域内的像素均为1（全为非地面像素，参数设置）时，才将其归类为非地面像素（non_ground_pixel））再进行开运算。

对可能性图和MVK后的掩码运算：

possibility = (possibility & ground_pixel) | non_ground_pixel

该流程均为图像操作，可高效并行，执行速度极快。

地面像素标记与重投影

连通域标注：使用 BFS 在优化后的像素图中提取地面连通区域。

重投影回 3D：将这些标注的地面像素索引用于原点云，即可快速标记地面点。

地面点滤除算法存档

地面点滤除算法存档

基于栅格化点云高度差的地面点滤除算法

range image分割

DipG-Seg复现

1.数据投影

2. 特征提取

相关文章