前言
-
本教程使用PCL对ROS深度相机捕获到的画面进行操场上锥桶的分割
-
本人相关的RGBD深度相机原理及其使用教程：
-
关于PCL的学习教程强烈推荐双愚大大的文章：PCL(Point Cloud Library)学习指南&资料推荐（2024版）
-
同样不错的基础入门：黑马机器人PCL教程
-
本文使用的环境和设备
- ubuntu 20.04LTS
- ros1 noetic
- Astra S 深度相机
- pcl-1.10
-
上一期我们讲到PCL库的安装和基础使用，并介绍了如何使用PCL库对
sensor_msgs/PointCloud2消息转换为pcl::PointCloud，并使用pcl::visualization::PCLVisualizer. -
本期我们将使用
Plane Model Segmentation实现平面分割进行可视化达到如下效果,同时学习原理及其代码实现：
1 分割Segmentation
- 在开始PCL进行点云分割之前，我们先了解一下在图像处理领域中，分割是什么意思
1-1 概念
- 图像分割（Image Segmentation）是计算机视觉和图像处理中的一个重要任务，其目的是将图像划分为若干个具有相似属性的区域。这些属性可以是颜色、纹理、形状、亮度、空间关系等。通过图像分割，可以将图像中的对象从背景中分离出来，以便于进一步的图像分析或处理。
- 图像分割的方法有很多，按照分割技术可以分为以下几类：
- 基于阈值的分割（Threshold-based Segmentation）：通过设定一个或多个阈值，将图像中的像素分为两个或多个类别。常用的方法有全局阈值分割和自适应阈值分割。
- 基于边缘的分割（Edge-based Segmentation）：通过检测图像中的边缘，将图像分割成多个部分。常用的方法有Sobel算子、Canny边缘检测等。
- 基于区域的分割（Region-based Segmentation）：基于区域的相似性，将图像分割成多个区域。常用的方法有区域生长、分水岭变换等。
- 基于图的分割（Graph-based Segmentation）：将图像视为一个图，通过图的优化方法来分割图像。常用的方法有最小割（Min-cut）和最大流（Max-flow）等。
- 基于聚类的分割（Clustering-based Segmentation）：通过聚类算法，将图像中的像素分为多个类别。常用的方法有K-means聚类、模糊C-means聚类等。
- 基于深度学习的分割（Deep Learning-based Segmentation）：利用深度神经网络模型，如卷积神经网络（CNN），进行图像分割。这种方法近年来取得了很大的进展，如U-Net、SegNet、FCN等。
- 图像分割的应用非常广泛，包括医学影像分析、自动驾驶、机器人导航、图像识别等领域。通过图像分割，可以提取图像中的关键信息，为后续的图像处理和分析提供便利。
1-2 PCL中的Segmentation
-
我们进入官网的首页链接，可以看到Segmentation模块
-
PCL中的分割模块提供了多种算法，用于将点云分割成不同的簇（clusters）。这些算法特别适用于处理由空间上隔离的区域组成的点云。在这种情况下，聚类通常用于将点云分解为其组成部分，然后可以独立处理每个部分。
-
PCL中的分割算法主要包括以下几种：官网导引
基于欧几里得距离的聚类（Euclidean Clustering）：这种算法将点云中的点根据它们之间的欧几里得距离进行分组。通常，它会设置一个距离阈值，将距离小于该阈值的点归为一个簇。基于平面模型的分割（Plane Model Segmentation）：这种算法首先检测点云中的平面，然后使用平面模型来分割点云。它通常用于从点云中移除地面或桌子等平面区域。基于区域的增长（Region Growing：这种算法从一个或多个种子点开始，逐渐增长区域，直到满足特定的条件（如最大距离或区域大小）。基于栅格的分割（Grid-based Segmentation）：这种算法将点云分割成规则的三维网格，然后对每个网格内的点进行聚类。基于模型的分割（Model-based Segmentation）：这种算法使用预定义的模型（如球体、圆柱体、圆锥体等）来分割点云，通常用于识别特定的物体或结构。基于随机样本一致性的分割（Random Sample Consensus, RANSAC）：这种算法用于估计数据中的数学模型，如平面或直线，然后使用这些模型来分割点云。
2 Plane Model Segmentation原理和实践
- 本节我们利用
基于平面模型的分割（Plane Model Segmentation）来实现我们的分离目标
2-1 算法原理
-
PCL中的平面模型分割（Plane Model Segmentation）算法通常指的是
RANSAC（Random Sample Consensus）算法在点云分割中的应用。RANSAC是一种迭代算法，用于估计数据中的数学模型，如平面，并从数据中移除或分离出该模型。
-
平面模型分割的原理如下：
- 选择随机样本：从点云中随机选择三个点（对于平面模型）作为初始假设。这三个点可以定义一个平面。
- 计算模型参数：使用这三个点计算平面的方程参数。平面方程通常表示为
ax + by + cz + d = 0，其中 a、b、c 和 d 是平面方程的系数。 - 内点计数：对于点云中的每个点，计算它到平面的距离。如果距离小于某个阈值，则该点被认为是平面上的内点。统计所有内点的数量。
- 评估模型：根据内点的数量评估当前模型的优劣。如果内点数量超过某个阈值，则认为找到了一个有效的平面模型。
- 迭代优化：如果找到了一个有效的模型，可以使用内点来重新估计平面参数，以提高模型的准确性。如果没有找到有效的模型，则重复步骤1到4，使用不同的随机样本进行迭代。
- 分割点云：一旦找到了一个或多个有效的平面模型，可以将点云分割为属于不同平面的点。通常，分割后的点云可以用于进一步的处理，如移除地面或检测障碍物。
- 下面我将依次介绍出现在代码中的部分，最后展示完整代码
2-2 初始化参数
- 头文件包含
#include <pcl/segmentation/sac_segmentation.h>: 包含了PCL中用于分割的SAC（Sample Consensus）类，这是用于平面模型分割的关键类。#include <pcl/sample_consensus/model_types.h>: 包含了用于定义分割模型类型的头文件。#include <pcl/sample_consensus/method_types.h>: 包含了用于定义分割方法的头文件。
- 成员变量:
pcl::SACSegmentation<pcl::PointXYZ> seg: 创建了一个SAC分割对象，用于执行平面模型分割。pcl::ModelCoefficients::Ptr coefficients: 用于存储分割出的平面模型的系数。pcl::PointIndices::Ptr inliers: 用于存储分割平面时的内点索引。
- 配置平面模型分割
// 可选配置：是否优化模型系数
seg.setOptimizeCoefficients(true);
// 必选配置：设置分割的模型类型、分割算法、距离阈值、输入点云
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.01);
seg.setOptimizeCoefficients(true);设置了SAC分割器是否应该优化分割出的模型的系数。如果设置为true，算法会在找到初始模型后尝试优化模型的参数，以提高模型的准确性。这对于确保分割出的平面尽可能精确地拟合数据是有帮助的。seg.setModelType(pcl::SACMODEL_PLANE);设置了分割器要估计的模型类型。在这里，SACMODEL_PLANE指定了算法应该寻找平面模型。这意味着算法会尝试找到一个最佳拟合的平面来分割点云。seg.setMethodType(pcl::SAC_RANSAC);设置了分割算法的方法类型。SAC_RANSAC指定了使用随机样本一致性（RANSAC）方法。seg.setDistanceThreshold(0.02);设置了模型内点与模型之间的最大允许距离。在这里，距离阈值被设置为0.02。这意味着任何与估计的平面距离小于0.02的点都被认为是内点（inliers），而距离大于这个阈值的点则被排除在平面模型之外。
2-3 滤波
- 由于深度相机RGBD捕获到的是640*480的深度点云信息，这过于冗余了，这里我们选择
体素下采样进行信息简化。 体素下采样是一种损失信息的操作，因为它减少了点云中的数据点，但它可以显著提高处理速度，特别是在进行分割、特征提取或其他需要迭代算法的步骤时。通过调整体素大小，可以控制点云的分辨率和细节级别。- 头文件
#include <pcl/filters/voxel_grid.h>
- 在PCL中，
pcl::VoxelGrid类是一个用于执行体素下采样的过滤器。体素下采样是一种常用的预处理步骤，用于减少点云中的点数，同时保留点云的基本形状和结构。
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud (raw_cloud.makeShared());
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (raw_cloud);
pcl::VoxelGrid<pcl::PointXYZ> sor;创建了一个名为sor的VoxelGrid对象，它专门用于处理PointXYZ类型的点云数据。sor.setInputCloud (raw_cloud.makeShared());将原始点云raw_cloud作为输入传递给体素下采样过滤器。makeShared()方法用于从raw_cloud创建一个共享指针，以便在过滤过程中安全地使用它。sor.setLeafSize (0.01f, 0.01f, 0.01f);设置体素的大小。这里的参数0.01f表示在x、y和z方向上每个体素的大小。这意味着原始点云中的点将被分组到边长为0.01的立方体体素中。sor.filter (raw_cloud);执行体素下采样过滤。这个步骤会将原始点云中的点分配到相应的体素中，并为每个体素保留一个点（通常是体素中心或最接近体素中心的点）。这样，每个体素只贡献一个点，从而减少了点云中的点数。
2-4 分割执行
- 分割
seg.setInputCloud(raw_cloud.makeShared());
// 执行分割操作，并存储分割结果保存到点集合 inliers 及存储平面模型系数 coefficients
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0) {
PCL_ERROR ("Could not estimate a planar model for the given dataset.");
continue;
}
seg.setInputCloud(raw_cloud.makeShared());将原始点云raw_cloud作为输入传递给SAC分割器。makeShared()方法用于从raw_cloud创建一个共享指针，以便在分割过程中安全地使用它。seg.segment(*inliers, *coefficients);segment方法尝试在点云中找到一个平面模型，并将分割结果存储在inliers和coefficients中。*inliers是一个pcl::PointIndices对象，它将存储属于平面模型的点的索引。*coefficients是一个pcl::ModelCoefficients对象，它将存储平面模型的参数，通常是一个平面方程的四个系数ax + by + cz + d = 0。
if (inliers->indices.size() == 0) { ... }检查inliers中存储的索引数量。如果inliers->indices.size()等于0，意味着没有找到内点，即没有足够的点与平面模型一致，因此无法估计出一个有效的平面模型。- 值得一提的是下述代码可以看到
ax + by + cz + d = 0。的四个参数ABCD
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
2-5 分割可视化
- 提取平面点云:
pcl::PointCloud<pcl::PointXYZ> plane_cloud;
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(raw_cloud.makeShared());
extract.setIndices(inliers);
extract.setNegative(false); // 设置为false以提取平面点云
extract.filter(plane_cloud);
pcl::PointCloud<pcl::PointXYZ> plane_cloud;: 创建一个新的点云对象plane_cloud，用于存储提取出的平面点云。pcl::ExtractIndices<pcl::PointXYZ> extract;: 创建一个提取索引的对象，用于从点云中提取点。extract.setInputCloud(raw_cloud.makeShared());: 设置提取操作输入的点云为raw_cloud。extract.setIndices(inliers);: 设置内点索引，这些索引是从平面分割步骤中获得的，代表属于平面的点。extract.setNegative(false);: 设置为false表示提取与索引匹配的点（即平面点云）。extract.filter(plane_cloud);: 执行提取操作，将平面点云存储在plane_cloud中。- 同理提取剩余点云:
pcl::PointCloud<pcl::PointXYZ> remaining_cloud;
extract.setNegative(true); // 设置为true以提取非平面点云
extract.filter(remaining_cloud);
- 在PCL可视化器中显示点云,上一节已经说明了，这就不说明了
viewer.removeAllPointClouds();
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> plane_color_handler(plane_cloud.makeShared(), 255, 0, 0);
viewer.addPointCloud<pcl::PointXYZ>(plane_cloud.makeShared(), plane_color_handler, "plane cloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "plane cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> remaining_color_handler(remaining_cloud.makeShared(), 0, 255, 0);
viewer.addPointCloud<pcl::PointXYZ>(remaining_cloud.makeShared(), remaining_color_handler, "remaining cloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "remaining cloud");
viewer.setBackgroundColor(0, 0, 0);
viewer.spinOnce(0.0001);
3 完整代码和效果展示
- 代码如下
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <cv_bridge/cv_bridge.h> // Include c to convert ROS images to OpenCV images
#include <opencv2/opencv.hpp>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
using namespace cv;
class PCL_center
{
private:
ros::NodeHandle nh;
ros::Subscriber imageSub;
ros::Subscriber depthImageSub;
ros::Subscriber depthPCloudSub;
ros::Subscriber pointCloudSub;
pcl::visualization::PCLVisualizer viewer;
pcl::PointCloud<pcl::PointXYZ> raw_cloud;
pcl::ModelCoefficients::Ptr coefficients;
pcl::PointIndices::Ptr inliers;
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
void imageCB(const sensor_msgs::ImageConstPtr& img_msg)
{
// // Convert ROS image message to OpenCV image
// cv_bridge::CvImagePtr cv_ptr;
// try
// {
// cv_ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::BGR8);
// }
// catch (cv_bridge::Exception& e)
// {
// ROS_ERROR("cv_bridge exception: %s", e.what());
// return;
// }
// std::cout << "imageCB()" << std::endl;
// imshow("Input Window", cv_ptr->image);
// waitKey(1);
}
void depthImageCB(const sensor_msgs::ImageConstPtr& dimg_msg)
{
// // Your depth image processing code here
// std::cout << "depthImageCB()" << std::endl;
// cv_bridge::CvImagePtr cv_ptr;
// try
// {
// // 注意这里使用的是sensor_msgs::image_encodings::TYPE_16UC1
// cv_ptr = cv_bridge::toCvCopy(dimg_msg, sensor_msgs::image_encodings::TYPE_16UC1);
// }
// catch (cv_bridge::Exception& e)
// {
// ROS_ERROR("cv_bridge exception: %s", e.what());
// return;
// }
// Mat depth_display;
// cv_ptr->image.convertTo(depth_display, CV_8U, 255.0 / 1000);
// // Display the depth image
// imshow("Depth Window", depth_display);
// waitKey(0.1);
}
void depthPCloudCB(const sensor_msgs::PointCloud2ConstPtr& pc_msg)
{
// // Your point cloud processing code here
// std::cout << "depthPCloudCB()" << std::endl;
}
void pointCloudCB(const sensor_msgs::PointCloud2& pc_msg)
{
// 检查点云是否为空
if (pc_msg.width * pc_msg.height == 0)
{
std::cout << "Received an empty point cloud message." << std::endl;
return; // 如果消息为空，则返回
}
std::cout << "pointCloudCB()" << std::endl;
// 从sensor_msgs/PointCloud2消息转换为PCL点云
pcl::fromROSMsg(pc_msg, raw_cloud);
}
public:
PCL_center(ros::NodeHandle& nh_) : nh(nh_),viewer("3D Viewer"),coefficients(new pcl::ModelCoefficients),inliers(new pcl::PointIndices)
{
imageSub = nh.subscribe("/camera/color/image_raw", 10, &PCL_center::imageCB, this);
depthImageSub = nh.subscribe("/camera/depth/image_raw", 10, &PCL_center::depthImageCB, this);
depthPCloudSub = nh.subscribe("/camera/depth/points", 10, &PCL_center::depthPCloudCB, this);
pointCloudSub=nh.subscribe("/camera/depth/points",10,&PCL_center::pointCloudCB,this);
// 可选配置：是否优化模型系数
seg.setOptimizeCoefficients(true);
// 必选配置：设置分割的模型类型、分割算法、距离阈值、输入点云
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.02);
}
void run()
{
while(ros::ok())
{
ros::spinOnce();
if (raw_cloud.width * raw_cloud.height == 0)
{
std::cout << "Received an empty point cloud message." << std::endl;
continue; // 如果消息为空，则返回
}
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud (raw_cloud.makeShared());
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (raw_cloud);
seg.setInputCloud(raw_cloud.makeShared());
// 执行分割操作，并存储分割结果保存到点集合 inliers 及存储平面模型系数 coefficients
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0) {
PCL_ERROR ("Could not estimate a planar model for the given dataset.");
continue;
}
// 提取平面点云
pcl::PointCloud<pcl::PointXYZ> plane_cloud;
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(raw_cloud.makeShared());
extract.setIndices(inliers);
extract.setNegative(false); // 设置为false以提取平面点云
extract.filter(plane_cloud);
// 提取剩余点云
pcl::PointCloud<pcl::PointXYZ> remaining_cloud;
extract.setNegative(true); // 设置为true以提取非平面点云
extract.filter(remaining_cloud);
// 清除所有点云并重新添加
viewer.removeAllPointClouds();
// 添加平面点云到可视化窗口，并设置颜色为红色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> plane_color_handler(plane_cloud.makeShared(), 255, 0, 0);
viewer.addPointCloud<pcl::PointXYZ>(plane_cloud.makeShared(), plane_color_handler, "plane cloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "plane cloud");
// 添加剩余点云到可视化窗口，并设置颜色为绿色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> remaining_color_handler(remaining_cloud.makeShared(), 0, 255, 0);
viewer.addPointCloud<pcl::PointXYZ>(remaining_cloud.makeShared(), remaining_color_handler, "remaining cloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "remaining cloud");
// 设置背景色
viewer.setBackgroundColor(0, 0, 0);
viewer.spinOnce(0.0001);
}
}
};
int main(int argc, char *argv[])
{
ros::init(argc, argv, "pcl_test");
ros::NodeHandle nh;
PCL_center pcl_center(nh);
pcl_center.run();
namedWindow("Input Window", WINDOW_AUTOSIZE);
ros::spin();
destroyWindow("Input Window");
return 0;
}
- 效果：
4 总结
- 本节讲述如何讲述了PCL中Plane Model Segmentation的原理和代码时间操作及其细节
- 下一节将讲讲如何在2D图像中获取物体信息以实现类似图像的效果
- 如有错误，欢迎指出！！！感谢大家的支持
扫一扫
1106
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
