深度相机辅助导航避障(三)：地面点云滤除

前面的章节介绍了坐标变换，以及如何设置深度相机的坐标变换。那就可以很直观从机器人的坐标系对深度相机扫描到的障碍物点云进行处理。


在实际应用中，机器人正确估计周围地形，对于道路的可通过性、路径规划和障碍物检测等方面都很重要。那么在获取深度相机点云数据后就得准确分割出地面点云，去除地面对机器人导航的干扰。

对地面点云进行分割去除，通常是使用深度相机辅助导航避障等应用前的第一个步骤。地面点云的准确去除能够显著提升机器人导航的流畅性及避障能力。本文将介绍使用PCL点云处理库对地面点云分割滤除的实现方法。

PCL介绍

PCL（英语：Point Cloud Library）是一个开源的算法库，用于处理点云和计算机视觉中的三维几何过程。

PCL包含点云滤波、特征估计、三维重建、点云配准、模型拟合、目标识别和分割等算法。

对PCL库使用的入门，可以到官方网站https://pointclouds.org/ 进行详细的学习。

PCL库本身已经包含了与ROS点云数据类型进行互相转换的功能，所以在ROS中使用起来非常方便。

地面点云滤除流程

地面点云滤除的主要目的是从原始点云数据中分离出地面部分，保留非地面点云数据。本文将介绍地平面拟合方法以及基于高程差的滤除方法，实现简单、效果稳定。

以下为地面点云滤除的整体运行流程。

1.深度相机ros点云数据转换至机器人坐标系

在前面的章节《设置坐标变换》中已经介绍了如何发布深度相机安装位置的静态TF，如果不熟悉的可以再去翻看一下。

这里我们将获取该tf，并执行坐标变换。

// 转换成机器人坐标系
sensor_msgs::msg::PointCloud2 pointCloudBase;

geometry_msgs::msg::TransformStamped tfStamped;
try {
    tfStamped =
        tfBuffer_->lookupTransform(
        "base_link", msg.header.frame_id,
        tf2_ros::fromMsg(msg.header.stamp));
} catch (tf2::LookupException & ex) {
    RCLCPP_ERROR(
        this->get_logger(),
        "No Transform available Error looking up target frame: %s\n", ex.what());
    return;
}
tf2::doTransform(msg, pointCloudBase, tfStamped);

在获取到tf后，直接通过tf2::doTransform方法，将深度相机点云话题数据转换成基于机器人“base_link”坐标系的pointCloudBase。

2.ros数据类型转成pcl数据类型

ros2点云的数据类型为sensor_msgs::msg::PointCloud2。要使其能够使用pcl库处理，需要将其转为pcl::PointCloud<pcl::PointXYZ>

使用pcl::fromROSMsg方法就可以很方便转换。

pcl::PointCloud<pcl::PointXYZ>::Ptr pclcloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(pointCloudBase, *pclcloud);

至此，得到了能使用pcl库处理的原始点云数据。

3.数据预处理

对原始的点云数据需要进行一些预处理操作，如滤波、去噪、降采样等，以提高后续处理的准确性以及处理效率。

在pcl库中。使用PassThrough filter，可以截掉范围外的点云，只保留想要的范围内的点云。

使用StatisticalOutlierRemoval filter，可以去除一些稀疏的噪声异常点云。

使用VoxelGrid filter，使用体素化网格方法对点云数据进行降采样（即减少点的数量）。

上图是一张深度相机斜向下打的点云图。

可以看到两侧的点云有点畸变，较远的点云比较稀疏不清晰，整体点云密度比较高，没有什么异常点。 因此对它的预处理只需要做直通滤波加体素滤波即可。

滤波器的用法教程可以看官方pcl文档的这几篇：

https://pcl.readthedocs.io/projects/tutorials/en/master/passthrough.html#passthrough

https://pcl.readthedocs.io/projects/tutorials/en/master/statistical_outlier.html#statistical-outlier-removal

https://pcl.readthedocs.io/projects/tutorials/en/master/voxel_grid.html#voxelgrid

我们对上面的点云数据进行预处理后，如下图所示

4.进行平面分割

pcl常用的平面分割方法为使用sac_segmentation模块，基于平面模型使用RANSAC算法进行平面分割。不过该方法一次只能拟合一个平面，可能没法一次就找出地面所在的平面。且由于现实的地面总是不会那么平滑，可能有起伏，可能有小坡度连接，也有可能深度相机本身给出的平面点云就是波动的。所以使用sac_segmentation就没有那么符合地面点云滤除的应用。

这里将介绍使用pcl 的Region growing segmentation（区域生长分割）方法，就能比较好的解决地面点云分割的问题。

pcl在pcl::RegionGrowing类中实现了区域生长算法。

该算法的目的是将在平滑度约束方面足够接近的点集合起来构成区域。输出一组点云簇。

基本思想从点云数据中选取一个或多个点作为种子点（通常选择曲率最小的点作为初始种子点），根据某种相似性准则（如法向量、曲率等），将种子点周围与种子点性质相似（或相同）的邻近点归并到种子点所在的区域中。重复上述过程，将新归并的点作为新的种子点，继续向周围生长，直到没有满足条件的点可以包括进来。

因此它能够解决不太平滑的地面也能分割到同一平面点云簇中。

具体算法理论和示例程序以及如何调参均可以看pcl这一篇文档：https://pcl.readthedocs.io/projects/tutorials/en/master/region_growing_segmentation.html#region-growing-segmentation 进行自行尝试，这里不再重复说明。

对前面预处理过的点云进行区域生长算法处理，分割后的结果，如下图所示。

5.选出地面点云并去除

通过上一个步骤，我们获得了一组分割出各个平面的点云簇。这一步，将选出地面部分的点云簇并去除！

这里将基于比较各个点云簇的高差的方法来滤除地面点云。处理简单且稳定。

方法可以简单描述为

• 计算每个平面点云簇的的平均高度。
• 去掉太低的平面。比较点云簇高度与预设的地面高度，若高度低于地面高度阈值，可以直接去除该点云簇。
• 选出剩余点云簇中高度最低的，就是地面点云簇，将其去除，则处理完成。

pcl点云去除的教程可以看这篇文档：https://pcl.readthedocs.io/projects/tutorials/en/master/extract_indices.html#extract-indices

最终处理完的结果如下图所示：

6.转换回ros点云输出结果数据

使用pcl::toROSMsg方法就可以将pcl数据类型转回ros点云。

然后再进行一次tf变换将其还原回深度相机坐标系，即可输出去掉地面点云的结果数据。