Patchwork++论文和代码学习——CZM

1.问题提出

 大多数基于多平面的方法都基于假设：可观测世界可能不是平坦的。因此，不应该直接估计地面，而是通过假设非平坦世界有小块或bins，并且地面在该区域内确实可以是平坦的。因此，将地面划分为如下图

        但是这样划分会导致一些问题，比如说大多数地面点都位于靠近传感器的位置，即90%以上属于地面的点位于20m以内。所以，越靠近外围的bin，bin中的点云就越稀疏，这会导致无法准确捕捉数据特征从而无法找到正确的地平面，论文中称为稀疏问题。还有一个问题就是当靠近原点的 bin 的太小而无法表示扇区中的单位空间时，有时会导致地平面的法向量估计失败，论文中称之为代表性问题。

2.问题解决

        同心区模型是用来解决以上问题的，从下图(b)中可以看出来，地面被划分为4个区域，最外围的区块和最内层的区块中的bin都比(a)中的大，用来解决稀疏性问题和代表性问题。

每个区域内的bin大小都不相同，代表每个 bin的变量定义如下:

代表所有点中的第k个点。

代表当前所在区域，根据经验，将区域个数设置成了4个，从代码中可以看到作者是强制性的要求这一点。所以m={1,2,3,4}.在论文中Z1​、Z2​、Z3和 Z4​分别称为中心区、四分之一区、半区和外区。

if (num_zones_ != 4 || num_sectors_each_zone_.size() != num_rings_each_zone_.size()) {
            throw invalid_argument("Some parameters are wrong! Check the num_zones and num_rings/sectors_each_zone");
}

论文中的公式如下：

根据以上公式就可以推导出每个，在代码中初始默认值为

        node_handle_.param("max_r", max_range_, 80.0);
        node_handle_.param("min_r", min_range_, 2.7);

个人感觉如果雷达和地面之间没什么遮挡的话，最小值2.7m还是缩小一点的好。

是区域内的环的个数。

是区域内bin的个数。

3.代码

以下是相关代码，可以看出来如果不再规定范围内，就直接归为非地面点，否则就通过计算得出属于哪个区域的哪个环中的哪个bin：

template<typename PointT> inline
double PatchWorkpp<PointT>::xy2theta(const double &x, const double &y) { // 0 ~ 2 * PI
    // if (y >= 0) {
    //     return atan2(y, x); // 1, 2 quadrant
    // } else {
    //     return 2 * M_PI + atan2(y, x);// 3, 4 quadrant
    // }

    double angle = atan2(y, x);
    return angle > 0 ? angle : 2*M_PI+angle;
}

template<typename PointT> inline
double PatchWorkpp<PointT>::xy2radius(const double &x, const double &y) {
    return sqrt(pow(x, 2) + pow(y, 2));
}

template<typename PointT> inline
void PatchWorkpp<PointT>::pc2czm(const pcl::PointCloud<PointT> &src, std::vector<Zone> &czm, pcl::PointCloud<PointT> &cloud_nonground) {

    for (int i=0; i<src.size(); i++) {
        if ((!noise_idxs_.empty()) &&(i == noise_idxs_.front())) {
            noise_idxs_.pop();
            continue;
        }

        PointT pt = src.points[i];

        double r = xy2radius(pt.x, pt.y);
        if ((r <= max_range_) && (r > min_range_)) {
            double theta = xy2theta(pt.x, pt.y);

            int zone_idx = 0;
            if ( r < min_ranges_[1] ) zone_idx = 0;
            else if ( r < min_ranges_[2] ) zone_idx = 1;
            else if ( r < min_ranges_[3] ) zone_idx = 2;
            else zone_idx = 3;

            int ring_idx = min(static_cast<int>(((r - min_ranges_[zone_idx]) / ring_sizes_[zone_idx])), num_rings_each_zone_[zone_idx] - 1);
            int sector_idx = min(static_cast<int>((theta / sector_sizes_[zone_idx])), num_sectors_each_zone_[zone_idx] - 1);

            czm[zone_idx][ring_idx][sector_idx].points.emplace_back(pt);
        }
        else {
            cloud_nonground.push_back(pt);
        }
    }

    if (verbose_) cout << "[ CZM ] Divides pointcloud into the concentric zone model" << endl;
}

ring_sizes_是一个环所占的长度，sector_sizes_是一个bin所占的角度。

在配置文件中，同心区模型的参数如下：

czm:
    num_zones: 4
    num_sectors_each_zone: [16, 32, 54, 32] # 每个区域扇区数量
    mum_rings_each_zone: [2, 4, 4, 4]       # 每个区域环的数量
    elevation_thresholds:  [0.0, 0.0, 0.0, 0.0] # threshold of elevation for each ring using in GLE. Those values are updated adaptively.
    flatness_thresholds:  [0.0, 0.0, 0.0, 0.0]  # threshold of flatness for each ring using in GLE. Those values are updated adaptively.

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

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

参考资料：

点云地面分割算法—— Patchwork论文分析 - 古月居

https://arxiv.org/pdf/2108.05560