返回

[代码实践] 自动驾驶中常见的激光雷达点云滤波处理

整理一下自动驾驶中常见的激光雷达点云预处理操作。

简介

在无人驾驶应用中,经常需要使用激光雷达来做建图、定位和感知等任务。激光雷达的原始数据通常需要经过预处理才能使用,这篇博客主要结合 PCL 来整理一下激光雷达点云数据常见的预处理操作。用到的代码在:pointcloud_process

点云数据示例

以下为一帧激光雷达点云数据:

机械式的激光雷达通常按激光线束进行 360° 扫描,因此点云数据一般呈现圆环状从中心向四周扩散。在城市内场景下,通常可以看到一些结构性的特征。例子中的这一帧点云扫描范围大概在 100m 以内,大概有 12 万个点。可以看出来数据规模较大,且分布较广。下面整理一下一些常用的可以用来按需降低点云规模的算法。

对给定区域的点进行处理

去除给定区域外的点

首先可以看出,虽然点云的分布范围较广,但大部分的点都集中的中间区域,距离越远点云越稀疏,相对的信息量也越小。此外还能明显看到一些离群点,因此我们可以筛选掉一些较远的点,只保留我们感兴趣范围内的点。下面是实现方法与效果展示:

实现方式也比较简单,只需要给定保留点云在三个方向上最小值到最大值的范围,通过在三个坐标值进行比较即可。

template <class PointType>
void removePointsOutsideRegion(boost::shared_ptr<pcl::PointCloud<PointType>>& src_cloud_ptr,
                               boost::shared_ptr<pcl::PointCloud<PointType>>& dst_cloud_ptr,
                               const std::pair<double, double>& x_range,
                               const std::pair<double, double>& y_range,
                               const std::pair<double, double>& z_range) {
    int num_points = src_cloud_ptr->points.size();
    boost::shared_ptr<pcl::PointCloud<PointType>> cloud_ptr(new pcl::PointCloud<PointType>());
    cloud_ptr->points.reserve(num_points);

    for (const auto& pt : src_cloud_ptr->points) {
        bool inside = (pt.x >= x_range.first && pt.x <= x_range.second && pt.y >= y_range.first &&
                       pt.y <= y_range.second && pt.z >= z_range.first && pt.z <= z_range.second);

        if (inside) {
            cloud_ptr->points.push_back(pt);
        }
    }

    dst_cloud_ptr = cloud_ptr;
}

以下为保留 x 在 30m,y 在 15m,z 在 2m 范围内的点的效果

去除给定区域内的点

在某些情况下,我们也会需要去除给定区域内部的点,例如在这个例子中,可以看到中心区域有部分点,应该是来自搭载激光雷达的车子本身。这些点通常对建图和感知不会有作用,将上述函数稍作修改即可:

/**
 * @brief filter points within a given range
 * @param src_cloud_ptr source pointcloud
 * @param dst_cloud_ptr destination pointcloud
 * @param x_range min/max x range
 * @param y_range min/max y range
 * @param z_range min/max z range
 * @param remove if true, remove points inside the region, otherwise only keep those points
 */
template <class PointType>
void filterPointsWithinRegion(boost::shared_ptr<pcl::PointCloud<PointType>>& src_cloud_ptr,
                              boost::shared_ptr<pcl::PointCloud<PointType>>& dst_cloud_ptr,
                              const std::pair<double, double>& x_range,
                              const std::pair<double, double>& y_range,
                              const std::pair<double, double>& z_range,
                              bool remove) {
    int num_points = src_cloud_ptr->points.size();
    boost::shared_ptr<pcl::PointCloud<PointType>> cloud_ptr(new pcl::PointCloud<PointType>());
    cloud_ptr->points.reserve(num_points);

    for (const auto& pt : src_cloud_ptr->points) {
        bool inside = (pt.x >= x_range.first && pt.x <= x_range.second && pt.y >= y_range.first &&
                       pt.y <= y_range.second && pt.z >= z_range.first && pt.z <= z_range.second);

        if (inside ^ remove) {
            cloud_ptr->points.push_back(pt);
        }
    }

    dst_cloud_ptr = cloud_ptr;
}

PCL 实现

上述的处理也可以通过 PCL 库中自带的滤波器实现,下面来看一下:

去除给定区域外的点

针对这个功能 PCL 有一个比较直观的函数可以使用,如下所示:

  • CropBox

使用方式如下:

    // option 1: use CropBox
    pcl::CropBox<pcl::PointXYZ> box_filter;
    box_filter.setInputCloud(cloud_ptr);
    box_filter.setMin(Eigen::Vector4f(keep_x_range.first, keep_y_range.first, keep_z_range.first, 1.0));
    box_filter.setMax(Eigen::Vector4f(keep_x_range.second, keep_y_range.second, keep_z_range.second, 1.0));
    box_filter.filter(*temp_cloud_ptr);

效果图和之前几乎没有区别,所以就不放图了。

  • 除了 CropBox 以外,PCL 还提供了其他更灵活的滤波器也可以实现相同功能,如:PassThrough,ConditionalOr 等等,在下一个功能统一整理

去除给定区域内的点

CropBox 只能去除给定范围外的点,下面整理的两个函数使用起来更加灵活:

  • PassThrough: 可以指定点云中的点的某个字段进行范围限制,使用方式如下:
    pcl::PassThrough<pcl::PointXYZ> pass_filter;
    bool reverse_limits = true;
    pass_filter.setInputCloud(filtered_cloud_ptr);
    pass_filter.setFilterFieldName("x");
    pass_filter.setFilterLimits(-5, 5);
    pass_filter.getFilterLimitsNegative(reverse_limits);  // reverse the limits
    pass_filter.filter(*filtered_cloud_ptr);
    pass_filter.setFilterFieldName("y");
    pass_filter.setFilterLimits(-2, 2);
    pass_filter.getFilterLimitsNegative(reverse_limits);  // reverse the limits
    pass_filter.filter(*filtered_cloud_ptr);
    pass_filter.setFilterFieldName("z");
    pass_filter.setFilterLimits(-2, 2);
    pass_filter.getFilterLimitsNegative(reverse_limits);  // reverse the limits
    pass_filter.filter(*filtered_cloud_ptr);

代码中 getFilterLimitsNegative 可以对选定范围取反,将其设为 true 时可以进行给定只保留给定范围内的点的功能。

  • ConditionalRemoval: 相对于 PassThrough,这个方法能够同时设定多个条件,并一次性进行筛选,使用方法如下:
   pcl::ConditionOr<pcl::PointXYZ>::Ptr range_condition(new pcl::ConditionOr<pcl::PointXYZ>());
    range_condition->addComparison(pcl::FieldComparison<pcl::PointXYZ>::Ptr(
        new pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::GE, 5)));
    range_condition->addComparison(pcl::FieldComparison<pcl::PointXYZ>::Ptr(
        new pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::LE, -5)));
    range_condition->addComparison(pcl::FieldComparison<pcl::PointXYZ>::Ptr(
        new pcl::FieldComparison<pcl::PointXYZ>("y", pcl::ComparisonOps::GE, 2)));
    range_condition->addComparison(pcl::FieldComparison<pcl::PointXYZ>::Ptr(
        new pcl::FieldComparison<pcl::PointXYZ>("y", pcl::ComparisonOps::LE, -2)));
    range_condition->addComparison(pcl::FieldComparison<pcl::PointXYZ>::Ptr(
        new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GE, 2)));
    range_condition->addComparison(pcl::FieldComparison<pcl::PointXYZ>::Ptr(
        new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LE, -2)));

    pcl::ConditionalRemoval<pcl::PointXYZ> conditional_filter;
    conditional_filter.setInputCloud(temp_cloud_ptr);
    conditional_filter.setCondition(range_condition);
    conditional_filter.filter(*filtered_cloud_ptr);

注意,range_condition 不仅可以添加比较条件,还能加入其他 ContionalBase 对象进行条件的耦合,所以可以将前面两个预处理合并在一起。

点云下采样

除了将不符合我们感兴趣的点去除以外,还有一种常用的点云预处理方法为:点云下采样。这里 只介绍一种比较常用的栅格化的下采样,在 PCL 中对应的函数为体素滤波。

体素滤波的原理是通过对点云以原点为中心进行栅格化,对每个栅格最多只保留一个点(可以是该栅格内的所有点的重心,或者是该栅格的几何中心)。这样可以在降低点云的规模的前提下仅可能保持点云的结构信息。

以下是以 5 m 为分辨率进行栅格化的例子。(注:设置为 5m 只是为了可视化,一般来说分辨率如果设得太大,点云本身的结构特征也会受到影响。)

实现

栅格化下采样大致的思路是计算整体点云的中心,通过计算每个点到中心的距离结合要求的分辨率计算栅格对应的坐标,并入其中,最后遍历每个包含点的栅格计算其中点的几何中心或者取该栅格中心加入目标点云即可。其中要注意的点有:

  • 根据点云范围和分辨率栅格的分布有细微的区别,这里为了让栅格比较对称,我将参考点选为点云中心,因此在计算栅格坐标时根据每个坐标轴上栅格数量的奇偶性需要做细微的调整,具体见下面实现
  • 在计算栅格整型坐标时,需要对距离除以分辨率得到的浮点坐标进行向下取整
  • 为了节省每个栅格的查找时间,这里使用 unordered_map 来存储栅格坐标和对应的点集,但是这样最后输出的目标点云会失去原先的分布规律,如果对分布规律有要求的情况下,有以下思路:
    • 使用 map 结构,但是在栅格较多的情况下查找速度会降低
    • 用数组 vector 将所有栅格的空间预先保留,这样可以保留常数时间且在最后遍历时能够尽量保证点云顺序,但耗费空间较大(pcl 库使用的是这种方法)
    • 使用 vector 会造成额外空间的原因时根据点云范围和分辨率计算出来的栅格有很多其实是空的(见上图),在这种情况下我们可以采用多层分辨率的 Octotree 来降低多余空间的产生

简单的实现方法如下:

template <class PointType>
void voxelFiltering(boost::shared_ptr<pcl::PointCloud<PointType>>& src_cloud_ptr,
                    boost::shared_ptr<pcl::PointCloud<PointType>>& dst_cloud_ptr,
                    float resolution,
                    bool use_geometric_center = true) {
    // find pointcloud range
    float max_x = std::numeric_limits<float>::min(), min_x = std::numeric_limits<float>::max();
    float max_y = std::numeric_limits<float>::min(), min_y = std::numeric_limits<float>::max();
    float max_z = std::numeric_limits<float>::min(), min_z = std::numeric_limits<float>::max();
    for (const auto& pt : src_cloud_ptr->points) {
        max_x = std::max(max_x, pt.x);
        min_x = std::min(min_x, pt.x);
        max_y = std::max(max_y, pt.y);
        min_y = std::min(min_y, pt.y);
        max_z = std::max(max_z, pt.z);
        min_z = std::min(min_z, pt.z);
    }

    // calculate pointcloud center
    float center_x = (max_x + min_x) / 2.0;
    float center_y = (max_y + min_y) / 2.0;
    float center_z = (max_z + min_z) / 2.0;

    // calculate needed grid nums in each axis
    int x_grid_nums = std::ceil((max_x - min_x) / resolution);
    int y_grid_nums = std::ceil((max_y - min_y) / resolution);
    int z_grid_nums = std::ceil((max_z - min_z) / resolution);

    // map: x_idx -> {y_idx -> {z_idx -> {sum_points, count}}}
    std::unordered_map<int, std::unordered_map<int, std::unordered_map<int, std::pair<PointType, int>>>> status;

    // function for handling float grid idx
    auto process_idx = [](float idx, int grid_num) -> int {
        if (grid_num % 2 == 1) {
            idx += 0.5;
        }

        return std::floor(idx);
    };

    auto reverse_idx = [resolution](int idx, int grid_num, double center) -> float {
        float coord = center + idx * resolution;
        if (grid_num % 2 == 0) {
            coord += 0.5 * resolution;
        }

        return coord;
    };

    // for each point in pointcloud, find its grid index and accumulate points
    for (const auto& pt : src_cloud_ptr->points) {
        float x_idx = (pt.x - center_x) / resolution;
        float y_idx = (pt.y - center_y) / resolution;
        float z_idx = (pt.z - center_z) / resolution;

        int x_idx_clipped = process_idx(x_idx, x_grid_nums);
        int y_idx_clipped = process_idx(y_idx, y_grid_nums);
        int z_idx_clipped = process_idx(z_idx, z_grid_nums);

        if (status[x_idx_clipped][y_idx_clipped][z_idx_clipped].second == 0) {
            status[x_idx_clipped][y_idx_clipped][z_idx_clipped] = {pt, 1};
        } else {
            status[x_idx_clipped][y_idx_clipped][z_idx_clipped].first.x += pt.x;
            status[x_idx_clipped][y_idx_clipped][z_idx_clipped].first.y += pt.y;
            status[x_idx_clipped][y_idx_clipped][z_idx_clipped].first.z += pt.z;
            status[x_idx_clipped][y_idx_clipped][z_idx_clipped].second += 1;
        }
    }

    boost::shared_ptr<pcl::PointCloud<PointType>> cloud_ptr(new pcl::PointCloud<PointType>());
    cloud_ptr->points.reserve(x_grid_nums * y_grid_nums * z_grid_nums);
    for (const auto& x_yz : status) {
        int x_idx = x_yz.first;
        for (const auto& y_z : x_yz.second) {
            int y_idx = y_z.first;
            for (const auto& z_pt : y_z.second) {
                int z_idx = z_pt.first;
                PointType pt;
                if (use_geometric_center) {
                    pt.x = z_pt.second.first.x / z_pt.second.second;
                    pt.y = z_pt.second.first.y / z_pt.second.second;
                    pt.z = z_pt.second.first.z / z_pt.second.second;
                } else {
                    pt.x = reverse_idx(x_idx, x_grid_nums, center_x);
                    pt.y = reverse_idx(y_idx, y_grid_nums, center_y);
                    pt.z = reverse_idx(z_idx, z_grid_nums, center_z);
                }
                cloud_ptr->points.push_back(pt);
            }
        }
    }

    dst_cloud_ptr = cloud_ptr;
}

实现中提供了返回栅格内点集中心和栅格中心的选项,用以效果比较。在实际使用中可以根据不同需要调整体素内点的计算策略。例如,如果我们对返回点对应的线索引如果有要求,我们可以按线返回几何中心,这样会丧失其线数信息。以下是以 0.1m 的分辨率的下采样结果。

返回栅格中心:

返回栅格内点集的几何中心:

很明显可以看到:如果返回栅格中心的话,点的排布似乎“规整了起来",原来呈环状排布的点变成栅格状排布,一定程度上也丧失原始点云的排布结构。而返回几何中心的例子则较好的保持了点云整体和局部特征。通过下采样后,点云数量从 11 万左右降到了 4万左右,数据规模大约下降 60%。

PCL 实现

PCL 中也提供了体素滤波的方法,具体使用方法如下:

    pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
    voxel_filter.setLeafSize(0.1, 0.1, 0.1);
    voxel_filter.setInputCloud(cloud_ptr);
    voxel_filter.filter(*filtered_cloud_ptr);

滤波后的点云和上图类似,简单进行了一下滤波时间计算,发现在 debug 模式下自己实现的版本相较于 PCL 的体素滤波器速度慢许多,对 12 万点的点云, PCL 的体素滤波耗时大约 6 ms,自己实现的版本耗时大约 15 ms。简单看了一下 PCL 库的实现版本,大致原因可能如下:

  • 体素滤波器中预先保留好了大量空间,而自己实现的函数由于用的是多层哈希表,所以在添加点时会频繁进行空间分配(扩张)
  • 体素滤波器中将三个方向上的栅格索引压缩至一维,因此空间分布更加整齐,可以使用 C 语言上的空间操作来加速
  • 体素滤波器将需要进行的除法操作提前进行处理,因此不用频繁进行除法,而改用乘法,这样在点数多的时候也可以提高一定操作时间

参考

Built with Hugo
Theme Stack designed by Jimmy