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

1.问题提出

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

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

2.问题解决

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

每个区域内的bin大小都不相同，代表每个 bin的变量 $^{S_{i,j,m}{}}$ 定义如下:

$P_{k}$ 代表所有点中的第k个点。

$Z_{m}$ 代表当前所在区域，根据经验，将区域个数设置成了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");
}

论文中的 $\triangle L_{m}$ 公式如下：

$\triangle L_{m}=L_{max,m} - L_{min, m}$

$L_{max,m} = L_{min, m+1}$

$L_{max} = L_{max,4}$

$L_{min} = L_{min, 1}$

$L_{min,2} = \frac{7L_{min}+L_{max}}{8}$

$L_{min,3} = \frac{3L_{min}+L_{max}}{4}$

$L_{min,4} = \frac{L_{min}+L_{max}}{2}$

根据以上公式就可以推导出每个 $\triangle L_{m}$ ，在代码中 $L_{min},L_{max}$ 初始默认值为

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

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

$N_{r,m}$ 是区域内的环的个数。

$N_{\theta ,m}$ 是区域内bin的个数。

$\rho _{k} = \sqrt{x^{2}+y^{2}}$

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: 4num_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