Patchwork原理

问题定义

首先，我们首先将此时的点云表示为P。然后，让 $P = \{p_1, p_2, . . . , p_k, . . . , p_N\}$ 是一组云点，包含由 3D LiDAR 传感器获取的时刻的 N 个点，其中每个点 $p_k$ 由笛卡尔坐标中的 $p_k = \{x_k, y_k, z_k\}$ 组成。在本文中，P被明确分为两类：一组地面点 $G$ 及其补集 $G_c$ ，满足 $\hat{G} ∪ \hat{G}^c = P$ 。注意 $G_c$ 表示非地面点，包括车辆、墙壁、街道树、行人等。

接下来，估计的地面点可以定义为 $\hat G$ ,由于估计不可避免地包含固有误差，实际上来自非地面物体的一些点可能包含在 G^ 中，反之亦然。综上所述， $\hat G$ 和 $\hat G_c$ 表示如下：

$\hat{G}=TP∪FP \quad and \quad \hat{G}^c = FN ∪ TN$
其中 $\hat{G}$ 和 $\hat{G}^c$ 也满足 $\hat{G} ∪ \hat{G}^c = P$ ，并且 TP、FP、FN 和 TN 分别表示真阳性、假阳性、假阴性和真阴性的集合。因此，我们的目标是从点云 P 中辨别 $\hat{G}$ 和 $\hat{G}^c$ ，同时估计尽可能少的 FP 和 FN。

同心区模型

如前所述，大多数基于多平面的方法都基于假设：可观测世界可能不是平坦的。因此，地平面估计应该被推倒，通过假设可能的非平坦世界有小块或bins，并且地面在该区域内确实可以是平坦的

因此，以前的一些方法利用统一的极坐标网格表示，或 S，将点云划分为具有规则间隔的径向和方位角方向的多个 bin，即环和扇区。更具体地说，让 $N_r$ 和 $N_θ$ 分别是环和扇区的数量。然后，S 被划分为相同大小的扇环bin，其径向大小为 $L_{max}/N_r$ ，其中 $L_{max}$ 表示最大边界长，扇区方位角大小为 $2π/N_θ$ ，如图 2(a) 所示。

不幸的是，如图 2© 所示，为了考虑泛化，在 SemanticKITTI 数据集 [1] 上的整个序列上测量的实验证据表明，大多数地面点都位于靠近传感器框架的位置。即90%以上属于地面的点位于20m以内。
因此，S 有两个限制。首先，随着距离越来越远，点云变得太稀疏而无法找到正确的地平面，我们称之为稀疏问题。一些方法自适应地调整 bin 的大小以应对对数点分布。但是，bin 大小以线性或二次方式增加，因此稀疏问题并未完全解决。另一方面，当靠近原点的 bin 的太小而无法表示 S 中的单位空间时，有时会导致地平面的右法向量估计失败，我们称之为代表性问题。

为了解决这些问题，提出了基于 CZM 的极坐标网格表示，表示为 C，以一种计算不复杂的方式在 bin 之间分配适当的密度。因此，P被分成多个区域，每个区域由不同大小的bin组成，如图2（b）所示。设 $N 〉 = {1, 2, . . . , N}$ ，那么我们提出的模型定义如下：

$C=\bigcup_{m\in < N_Z>}Z_m$
其中 $Z_m$ 表示 C 的第 m 个区域， $N_Z$ 表示区域的数量，本文根据经验将其设置为 4。令
$Z_m=\{p_k\in P |L_{min,m}\leq \rho _k < L_{max,m} \}$
其中 $L_{min,m}$ 和 $L_{max,m}$ 分别表示 $Z_m$ 的最小和最大径向边界；然后， $Z_m$ 也被划分为 $N_{r,m} × N_{θ,m}$ 个 bin，其中每个区域具有不同的 bin 大小。因此，每个 bin $S_{i,j,m}$ 定义如下：
在这里插入图片描述
其中， $\rho _k = \sqrt{x_k^2+y_k^2},\theta _k =arctan2(y_k,x_k),\Delta L_m=L_{max,m}-L_{min,m},L_{max,m}=L_{min,m+1} \qquad m=1,2,3$

其中全局最小边界 $L_{min}$ 用于考虑移动平台或车辆附近的空旷。实际上， $Z_1$ 、 $Z_2$ 、 $Z_3$ 和 $Z_4$ 分别称为中心区、四分之一区、半区和外区。因此有
$L_{min,2}=\frac{7.L_{min}+L_{max}}{8},L_{min,3}=\frac{3.L_{min}+L_{max}}{4},L_{min,4}=\frac{L_{min}+L_{max}}{2}$

区域级地平面拟合

此后，每个 bin 通过 R-GPF 分配估计的部分接地；然后合并部分地面点。在本文中，使用主成分分析 (PCA) 而不是使用 RANSAC。当然，与 PCA 相比，RANSAC 对异常值的敏感性往往较低。然而，使用 PCA 的速度比使用 RANSAC 快得多，并且表现出可接受的性能；因此，基于 PCA 的估计更适合作为预处理过程。此外，实验表明基于 PCA 的方法至少比基于 RANSAC 的方法快两倍。

给定一个bin，令 $C∈R^{3×3}$ 为单位空间内云点的协方差矩阵；三个特征值 $\lambda_a$ 和对应的三个特征向量 $\nu_a$ 计算如下：
$\nu _a=\lambda _a\nu _a$
其中 α = 1、2、3，并假设 $λ_1 ≥ λ_2 ≥ λ_3$ 。然后，对应于最小特征值的特征向量，即 $v_3$ 最有可能表示地平面的法线向量。因此，设 $=\nu_3 = [a\quad b\quad c]^T$ ，平面系数可以计算为 $-n^T\bar p$ ，其中 $\bar p$ 表示单位空间的平均点。

为简单起见，让第 n 个 bin 是所有 bin 上的 Sn，其值等于 $N_c = \sum_{m=1}^{N_z}N_{r,m}\times N_{\theta ,m}$ 。如果 Sn 的基数足够大，则选择最低高度的点作为初始种子。事实上，每个bin 子高度最低的点最有可能属于地面。设̄ $\tilde z_{init}$ 为所选种子点的总$N_{seed}个数的z均值；然后，初始估计地面点集 $\hat G^0_n$ 得到如下:
$\hat G^0_n= \{ p_k \in S_n | z(p_k) < \bar z_{init}+z_{seed}\}$
其中 z(·) 返回一个点的 z 值， $z_{seed}$ 表示高度边距。

因为我们的方法是迭代的，所以让在第 $l$ 次迭代时设置的估计地面点为 $\hat G^l_n$ ，然后得到 $\hat G^l_n$ 的法向量 $n_n^l$ ，平面系数 $d_n^l$ 计算为: $d_n^l=-(n_n^l)^T \bar p^l_n$ ，其中 $\bar p^l_n$ 表示 $\hat G^l_n$ 的均值点，最后 $\hat G^{l+1}_n$ 公式计算如下：
在这里插入图片描述
其中 $\hat d_k = -(n^l_n)^T p_k$ 和 Md 表示平面的距离边距。该过程重复多次。根据 Zermas 等人的说法， $\hat G_n = \hat G^3_n$ 在经验上是本文中每个 Sn 的最终输出。

为了解决这个问题，我们利用仅在 $Z_1$ 中的地面点的 z 值主要分布在 $h_s$ 附近的事实，其中 hs 代表传感器高度。因此，当估计 $\hat G_n^0$ 时，如果 $z_k$ 低于 $M_h · h_s$ ，则过滤掉属于 $Z_1$ 的 $S_n$ 中的 $p_k$ ，其中 $M_h$ < -1 是高度边际。对于不属于 $Z_1$ 的 $S_n$ ，自适应阈值随着m变大而减小，以避免对可能来自下坡的点进行不当过滤，这些点实际上是TP。

地面似然估计

为有必要稳健地辨别 $\hat G_n$ 是否属于实际地面，提出了 GLE，这是一种用于二元分类的区域概率测试。为这样做，Patchwork 利用 GLE 来提高整体精度，不包括由非地面点组成的初始非预期平面。

令 $L(\theta |\chi )$ 为 GLE，其中 θ 表示 Patchwork 的所有参数， $\chi$ 表示遵循具有密度函数 f 的连续概率分布的随机变量。让我们假设每个 bin 彼此独立。然后， $L(\theta |\chi )$ 表示为
在这里插入图片描述

其中 θn 和 Xn 分别表示每个 $\hat G_n$ 的参数和随机变量。请注意，下标 n 表示参数来自 $\hat G_n$ 。

根据我们的先验知识，每个 $\hat G_n$ 是地面点以垂直度、高程和平面度来定义，分别表示为 $φ (\cdot) 、 ψ (\cdot) 和 φ (\cdot)$ ，如下：
$f(X_n | θ_n) ≡ φ(v_3,n) · ψ(\bar z_n, r_n) · φ(ψ(\bar z_n, r_n), σ_n)$

其中 $\bar z_n$ , $r_n$ , 和 $σ_n$ 表示平均 z 值，原点之间的距离和 $S_n$ 的质心，以及表面变量，其中 $\sigma_n=\frac{\lambda _{3,n}}{\lambda _{1,n}+\lambda _{2,n}+\lambda_ {3,n}}$ 。

Uprightness
如果 $\hat G_n$ 属于实际地面
（即大部分点都在 TP 中），观察到 $v_{3,n}$ 可能与地面车辆接触的地面正交。换句话说， $v_{3,n}$ 倾向于垂直于传感器框架的 X-Y 平面。因此，提出了垂直度指示函数来利用几何特征作为

其中 z = [0 0 1] 和 $\theta_{\tau}$ 是直立边距，表示为 $v_{3,n}$ 与 X-Y 平面之间的角度。也就是说， $\theta_{\tau}$ 越大，标准就越保守。如图 4(a) 和 (b) 所示，红色区域代表不满足垂直度的情况，因此 $\phi(v_{3,n})$ 等于 0。通过实验，我们将 $\theta_{\tau}$ 设置为 45°，根据经验确定它足够严格（参见第 IV.B 节）。
Elevation
不幸的是，仅使用 uprightness 无法过滤属于汽车引擎盖或车顶的 $\hat G_n$ 。此外，当汽车等大型物体靠近传感器框架时，会发生遮挡，从而产生局部观察问题。也就是说，被遮挡空间上方的部分测量浊点被预测为 $\hat G_n$ ，事实上，这不是空间中的最低部分。这种现象如图4（a）的左侧所示。

为了解决这个问题，提出了一个条件逻辑函数 $ψ(\hat z_n, r_n)$ 。高程过滤器的关键思想是由 Asvadi 等人提出的。 [9]：一旦传感器框架附近的 $\hat z_n$ 与 $h_s$ 相比相当高， $\hat G_n$ 可能不是地面。

实验证据支持我们的理论，如图 4（c）所示。仅使用直立性， $TP_s$ 和 $FP_s$ 基于 $\hat z_n$ 变得可区分，当 $r_n$ 很小时， $TP_s$ 的损失很小，即 $\hat G_n$ 在 $Z_1$ 和 $Z_2$ 中的情况。相反，当 rn 很大时， $TP_s$ 和 $FP_s$ 是无法区分的，即 $\hat G_n$ 在 $Z_4$ 中。

基于这些观察， $ψ(\hat z_n, r_n)$ 定义如下：

其中 $κ (\cdot)$ 表示根据 $r_n$ 呈指数增长的自适应中点函数。如图 4(a) 所示，如果 $\bar z_n$ 低于 $κ (r)$ ，则当 $r_n$ 小于恒定范围参数$L_\tau $时，$ ψ(\hat z_n, r_n)$ 的值高于 0.5。请注意，当 $r_n$ 超过$L_\tau $时，$ ψ(\hat z_n, r_n)$ 总是变为 1，因为随着 rn 变大，不清楚 G ^n 是来自非地面物体还是来自陡峭的斜坡。
Flatness
最后，平坦度的目的是还原一些通过高程过滤的 $FN_s$ ，如果它们绝对是一个偶数平面。例如，如果 $\hat G_n$ 属于一个非常陡峭的上坡，因此如果 $\hat z_n$ 大于 $κ (r)$ ，则 $\hat G_n$ 有时会通过 $ψ(\hat z_n, r_n)$ 过滤掉。为了解决这个问题，我们利用表面变量 $\sigma_n$ 来检查被认为是 $FN_s$ 的 $\hat G_n$ 的平坦度，即使 $ψ(\hat z_n, r_n)$ 低于 0.5。为此， $\varphi (ψ(\hat z_n, r_n),\sigma_n)$ 的可定义为

其中 $ζ > 1$ 和 $σ_{τ,m}$ 分别表示增益的大小和取决于 $Z_m$ 的表面变量的阈值。通过这样做，陡峭上坡的 GLE 增加，尽管 $\hat z_n$ 高于 $κ(r_n)$ ，但它们可以恢复为地面估计。

因此，最终估计的地面点可以直接表示为：

其中 [·] 表示 Iverson 括号，如果条件满足则返回 true，否则返回 false。

代码解析

本文采用kitti数据集，是通过HDL-64E的激光雷达采集的数据，在该数据的基础上来验证本说所说的地面分割方法

首先在地面分割之前需要进行一些预处理工作，包括提取ROI感兴趣区域和体素滤波

提取ROI

提取ROI感兴趣的区域可分为两种：

因雷达安装位置的原因，近处的车身反射会对Detection造成影响，需要滤除
过高的目标，过远处的点，如大树、高楼，对于无人车的雷达感知意义也不大，过远激光雷达反射滤，效果不好，也需要滤出

pcl::PointCloud<pcl::PointXYZI>::Ptr RoiClip::GetROI(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr in,pcl::PointCloud<pcl::PointXYZI>::Ptr &out)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr clipCloud = ClipVehicle(in);
    if (clipCloud->points.size() > 0)
    {
        for (auto &p : clipCloud->points)
        {
            if (IsIn(p.x, roi_x_min_, roi_x_max_) && IsIn(p.y, roi_y_min_, roi_y_max_) && IsIn(p.z, roi_z_min_, roi_z_max_))
            out->push_back(p);
        }
        ROS_INFO("GetROI sucess");
        return out;

    }

    ROS_ERROR("GetROI fail");
    return nullptr;
}

pcl::PointCloud<pcl::PointXYZI>::Ptr RoiClip::ClipVehicle(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr in){
    if(in->points.size() > 0)
    {
        pcl::PointCloud<pcl::PointXYZI>::Ptr out(new pcl::PointCloud<pcl::PointXYZI>);
        for (auto &p : in->points)
        {
            if (IsIn(p.x, vehicle_x_min_, vehicle_x_max_) && IsIn(p.y, vehicle_y_min_, vehicle_y_max_) && IsIn(p.z, vehicle_z_min_, vehicle_z_max_)){
                
            }
            else out->push_back(p);
        }
        return out;
    }
    return nullptr; 
}

体素滤波

由于点云聚类的实时性要求，我们需要通过减少点云的密度来加速地面过滤和后续聚类。一种简单的方法就是使用Voxel Grid Filter对点云进行降采样，代码如下：

void VoxelGridFilter::downsample(const pcl::PointCloud<pcl::PointXYZI>::Ptr &in_cloud_ptr,
                                 pcl::PointCloud<pcl::PointXYZI>::Ptr &out_cloud_ptr)
{
  // if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL)
  if (leafSize >= 0.1)
  {
    // Downsampling the velodyne scan using VoxelGrid filter
    pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter;
    voxel_grid_filter.setInputCloud(in_cloud_ptr);
    voxel_grid_filter.setLeafSize(leafSize, leafSize, leafSize);
    voxel_grid_filter.filter(*out_cloud_ptr);
  }
  else
  {
    out_cloud_ptr = in_cloud_ptr;
  }
}

patchwork地面过滤

void PatchWork::estimate_ground(
    const pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud_in,
    pcl::PointCloud<pcl::PointXYZI>::Ptr &ground_cloud,
    pcl::PointCloud<pcl::PointXYZI>::Ptr &non_ground_cloud)
// double &time_taken)
{
    // 1.Msg to pointcloud
    pcl::PointCloud<pcl::PointXYZI> laserCloudIn;
    laserCloudIn = *cloud_in;

    // start = ros::Time::now().toSec();

    // 2.Sort on Z-axis value.
    sort(laserCloudIn.points.begin(), laserCloudIn.end(), point_z_cmp<pcl::PointXYZI>);

    // 3.Error point removal
    // As there are some error mirror reflection under the ground,
    // here regardless point under 1.8* sensor_height
    // Sort point according to height, here uses z-axis in default
    auto it = laserCloudIn.points.begin();
    for (int i = 0; i < laserCloudIn.points.size(); i++)
    {
        if (laserCloudIn.points[i].z < -1.8 * sensor_height_)
        {
            it++;
        }
        else
        {
            break;
        }
    }
    laserCloudIn.points.erase(laserCloudIn.points.begin(), it);

    // t1 = ros::Time::now().toSec();
    // 4. pointcloud -> regionwise setting
    for (int k = 0; k < num_zones_; ++k)
    {
        flush_patches_in_zone(ConcentricZoneModel_[k], num_sectors_each_zone_[k], num_rings_each_zone_[k]);
    }
    pc2czm(laserCloudIn, ConcentricZoneModel_);
    pcl::PointCloud<pcl::PointXYZI> cloud_nonground;
    pcl::PointCloud<pcl::PointXYZI> cloud_out;

    cloud_out.clear();
    cloud_nonground.clear();
    revert_pc.clear();
    reject_pc.clear();

    int concentric_idx = 0;
    for (int k = 0; k < num_zones_; ++k)
    {
        auto zone = ConcentricZoneModel_[k];
        for (uint16_t ring_idx = 0; ring_idx < num_rings_each_zone_[k]; ++ring_idx)
        {
            for (uint16_t sector_idx = 0; sector_idx < num_sectors_each_zone_[k]; ++sector_idx)
            {
                if (zone[ring_idx][sector_idx].points.size() > num_min_pts_)
                {
                    extract_piecewiseground(k, zone[ring_idx][sector_idx], regionwise_ground_, regionwise_nonground_);

                    // Status of each patch
                    // used in checking uprightness, elevation, and flatness, respectively
                    const double ground_z_vec = abs(normal_(2, 0));
                    const double ground_z_elevation = pc_mean_(2, 0);
                    const double surface_variable =
                        singular_values_.minCoeff() /
                        (singular_values_(0) + singular_values_(1) + singular_values_(2));
                    if (ground_z_vec < uprightness_thr_)
                    {
                        // All points are rejected
                        cloud_nonground += regionwise_ground_;
                        cloud_nonground += regionwise_nonground_;
                    }
                    else
                    { // satisfy uprightness
                        if (concentric_idx < num_rings_of_interest_)
                        {
                            if (ground_z_elevation > elevation_thr_[ring_idx + 2 * k])
                            {
                                if (flatness_thr_[ring_idx + 2 * k] > surface_variable)
                                {
                                    if (verbose_)
                                    {
                                        // std::cout << "\033[1;36m[Flatness] Recovery operated. Check "
                                        //           << ring_idx + 2 * k
                                        //           << "th param. flatness_thr_: " << flatness_thr_[ring_idx + 2 * k]
                                        //           << " > "
                                        //           << surface_variable << "\033[0m" << std::endl;
                                        // revert_pc += regionwise_ground_;
                                    }
                                    cloud_out += regionwise_ground_;
                                    cloud_nonground += regionwise_nonground_;
                                }
                                else
                                {
                                    if (verbose_)
                                    {
                                        // std::cout << "\033[1;34m[Elevation] Rejection operated. Check "
                                        //           << ring_idx + 2 * k
                                        //           << "th param. of elevation_thr_: " << elevation_thr_[ring_idx + 2 * k]
                                        //           << " < "
                                        //           << ground_z_elevation << "\033[0m" << std::endl;
                                        reject_pc += regionwise_ground_;
                                    }
                                    cloud_nonground += regionwise_ground_;
                                    cloud_nonground += regionwise_nonground_;
                                }
                            }
                            else
                            {
                                cloud_out += regionwise_ground_;
                                cloud_nonground += regionwise_nonground_;
                            }
                        }
                        else
                        {
                            if (using_global_thr_ && (ground_z_elevation > global_elevation_thr_))
                            {
                                std::cout << "\033[1;33m[Global elevation] " << ground_z_elevation << " > " << global_elevation_thr_
                                          << "\033[0m" << std::endl;
                                cloud_nonground += regionwise_ground_;
                                cloud_nonground += regionwise_nonground_;
                            }
                            else
                            {
                                cloud_out += regionwise_ground_;
                                cloud_nonground += regionwise_nonground_;
                            }
                        }
                    }
                }
            }
            ++concentric_idx;
        }
    }
    *ground_cloud = cloud_out;
    *non_ground_cloud = cloud_nonground;
}

实践

运行

编译运行ROS节点，修改输入topic名字，可以kitti数据集的点云运行我给出的代码，因为我代码里订阅的topic为/velodye_points，大家也可以自行更改

rosbag play -l kitti_2011_09_26_drive_0005_synced.bag /kitti/velo/pointcloud:=/velodye_points
roslaunch patch_work  path_work.launch

效果

在这里插入图片描述
效果确实比pcl方法、Ground Plane Fitting和ray_ground_filter方法好，我提供了几种方法的源码，大家可以自行修改对比

代码链接：