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

分类：菜鸟笔记作者：lovely_yoshino

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

2.2 三维激光雷达压缩成二维

…详情请参照古月居

版权声明：本文内容由互联网用户自发贡献，该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容， 请发送邮件至 dio@foxmail.com 举报，一经查实，本站将立刻删除。

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

相关推荐

如何学习编程？新手菜鸟如何学编程？
学习编程是顺着互联网的发展潮流，是一件好事。新手如何学习编程？其实不难，不过在学习编程之前你得先了解你的目的是什么？这个很重要，因为目的决定你的发展方向、决定你的发展速度。
it行业都是什么工作？it行业主要做什么？
IT行业是什么工作做什么？IT行业的工作有：产品策划类、页面设计类、前端与移动、开发与测试、营销推广类、数据运营类、运营维护类、游戏相关类等，根据不同的分类下面有细分了不同的岗位。
女生学Java好就业吗？女生适合学Java编程吗？
女生学Java好就业吗？女生适合学Java编程吗？目前有不少女生学习Java开发，但要结合自身的情况，先了解自己适不适合去学习Java，不要盲目的选择不适合自己的Java培训班进行学习。只要肯下功夫钻研，多看、多想、多练
mysql-connect-confused
Can’t connect to local MySQL server through socket \'/var/lib/mysql/mysql.sock问题 1.进入mysql路径
oracle基本命令
oracle基本命令 一、登录操作 1.管理员登录 # 管理员登录 sqlplus / as sysdba 2.普通用户登录
shell判断服务是否启动
一、背景 因为项目中需要通北京网络，所以需要连vpn，但是服务器有时候会断掉，所以写个shell脚本每五分钟去判断是否连接，于是就有下面的shell脚本。
SQL BETWEEN 操作符
BETWEEN 操作符选取介于两个值之间的数据范围内的值。这些值可以是数值、文本或者日期。
​​windows上传ipa到开发者中心app store的方法
假如你已经使用过苹果开发者中心上架app，你肯定知道在苹果开发者中心的web界面，无法直接提交ipa文件，而是需要使用第三方工具，将ipa文件上传到构建版本，开...
SQL 入门篇之什么是别名？
下面的 SQL 语句指定了两个别名，一个是 name 列的别名，一个是 country 列的别名。**提示：**如果列名称包含空格，要求使用双引号或方括号：
windows上传ipa文件到app store的方法​
在使用H5混合开发的app打包后，需要将ipa文件上传到appstore进行发布，就需要去苹果开发者中心进行发布。​
SQL AND & OR 运算符
+----+--------------+---------------------------+-------+---------+
C++ 数组
数组的声明并不是声明一个个单独的变量，比如 number0、number1、...、number99，而是声明一个数组变量，比如 numbers，然后使用 nu...
最全iOS 应用上架流程提交到AppStore
第一步：到appuploader官网下载辅助工具和iCloud驱动，使用前面创建的AppID登录。
SQL ALTER TABLE 语句
如需删除表中的列，请使用下面的语法（请注意，某些数据库系统不允许这种在数据库表中删除列的方式）：
dism操作巨慢
前不久在制作win11pe，制作了一版，1.26GB，太大了，不满意，想再裁剪下，发现这次dism mount正常，commit或discard巨慢，以前都很快...
三种方法卸载Symantec Endpoint Protection 赛门铁克
赛门铁克各个版本概览：https://knowledge.broadcom.com/external/article?legacyId=tech163829
python3.6、python3.10用什么版本的pip
实测Python 3.6.6用pip 21.3.1，再高就报错了，Python 3.10.7用pip 22.3.1是可以的
博通和诺顿
Broadcom Corporation （博通公司，股票代号AVGO）是全球领先的有线和无线通信半导体公司。其产品实现向家庭、 办公室和移动环境以及在这些环境...
高版本c4d在低版本windows系统上打开后不显示窗格
发现个问题，server2016上安装了c4d这些版本，低版本的正常显示窗格，但红色圈出的高版本c4d打开后不显示窗格，
基础网络windows不要使用TAT、如何卸载TAT
TAT：https://cloud.tencent.com/document/product/1340
pythonJavaScriptjavaHTMLPHPreactjsC#AndroidCSSNode.jssqlrpython-3.xMysqLjQueryc++pandasFlutterangularIOSdjangolinuxswifttypescript路由器JSON路由器设置无线路由器h3c华三华三路由器设置华三路由器电脑软件教程arraysdocker软件图文教程Cvue.jslaravelspring-boot