patchwork++源码学习(2)—Concentric Zone Model 同心区域模型（CZM）

前言

pathwork++中同心区域模型描述比较好理解，但是在代码中哪个变量对应哪个区域的控制量，尤其是代码中变量的定义容易理解错误，因而增加了CZM区域可视化的功能，便于直观的理解算法的边界。

1 Concentric Zone Model 同心区域模型

算法思路理解：基于路面非平坦假设，不应直接估计地面，而是通过假设非平坦地面有多个小块 ，在小块之内地面是平坦的，一般的思路采用进大远小的思路，远处点云较为稀疏。

图1 同心区域模型原理图

2 同心区域模型实现函数

清空每个小块区域

template <typename PointT>
inline void PatchWorkpp<PointT>::flush_patches(vector<Zone> &czm)
{
    for (int k = 0; k < num_zones_; k++) // 区域遍历
    {
        for (int i = 0; i < num_rings_each_zone_[k]; i++) // 每个环遍历
        {
            for (int j = 0; j < num_sectors_each_zone_[k]; j++) // 每个扇形区域遍历
            {
                if (!czm[k][i][j].points.empty())
                    czm[k][i][j].points.clear();
            }
        }
    }

    if (verbose_)
        cout << "Flushed patches" << endl;
}

点云转换到CZM中

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()))//过滤掉噪点区域不转换好CZM
        {
            noise_idxs_.pop();
            continue;
        }

        PointT pt = src.points[i];

        double r = xy2radius(pt.x, pt.y);//计算同心圆半径

        // 只识别一定范围内的地面，类似于栅格化 min_range_ 0.5米  max_range_ 50米
        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;
            
            //ring_idx 环数确定，扇形区域确定
            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;
}

3 理解与测试

个人理解

1. 在第一步RNR中的噪点，不在CZM中进行更新，
2. 在识别范围以外的点，同样会被认为是非地面点
3. 首先确定区域，然后根据区域的最小半径长度，再次计算对应的环和扇形区域

实测理解

CZM的参数如下，其中num_zones对应的是图2中的不同颜色区域，mum_rings_each_zone对应的是每个区域的环数，绿色对应的是4环，蓝色对应的是8环，num_sectors_each_zone对应的是扇形区域，理解为360度分成多少份，蓝色为32份。

czm:
    num_zones: 4
    num_sectors_each_zone: [32, 32, 54, 32] 
    mum_rings_each_zone: [4, 4, 4, 8]
    elevation_thresholds:  [0.0, 0.0, 0.0, 0.5] # threshold of elevation for each ring using in GLE. Those values are updated adaptively.
    flatness_thresholds:  [0.0, 0.0, 0.0, 0.5]  # threshold of flatness for each ring using in GLE. Those values are updated adaptively.

图2 同心区域模型可视化效果