2d激光slam点云滤波处理与cpu占用率

三十度角阳光的问候

已于 2024-10-01 22:21:47 修改

阅读量701

点赞数 10

文章标签：算法计算机视觉

于 2024-05-09 19:44:48 首次发布

本文链接：https://blog.csdn.net/weixin_43802726/article/details/138627848

版权

步骤简述

在2d激光slam 的建图定位中，会接收来着激光雷达传感器的数据，原始的点云数据是不能使用的，需要经过滤波处理，现将其常见用法步骤简述如下。

距离滤波

从ros话题中接收的数据会包含点云数据以及里程计数据等，其中对于激光雷达点云数据来说原始雷达数据包含噪点和异常点，首先在接收的回调函数中要对超过雷达最远测试距离例如100米范围的点进行滤波，其次要对强度不够的点进行滤波。滤波后的点云可以传入定位和匹配等处理函数进行处理。

动态障碍物的滤除

到定位和匹配等流程中的点云，首先需要进行动态障碍物的滤除，保证过滤掉动态障碍物的点云，之后进入csm扫描匹配的点云数据要进行最大最小距离的判断，保证所有点云到雷达预测位姿中心点的距离在一个常识范围内，这个范围可以从0到std：：numeric_limit<float>max（）也可以是从雷达盲区到最远雷达测试距离。

以移动机器人（AGV类）的使用场景为例，其基础使用场景是二维室内环境，所以使用的SLAM算法和机器人位姿估计器模型都是基于2D平面操作。那基于2D平面场景，SLAM中最常见、最经典的算法是gmapping、cartographer或者karto，定位中常用的算法是EKF、AMCL和ICP的组合，导航中的路径规划和运动控制就都是基于概率栅格地图（pgm）的平面规划了。由此可见，基于移动机器人（AGV）的SLAM、定位和导航都离不开概率栅格地图（pgm），而在概率栅格地图上最传统和最有效的去除动态障碍物的方法就是利用“ray casting算法”的栅格地图概率的“更新”，而“更新”的最终目的是为了在栅格地图上不留下动态障碍物的hit点（此处的更新之所以打上引号，是因为更新概率栅格的模型有很多，用引号表示泛指）。

体素滤波

在进行点云物理坐标到栅格坐标索引转换后要进行体素滤波即降采样操作，将属于同一栅格内的点云只保留一个，以便在后续匹配以及画线建图过程中避免重复计算，体素滤波方法可以采用c++的迭代器查找。

经过上述点云处理如果剩余的点云点数量大于一定值例如不为空且大于100个点则可以参与匹配和建图操作。在建图中又涉及栅格地图的匹配搜索步长以及搜索角度步长以及角度和栅格搜索范围，步长太大则计算结果不精确，步长太小计算精确但会导致cpu占用率增加，因此要根据cpu占用率和栅格地图匹配精度以及cpu性能综合确定步长以及范围参数，在cpu性能准许的基础上尽量减小搜索步长提高精度。

‌体素滤波是一种常用的点云下采样方法，它通过将点云划分为多个体素，并用每个体素的中心点来近似体素内的所有点，从而实现点云数据的简化。这种方法不仅可以提高处理效率，还能在一定程度上去除噪声点和离群点。‌

以下是使用PCL库实现体素滤波的代码示例：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>

int main() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("input_cloud.pcd", *cloud);

    pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
    voxel_filter.setInputCloud(cloud);
    voxel_filter.setLeafSize(0.01f, 0.01f, 0.01f); // 设置体素大小
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    voxel_filter.filter(*filtered_cloud);

    pcl::visualization::CloudViewer viewer("Voxel Filter");
    viewer.showCloud(filtered_cloud);
    return 0;
}

这段代码首先加载一个PCD格式的点云文件，然后创建一个VoxelGrid滤波器，设置体素的大小，并对点云进行滤波处理。最后，使用PCL Visualizer可视化滤波后的点云。