自动驾驶-激光雷达预处理/特征提取

原创已于 2024-08-26 10:30:26 修改 · 置顶 · 4.9k 阅读

32 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶 #人工智能 #计算机视觉

于 2022-08-24 21:12:44 首次发布

slam 专栏收录该内容

208 篇文章

订阅专栏

本文介绍了自动驾驶场景下激光雷达点云的预处理方法，包括指定区域获取、去除给定区域的点、点云下采样等，并探讨了不同特征提取手段，如时间序列排序和三维压缩成二维。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

0. 简介

激光雷达作为自动驾驶最常用的传感器，经常需要使用激光雷达来做建图、定位和感知等任务。而这时候使用降低点云规模的预处理方法，可以能够去除无关区域的点以及降低点云规模。并能够给后续的PCL点云分割带来有效的收益。

1. 点云预处理

1.1 指定区域获取点云

在实际使用中，我们可以看出，虽然点云的分布范围较广，但大部分的点都集中的中间区域，距离越远点云越稀疏，相对的信息量也越小。此外还能明显看到一些离群点，因此我们可以筛选掉一些较远的点，只保留我们感兴趣范围内的点。以下为保留 x 在 30m，y 在 15m，z 在 2m 范围内的点的效果：

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;
}


 // 或者使用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);

在这里插入图片描述

1.2 去除给定区域的点

在某些情况下，我们也会需要去除给定区域内部的点，比如在自动驾驶中激光扫描的区域有一部分来自搭载激光雷达的车子本身

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;
}

// PassThrough: 可以指定点云中的点的某个字段进行范围限制，将其设为 true 时可以进行给定只保留给定范围内的点的功能
 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);

在这里插入图片描述

1.3 点云下采样

1.3.1 栅格化采样

这里第一点介绍栅格化的下采样，在 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);

在这里插入图片描述

1.3.2 点云所在区域密度规律滤波

该方法直接基于点云分布密度进行去噪，直观的感受是可以根据点云中每个点所在区域判断其是否是噪声，一般来说噪声点所在区域都比较稀疏。

    pcl::RadiusOutlierRemoval<pcl::PointXYZ>::Ptr radius_outlier_removal(
        new pcl::RadiusOutlierRemoval<pcl::PointXYZ>(true));
    radius_outlier_removal->setInputCloud(cloud_ptr);
    radius_outlier_removal->setRadiusSearch(1.0);
    radius_outlier_removal->setMinNeighborsInRadius(10);
    radius_outlier_removal->filter(*filtered_cloud_ptr);

在这里插入图片描述

1.3.3 点云所在区域分布规律滤波

除了根据稠密意以外还可以根据距离来筛选滤波，每个点计算其到周围若干点的平均距离，如果这个平均距离相对于整体点云中所有点的平均距离较近，则认为其不是噪点

    // PCL built-in radius removal
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ>::Ptr statistical_outlier_removal(
        new pcl::StatisticalOutlierRemoval<pcl::PointXYZ>(true)); // set to true if we want to extract removed indices
    statistical_outlier_removal->setInputCloud(cloud_ptr);
    statistical_outlier_removal->setMeanK(20);
    statistical_outlier_removal->setStddevMulThresh(2.0);
    statistical_outlier_removal->filter(*filtered_cloud_ptr);

1.3.4 根据点云是否可以被稳定观察到筛选

LOAM 中对点云中的点是否能形成可靠特征的一个判断标准是它能否被稳定观察到。LOAM 中着重提了这两种情况的点是不稳定的：

特征成平面和扫描线近乎平行
特征扫描到的其中一端被另一个平面挡住，这部分的点也不稳定

template <typename PointType>
void filter_occuluded_points(boost::shared_ptr<pcl::PointCloud<PointType>>& src_cloud_ptr,
                             boost::shared_ptr<pcl::PointCloud<PointType>>& dst_cloud_ptr,
                             int neighbor_count,
                             float distance_threshold,
                             float horizontal_angle_diff_threshold,
                             boost::shared_ptr<std::vector<int>> removed_indices = nullptr) {
    int cloud_size     = src_cloud_ptr->points.size();
    distance_threshold = std::fabs(distance_threshold);

    boost::shared_ptr<pcl::PointCloud<PointType>> cloud_ptr(new pcl::PointCloud<PointType>());
    cloud_ptr->points.reserve(cloud_size);

    std::vector<int> status(cloud_size, 0);

    for (int i = neighbor_count; i < cloud_size - neighbor_count; ++i) {
        const PointType& pt1 = src_cloud_ptr->points[i];
        const PointType& pt2 = src_cloud_ptr->points[i + 1];

        double horizontal_angle_1 = std::atan2(pt1.y, pt1.x) / M_PI * 180.0;
        double horizontal_angle_2 = std::atan2(pt2.y, pt2.x) / M_PI * 180.0;

        if (std::fabs(horizontal_angle_1 - horizontal_angle_2) > horizontal_angle_diff_threshold) continue;

        float range1 = std::sqrt(pt1.x * pt1.x + pt1.y * pt1.y + pt1.z * pt1.z);
        float range2 = std::sqrt(pt2.x * pt2.x + pt2.y * pt2.y + pt2.z * pt2.z);

        if (range1 - range2 > distance_threshold)  // pt1 is occluded
        {
            for (int j = i; j >= i - neighbor_count; j--) {
                status[j] = 1;
            }
        } else if (range2 - range1 > distance_threshold) {  // pt2 is occluded
            for (int j = i + 1; j <= i + neighbor_count; j++) {
                status[j] = 1;
            }
        }
    }

    for (int i = 0; i < cloud_size; ++i) {
        if (status[i] == 0) {
            cloud_ptr->points.push_back(src_cloud_ptr->points[i]);
        } else if (removed_indices != nullptr) {
            removed_indices->push_back(i);
        }
    }

    dst_cloud_ptr = cloud_ptr;
}

在这里插入图片描述

2. 特征提取

2.1 激光雷达时间序列

这一帧数据中点的排列顺序为从最高的线束到最低的线束进行排列，每条线束之间点按逆时针的顺序排列。目前大部分主流激光雷达应该都可以直接在点云中提供每个点对应的扫描线已经时间戳，所以其实可以不用这种粗略的方法来进行计算。

template <typename PointType>
void sortPointCloudByScanLine(boost::shared_ptr<pcl::PointCloud<PointType>>& src_cloud_ptr,
                              std::vector<boost::shared_ptr<pcl::PointCloud<PointType>>>& dst_cloud_ptr_vec) {
    const int N_SCAN = 64;
    dst_cloud_ptr_vec.resize(N_SCAN);
    dst_cloud_ptr_vec.clear();

    PointType pt;
    int line_id;

    for (int i = 0; i < src_cloud_ptr->points.size(); ++i) {
        pt      = src_cloud_ptr->points[i];
        line_id = 0;

        double elevation_angle = std::atan2(pt.z, std::sqrt(pt.x * pt.x + pt.y * pt.y)) / M_PI * 180.0;

        // adapt from a-loam
        if (elevation_angle >= -8.83)
            line_id = int((2 - elevation_angle) * 3.0 + 0.5);
        else
            line_id = 64 / 2 + int((-8.83 - elevation_angle) * 2.0 + 0.5);

        if (elevation_angle > 2 || elevation_angle < -24.33 || line_id > 63 || line_id < 0) {
            continue;
        }

        if (dst_cloud_ptr_vec[line_id] == nullptr)
            dst_cloud_ptr_vec[line_id] = boost::make_shared<pcl::PointCloud<PointType>>();
        dst_cloud_ptr_vec[line_id]->points.push_back(pt);
    }
}