Patchwork++论文和代码学习——R-VPF和R-GPF

R-VPF

1.问题提出

        R-GPF在有些时候不能准确估计合适的地面平面，那就是当地面位于垂直的城市结构之上时（图1），因为垂直结构的点的z值比地面的z值要小，R-GPF会导致在错误地将其选择为初始种子点。例如，当挡土墙（紫色）在bin中且距离传感器足够近时，R-GPF会选择墙面的点作为地面的初始种子点。然后，由于PCA对异常点敏感，这些点会导致一个不合适的地面拟合结果。

图1

        作者在论文还提到了可能有争议的一点，即这个绿色的平面是否应该被算作地面。有人会说这个平面的z值明显的高于传感器所在的地面，不能算作地面。但是作者认为人或者其他的什么东西可以站在这个平面上就应该算作地面。

2.解决问题

        首先，R-VPF需要经过num_iter_次迭代，对每一个bin都要进行以下操作：

1.选取种子点：

        先判断当前bin是否是位于中心区，如果是，则过滤bin中z值小于特定值的点（这个特定值和传感器的高度有关）。然后，计算bin中的所有点（过滤后的）的平均z值。如果一个点的z值小于所有点的平均z值+种子阈值（这个值是规定的，默认是0.4）则被选为种子点。

2.估计平面参数

       计算基于种子点的协方差矩阵、x、y、z三个方向的均值和法线向量。

3.计算是否属于地面点

        如果bin属于中心区，且第2步得到的法线向量的z小于正常向量阈值，则将bin中的所有点转换为一个N*3的矩阵，将其与第2步得到的法线向量相乘，得到的乘积就是点到垂直平面的距离。如果点到平面的距离在阈值th_dist_v_和-d_之间，则认为这个点属于垂直平面或地面。

3.代码

R-VPF的主要代码：

if (enable_RVPF_)
    {
        for (int i = 0; i < num_iter_; i++)
        {
            // 选取种子点
            extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_, th_seeds_v_);
            // 
            estimate_plane(ground_pc_);
            // uprightness_thr_是正常向量阈值 
            if (zone_idx == 0 && normal_(2) < uprightness_thr_)
            {
                pcl::PointCloud<PointT> src_tmp;
                src_tmp = src_wo_verticals;
                src_wo_verticals.clear();

                Eigen::MatrixXf points(src_tmp.points.size(), 3);
                int j = 0;
                for (auto &p:src_tmp.points) {
                    points.row(j++) << p.x, p.y, p.z;
                }
                // ground plane model
                Eigen::VectorXf result = points * normal_;

                for (int r = 0; r < result.rows(); r++) {
                    //th_dist_v_= 0.3
                    if (result[r] < th_dist_v_ - d_ && result[r] > -th_dist_v_ - d_) {
                        non_ground_dst.points.push_back(src_tmp[r]);
                        vertical_pc_.points.push_back(src_tmp[r]);
                    } else {
                        src_wo_verticals.points.push_back(src_tmp[r]);
                    }
                }
            }
            else break;
        }
    }

选取种子点：

template<typename PointT> inline
void PatchWorkpp<PointT>::extract_initial_seeds(
        const int zone_idx, const pcl::PointCloud<PointT> &p_sorted,
        pcl::PointCloud<PointT> &init_seeds, double th_seed) {
    init_seeds.points.clear();

    // LPR is the mean of low point representative
    double sum = 0;
    int cnt = 0;

    int init_idx = 0;
    if (zone_idx == 0) {
        for (int i = 0; i < p_sorted.points.size(); i++) {
            if (p_sorted.points[i].z < adaptive_seed_selection_margin_ * sensor_height_) {
                ++init_idx;
            } else {
                break;
            }
        }
    }

    // Calculate the mean height value.
    for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) {
        sum += p_sorted.points[i].z;
        cnt++;
    }
    double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0

    // iterate pointcloud, filter those height is less than lpr.height+th_seeds_
    for (int i = 0; i < p_sorted.points.size(); i++) {
        if (p_sorted.points[i].z < lpr_height + th_seed) {
            init_seeds.points.push_back(p_sorted.points[i]);
        }
    }
}

计算种子点：
 

template<typename PointT> inline
void PatchWorkpp<PointT>::estimate_plane(const pcl::PointCloud<PointT> &ground) {
    pcl::computeMeanAndCovarianceMatrix(ground, cov_, pc_mean_);
    // Singular Value Decomposition: SVD
    Eigen::JacobiSVD<Eigen::MatrixXf> svd(cov_, Eigen::DecompositionOptions::ComputeFullU);
    // 获取奇异值矩阵
    singular_values_ = svd.singularValues();

    // 获取最小奇异向量作为法线方向。
    normal_ = (svd.matrixU().col(2));

    if (normal_(2) < 0) { for(int i=0; i<3; i++) normal_(i) *= -1; }

    // 种子点的均值向量
    Eigen::Vector3f seeds_mean = pc_mean_.head<3>();

    // according to normal.T*[x,y,z] = -d
    d_ = -(normal_.transpose() * seeds_mean)(0, 0);
}

R-GPF

1.目的

         R-GPF的目的是为每个bin估计一个部分地面平面，这些部分平面之后会被合并以形成整个地面的估计。另外，作者在论文中也说明使用PCA而不是RANSAC的原因：与PCA相比，RANSAC 对异常值的敏感度较低。然而，使用PCA显示出比使用RANSAC快得多的速度，并且显示出可接受的性能；因此，基于PCA的估计更适合作为预处理过程。此外，实验表明，基于PCA的方法至少比基于RANSAC的方法快两倍。

2.迭代过程

• 初始估计：选择每个bin中高度最低的点作为初始种子点集合，这些点最有可能属于地面。

• 迭代计算：在每次迭代中，使用当前迭代的估计地面点集合来重新计算该集合的均值点和法向量。

• 地面点更新：根据当前迭代的法向量和平面系数，更新地面点集合，将距离平面较近的点加入到估计的地面点集合中。

        简单来说，首先根据平均z值来选择种子点，使用PCA来计算一些参数，遍历bin中的点，计算点到地面的距离，如果小于阈值的话则归于地面点，否则为非地面点。

        另外，作者在论文中也给出了为什么要对中心区进行特殊处理的原因：有时，由于多路径问题或激光雷达信号的反射，会获取到低于实际地面的错误云点。观察到这种现象大多发生在中心区中，因为反射只发生在信号相对较强的区域。这些异常值阻碍了 R-GPF 正确估计地面平面。

        为解决这个问题，作者利用了一个事实，即只有 Z_1 中的地面点的 z 值主要分布在 −h_s 附近，其中 h_s 表示传感器高度。因此，当使用PCA计算参数时，如果属于 中心区 的bin中的点的z值低于 M_h · h_s，就将其过滤掉，其中 M_h < −1 是高度阈值。对于不属于中心的 bin，随着距离雷达的增大，自适应阈值会降低，以避免不当过滤可能来自下坡的点。

3.代码：

Eigen::MatrixXf points(src_wo_verticals.points.size(), 3);
    int j = 0;
    for (auto &p:src_wo_verticals.points) {
        points.row(j++) << p.x, p.y, p.z;
    }

    for (int i = 0; i < num_iter_; i++) {

        ground_pc_.clear();

        // ground plane model
        Eigen::VectorXf result = points * normal_;
        // threshold filter
        for (int r = 0; r < result.rows(); r++) {
            if (i < num_iter_ - 1) {
                if (result[r] < th_dist_ - d_ ) {
                    ground_pc_.points.push_back(src_wo_verticals[r]);
                }
            } else { // Final stage
                if (result[r] < th_dist_ - d_ ) {
                    dst.points.push_back(src_wo_verticals[r]);
                } else {
                    non_ground_dst.points.push_back(src_wo_verticals[r]);
                }
            }
        }

        if (i < num_iter_ -1) estimate_plane(ground_pc_);
        else estimate_plane(dst);
    }

因本人能力有限，本文难免有错误之处，欢迎大家评论区指正。

本文只做记录本人学习用途，如有侵权，请联系删除。

参考资料：

https://arxiv.org/pdf/2207.11919

2022年最新成果——Patchwork++地面分割 - 古月居