前言
-
本教程使用PCL对ROS深度相机捕获到的画面进行操场上锥桶的分割
- 上:[csdn 博客] 【PCL实现点云分割】ROS深度相机实践指南（上）:PCL库初识和ROS-PCL数据类型转换
- 中:[csdn 博客] 【PCL实现点云分割】ROS深度相机实践指南（中）:Plane Model Segmentation实现平面分割原理及其实践
-
本人相关的RGBD深度相机原理及其使用教程：
- [csdn 博客] 上手一个RGBD深度相机：从原理到实践–ROS noetic+Astra S（上）：解读深度测距原理和内外参推导
- [csdn 博客] 上手一个RGBD深度相机：从原理到实践–ROS noetic+Astra S（中）：RGB相机的标定和使用
-
关于PCL的学习教程强烈推荐双愚大大的文章：PCL(Point Cloud Library)学习指南&资料推荐（2024版）
-
同样不错的基础入门：黑马机器人PCL教程
-
本文使用的环境和设备
- ubuntu 20.04LTS
- ros1 noetic
- Astra S 深度相机
- pcl-1.10
-
上一节讲述如何使用PCL中
Plane Model Segmentation的原理和代码实践操作及其细节，这一节我们来讲讲点云轮廓提取的原理，及其如何使用pcl::BoundaryEstimation对提取到的点云信息进行轮廓提取进行代码实操，然后映射到2D平面上(类似于cv的轮廓检测)，得到如下图的结果。
1 论文解读Detecting Holes in Point Set Surfaces
- 在正式开始我们的代码实操环节之前，我们来阅览一下这篇论文。今天我们要使用到的
pcl::BoundaryEstimation类进行点云轮廓检测的原理和下述这篇论文中提到的点云轮廓检测原理有异曲同工之处，通过阅读这篇论文，能更好的理解空间中点云轮廓检测的原理，方便我们进行代码书写和记忆。 Detecting Holes in Point Set Surfaces这篇论文提出了一种基于概率的孔洞检测方法，通过结合多种边缘检测准则，利用点云数据中的局部邻域信息，有效地检测出点云表面上的孔洞。该方法具有较高的鲁棒性、可调性和效率，可以应用于多种场景，例如表面重建、数据采集、表面修复和模型编辑等。 [Detecting Holes in Point Set Surfaces 论文链接](NimbusSanL-Regu (zcu.cz))
1-1 总览
-
根据这篇论文的3，4，5节，我们大致可以把整个3D点云轮廓分为如下几个步骤
- 邻域构建（见3，4.1）： 首先，需要构建每个点的局部邻域。论文中提出了一种对称邻域构建方法
The symmetric kε neighbourhood，该方法能够有效地过滤掉采样密度突变的影响，从而提高边缘检测的鲁棒性。 - 边缘检测准则（见4.2~4.4）： 利用多种边缘检测准则，包括角度准则
Angle Criterion、半圆准则Halfdisc Criterion和形状准则Shape Criterion，计算每个点的边界概率。每种准则都从不同的角度对点云数据进行分析，并判断哪些点更有可能是边界点。 - 概率融合（见4.5）： 将每种准则的计算结果进行融合，得到一个综合的边界概率
Π(p)。综合概率反映了每个点属于边界点的可能性。 - 边界一致性处理（见5.1）： 利用边界点之间的一致性，使用
Kruskal 算法构建基于邻域图G的最小生成图MSG,进一步确认边界点，并提取出包围空洞的闭合循环。 - 轮廓提取（见5.2）： 通过
BFS遍历闭合循环，提取出点云的轮廓信息。
- 邻域构建（见3，4.1）： 首先，需要构建每个点的局部邻域。论文中提出了一种对称邻域构建方法
-
在论文的第3小节提到了关于整个算法的一个基本概述：
-
这段概述结合第3小节提出的边界计算算子，解释了算法通过计算每个在P点集中的点的边界概率Π§，来判断这个点是否属于点云边界。
-
下面我们来分别看上述步骤涉及到的每一部分内容解读：
1-2 邻域构建（见3，4.1）
1-2-1 三种邻域
-
对于点集中的一个点p，判断它为边界点的条件往往不是p本身，而是由p周围的领域点决定的
-
故论文的4.1小节中讨论了三种关于一个点p领域的确定方法：
k-邻域:包含距离点p最近的k个样本点。这种定义简单直观，但在采样密度变化较大的区域(例如边缘区域)会出现偏差，导致邻域被密集采样区域所主导（如上左图，取的点受到了密集点集的影响）kε-邻域: 包含距离点 p 最近的k个样本点，以及距离点p中心在 ε ε ε范围内的所有点（ 在k-邻域的基础上，增加一个半径为 ϵ 的球体，将球体内的所有点也包含在内。）。这种定义可以缓解 k-邻域的偏差问题，但在稀疏采样区域中，ke-邻域会包含来自密集采样区域的点，增加了计算成本。对称 ke-邻域: 包含距离点 p最近的 k个样本点，以及距离这些k个样本点中心在€范围内的所有点。(同时考虑点p和其 k 个最近邻点q的kϵ-邻域，并将两个邻域的交集作为最终的邻域。)这种定义可以克服kε-邻域的偏差问题，并有效地处理采样密度变化，因为它考虑了点p的k个最近邻域点的邻域信息。
-
-
论文采用
对称 ke-邻域的方法进行领域确认，同时为了高效地收集每个点的邻域，论文中使用了kd-tree数据结构。
1-2-2 KD树-kd_tree
- kd-tree 是一种用于多维空间中快速检索最近邻点的数据结构，它类似于二叉搜索树，但每个节点包含多个键值。
- *kd-tree 的构造：
- 选择一个维度作为分割维度，例如 x 维。
- 将点云中的所有点按照该维度的值进行排序。
- 选择排序后的中点作为根节点，将点云分成两部分，左半部分的所有点都小于根节点，右半部分的所有点都大于根节点。
- 对左半部分和右半部分分别递归地进行步骤 1-3，直到所有点都被分配到叶子节点。
- *kd-tree 的查询：
- 从根节点开始，比较查询点与当前节点的分割维度值。
- 如果查询点小于分割维度值，则进入左子树；否则进入右子树。
- 重复步骤 1-2，直到找到叶子节点或者当前节点与查询点的距离小于某个阈值。
- *kd-tree 的优点：
- 查询效率高： kd-tree 可以在 O(log n) 的时间复杂度内找到最近邻点，其中 n 是点云中点的数量。
- 空间复杂度低： kd-tree 的空间复杂度为 O(n)，与点云中点的数量成正比。
- *kd-tree 的应用：
- 最近邻搜索： 在点云处理、图像处理、机器学习等领域中，经常需要进行最近邻搜索，kd-tree 可以有效地加速搜索过程。
- 空间分割： kd-tree 可以将空间分割成多个区域，每个区域包含一组相邻的点。
- 聚类分析： kd-tree 可以用于聚类分析，例如将点云中的点分成不同的簇。
1-3 边缘检测准则（见4.2~4.4）
- 这一节介绍了三种准则用于计算每个点的边界概率：角度准则
Angle Criterion、半圆准则Halfdisc Criterion和形状准则Shape Criterion。
1-3-1 角度准则Angle Criterion
- 角度准则的原理是基于点与其邻域点之间的角度关系来判断一个点是否位于边界上。具体步骤如下：
- 对于点云中的每个点，找出其最近的几个邻居点（通常使用k-邻域）。
- 计算这些邻居点与当前点形成的向量。
- 分析这些向量之间的
角度。如果这些角度中的最大值超过某个阈值，则认为当前点位于边界上，因为边界点通常会有较大的角度变化。 - 边界概率可以通过角度大小与预设阈值的比较来计算，如果角度接近阈值，则边界概率较低；如果远超阈值，则边界概率较高。
1-3-2 半圆准则Halfdisc Criterion
- 半圆准则的原理是基于点与其邻居点的位置关系来判断边界概率。具体步骤如下：
- 对于每个点，计算其k-邻域内所有点的
平均值，作为半圆的中心。 - 计算当前点到半圆中心的距离。
- 比较这个距离与半圆的半径。如果当前点位于半圆的边缘，即其到半圆中心的距离接近半径，则认为该点有较高的边界概率。
- 边界概率可以通过当前点到半圆中心的距离与半径的比例来计算。
- 对于每个点，计算其k-邻域内所有点的
- 原文提到半圆准则借鉴了2D图像中提取图像轮廓的算法原理，即在识别图像轮廓时，通过检测亮度偏差较大的像素点来提取边缘。
1-3-3 形状准则Shape Criterion
- 形状准则利用了点的局部几何形状信息来判断边界概率。具体步骤如下：
- 对于每个点，计算其k-邻域内点的协方差矩阵。(协方差矩阵能告诉我们这些点在各个方向上的分布情况。)
- 计算协方差矩阵的特征值和特征向量。
- 分析特征值的大小。
特征值可以帮助我们了解这些点是如何分布的。如果特征值之间有很大的差异，那就意味着这些点在某个方向上特别集中，而在其他方向上则比较分散。 - 边界概率可以通过特征值差异的量化来计算。
特征值差异越大，边界概率越高。
1-3-4 法线估计
- 值得一提的是由于
角度准则和平均准则都严重依赖于点 p 处的法线，因此在没有法线信息的情况下，需要对法线进行良好的估计。- 角度准则： 该准则通过计算邻域内点在切平面上的投影之间的最大夹角来判断点 p 是否位于边界上。如果点 p 位于边界上，那么其邻域内点在切平面上的投影将会出现较大的夹角，因为它们分布在边界两侧。而切平面的计算需要依赖于点 p 处的法线。
- 平均准则： 该准则通过比较点 p 与其邻域内点的平均值来判断点 p 是否位于边界上。如果点 p 位于边界上，那么其邻域内点的平均值将会偏离点 p，因为它们分布在边界两侧。而邻域内点的平均值计算也需要依赖于点 p 处的法线，以便将邻域内点投影到切平面上进行计算。
1-4 概率融合（见4.5）
-
上一节论文提到了三种准则用于计算一个点的边界概率，这一节中论文分析了三个方法的优势
- 角判据: 擅长检测较宽的边界，并能突出边界点形成的细线。
- 半圆判据: 更适合检测小孔，尤其是当孔被邻域图的边交叉时。
- 形状判据: 在噪声环境下表现最佳。
-
同时论文提出了一种加权和的方案：
-
其中，w̸、wµ 和 wφ 分别是角判据、半圆判据和形状判据的权重，且它们的和为 1。用户可以根据视觉检查调整这些权重，以适应不同的应用场景。例如，对于噪声模型，可以增加形状判据的权重。
1-5 边界一致性处理（见5.1）
- 论文中这一小节主要介绍了如何利用边界点的连贯性来提高孔洞检测的鲁棒性。其核心思想是，边界上的任意点都至少有一个邻居点也属于边界（
孔洞的边界不会是孤立的，而是由一系列相邻的边界点组成的。）。利用这一特性，可以采用以下步骤进行迭代分类：- 初始分类： 首先根据用户定义的阈值，将边界概率大于该阈值的点标记为边界点。
- 迭代更新：（不断迭代的过程删除
误检点）- 选择已标记为边界的点： 从所有已标记为边界的点中选择一个点 p。
- 寻找最大角度差的邻居点： 找到 p 的邻居点中，与 p 构成最大角度差的两个点 q1 和 q2。
- 检查邻居点是否为边界点： 检查 q1 和 q2 是否也是边界点。
- 更新边界点标记：
- 如果 q1 和 q2 都是边界点，则 p 保持为边界点。
- 如果 q1 和 q2 中至少有一个不是边界点，则将 p 标记为内部点。
- 重复步骤 1-4： 重复以上步骤，直到没有更多点的状态发生变化。
- 构造最小生成树： 利用邻接图 G 构造最小生成树 (MST)。MST 中的环与边界环相对应。
- 环提取： 使用广度优先搜索 (BFS) 从 MST 中提取边界环。
1-6 论文小结
- 至此论文的分析就到这里，后续的内容是有关轮廓上孔洞的检测，我们只需要前面有关点的轮廓概率的原理即可。
pcl::BoundaryEstimation的实现中，使用到了和这篇论文在4.3小结提到的角度准则Angle Criterion一致。
2 pcl::BoundaryEstimation进行轮廓提取
2-1 计算法向量
- 正如1-3-4 法线估计中提到的
角度准则严重依赖于点 p 处的法线，故在进行轮廓提取之前，我们需要计算点集的法向量。（remaining_cloud是上一节通过Plane Model Segmentation分离出的平面以上的点云）
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;normalEstimation.setInputCloud(remaining_cloud.makeShared());
normalEstimation.setSearchMethod(tree);
normalEstimation.setRadiusSearch(0.02); // 法向量的半径
normalEstimation.compute(*normals);
- 创建 Kd 树搜索对象： 使用
KdTree类创建一个 Kd 树搜索对象tree，用于加速近邻搜索。 - 创建法线点云对象： 使用
PointCloud类创建一个法线点云对象normals，用于存储每个点的法线信息。 - 创建法线估计对象： 使用
NormalEstimation类创建一个法线估计对象normalEstimation，用于计算点云中每个点的法线。 - 设置输入点云： 使用
setInputCloud方法将剩余点云remaining_cloud设置为输入点云。 - 设置搜索方法： 使用
setSearchMethod方法将 Kd 树搜索对象tree设置为搜索方法。 - 设置搜索半径： 使用
setRadiusSearch方法设置搜索半径为 0.02 米。这意味着每个点的法线将根据其 0.02 米范围内的邻域点进行计算。 - 计算法线： 使用
compute方法计算点云中每个点的法线，并将结果存储在normals对象中。
2-2 pcl 计算边界
- 如下
pcl::PointCloud<pcl::Boundary>::Ptr boundaries(new pcl::PointCloud<pcl::Boundary>); //声明一个boundary类指针，作为返回值
boundaries->resize(remaining_cloud.size()); //初始化大小
pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> boundary_estimation; //声明一个BoundaryEstimation类
boundary_estimation.setInputCloud(remaining_cloud.makeShared()); //设置输入点云
boundary_estimation.setInputNormals(normals); //设置输入法线
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree_ptr(new pcl::search::KdTree<pcl::PointXYZ>);
boundary_estimation.setSearchMethod(kdtree_ptr); //设置搜寻k近邻的方式
boundary_estimation.setKSearch(30); //设置k近邻数量
boundary_estimation.setAngleThreshold(M_PI * 0.6); //设置角度阈值，大于阈值为边界
boundary_estimation.compute(*boundaries); //计算点云边界，结果保存在boundaries中
-
创建边界点云对象： 使用
PointCloud类创建一个边界点云对象boundaries，用于存储每个点的边界信息。 -
初始化大小： 使用
resize方法将boundaries的大小初始化为剩余点云的大小。 -
创建边界估计对象： 使用
BoundaryEstimation类创建一个边界估计对象boundary_estimation，用于计算点云的边界。 -
设置输入点云和法线： 使用
setInputCloud和setInputNormals方法将剩余点云remaining_cloud和法线点云normals设置为输入。 -
创建 Kd 树搜索对象： 使用
KdTree类创建一个 Kd 树搜索对象kdtree_ptr，用于加速近邻搜索。 -
设置搜索方法： 使用
setSearchMethod方法将 Kd 树搜索对象kdtree_ptr设置为搜索方法。 -
设置 k 近邻数量： 使用
setKSearch方法设置 k 近邻数量为 30。这意味着每个点的边界信息将根据其 30 个近邻点进行计算。 -
设置角度阈值： 使用
setAngleThreshold方法设置角度阈值为M_PI * 0.6。这意味着如果一个点的法线与其 k 个近邻点的法线之间的最大夹角大于该阈值，则该点将被视为边界点。
-
计算边界： 使用
compute方法计算点云的边界，并将结果存储在boundaries对象中。
2-3 可视化轮廓点云
- 值得注意的是
pcl::PointCloud<pcl::Boundary>::Ptr boundaries不是正常的点云信息，如果需要使用viewer可视化，需要转换为正常的点云信息。
pcl::PointCloud<pcl::PointXYZ>::Ptr boundary_cloud(new pcl::PointCloud<pcl::PointXYZ>);// 遍历所有边界点，并将它们转换为pcl::PointXYZ类型
for (size_t i = 0; i < boundaries->size(); ++i)
{pcl::PointXYZ point;if(boundaries->points[i].boundary_point != 0){boundary_cloud->push_back(remaining_cloud.points[i]);}
}
// 清除所有点云并重新添加
viewer.removeAllPointClouds();
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> boundary_color_handler(plane_cloud.makeShared(), 0, 0, 255);
viewer.addPointCloud<pcl::PointXYZ>(boundary_cloud->makeShared(), boundary_color_handler, "boundary_cloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "boundary_cloud");// 设置背景色
viewer.setBackgroundColor(0, 0, 0);viewer.spinOnce(0.0001);
-
效果如下，下图是本人在室内房间里头测试的画面，分别是洗衣液一瓶和一个椅子，可以看到点云的蓝色轮廓被很好的勾勒出来
-
我们来做一个小实验，回顾角度准则
Angle Criterion的其中一个步骤：
3. 比较这个距离与半圆的半径。如果当前点位于半圆的边缘，即其到半圆中心的距离接近半径，则认为该点有较高的边界概率。 -
对应的代码如下
boundary_estimation.setAngleThreshold(M_PI * 0.6); //设置角度阈值，大于阈值为边界
-
我们略微增大和减小这个角度阈值，来看看轮廓的变化：
-
0.2
-
0.9
-
可以看的出来，这个角度阈值直接决定轮廓最终提取的结果。
3 轮廓2D投射
-
在这一节中，我希望把3维度的轮廓信息透色到2D的平面上，着非常简单。我们只需要把轮廓点云映射到一个平面上即可，这里我的相机是向下进行拍摄的（如下坐标系）
-
值得一提的是这个坐标系可以在viewer初始化的时候进行设置
viewer.addCoordinateSystem(0.1f); // 添加基础坐标系，大小为0.1
- 而上述图像的点云信息其实是基于xoy平面进行拍摄获取到的（也就是从xoy平面往z轴看去，应该得到的是一张640* 480大小的平面图像），那么我们只需要把点云轮廓信息的z轴设置为零，并根据自己相机的映射关系映射，便可以得到轮廓信息的2D映射图
cv::Mat contour_image = cv::Mat(480,640, CV_8UC3, cv::Scalar(255, 255, 255)); // 白色背景if (boundary_cloud->size() > 0) {// 在新图像上绘制轮廓点for (const auto& point : *boundary_cloud) {// 转换点云坐标到图像坐标int u = static_cast<int>(point.x * 570 + 300); // 假设x坐标直接对应图像宽度int v = static_cast<int>(point.y * 570 + 220); // 假设y坐标直接对应图像高度// 检查点是否在图像范围内if (u >= 0 && u < contour_image.cols && v >= 0 && v < contour_image.rows) {// 在新图像上绘制点cv::circle(contour_image, cv::Point(u, v), 2, cv::Scalar(0, 0, 255), -1); // 使用蓝色绘制点，大小为2}}
}// 显示新的轮廓图像
imshow("Contour Image", contour_image);
cv::waitKey(1); // 等待按键后继续
-
地板，洗衣液，椅子
-
电脑，书桌(乱七八糟没收拾)![[da88a222cee5be765ab59bf5cf5f95d.png]]
-
操场
4 完整代码
#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>
#include <pcl/filters/passthrough.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/boundary.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/range_image_visualizer.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;cv_bridge::CvImagePtr cv_ptr;pcl::SACSegmentation<pcl::PointXYZ> seg;void imageCB(const sensor_msgs::ImageConstPtr& img_msg){}void depthImageCB(const sensor_msgs::ImageConstPtr& dimg_msg){}void depthPCloudCB(const sensor_msgs::PointCloud2ConstPtr& pc_msg){}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);viewer.addCoordinateSystem(0.1f); // 添加基础坐标系，大小为0.1}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 及存储平面模型系数 coefficientsseg.segment(*inliers, *coefficients);if (inliers->indices.size() == 0) {PCL_ERROR ("Could not estimate a planar model for the given dataset.");continue;}//std::cout<<coefficients->values[0]<<"x+"<<coefficients->values[1]<<"y+"<<coefficients->values[2]<<"z+"<<coefficients->values[3]<<std::endl;// 提取平面点云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);// 1 计算法向量pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;normalEstimation.setInputCloud(remaining_cloud.makeShared());normalEstimation.setSearchMethod(tree);normalEstimation.setRadiusSearch(0.02); // 法向量的半径normalEstimation.compute(*normals);//2 pcl计算边界pcl::PointCloud<pcl::Boundary>::Ptr boundaries(new pcl::PointCloud<pcl::Boundary>); //声明一个boundary类指针，作为返回值boundaries->resize(remaining_cloud.size()); //初始化大小pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> boundary_estimation; //声明一个BoundaryEstimation类boundary_estimation.setInputCloud(remaining_cloud.makeShared()); //设置输入点云boundary_estimation.setInputNormals(normals); //设置输入法线pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree_ptr(new pcl::search::KdTree<pcl::PointXYZ>);boundary_estimation.setSearchMethod(kdtree_ptr); //设置搜寻k近邻的方式boundary_estimation.setKSearch(30); //设置k近邻数量boundary_estimation.setAngleThreshold(M_PI * 0.6); //设置角度阈值，大于阈值为边界boundary_estimation.compute(*boundaries); //计算点云边界，结果保存在boundaries中pcl::PointCloud<pcl::PointXYZ>::Ptr boundary_cloud(new pcl::PointCloud<pcl::PointXYZ>);// 遍历所有边界点，并将它们转换为pcl::PointXYZ类型for (size_t i = 0; i < boundaries->size(); ++i){pcl::PointXYZ point;if(boundaries->points[i].boundary_point != 0){boundary_cloud->push_back(remaining_cloud.points[i]);}}cv::Mat contour_image = cv::Mat(480,640, CV_8UC3, cv::Scalar(255, 255, 255)); // 白色背景if (boundary_cloud->size() > 0) {// 在新图像上绘制轮廓点for (const auto& point : *boundary_cloud) {// 转换点云坐标到图像坐标int u = static_cast<int>(point.x * 570 + 300); // 假设x坐标直接对应图像宽度int v = static_cast<int>(point.y * 570 + 220); // 假设y坐标直接对应图像高度// 检查点是否在图像范围内if (u >= 0 && u < contour_image.cols && v >= 0 && v < contour_image.rows) {// 在新图像上绘制点cv::circle(contour_image, cv::Point(u, v), 2, cv::Scalar(0, 0, 255), -1); // 使用蓝色绘制点，大小为2}}}// 显示新的轮廓图像imshow("Contour Image", contour_image);cv::waitKey(1); // 等待按键后继续// 清除所有点云并重新添加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");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> boundary_color_handler(plane_cloud.makeShared(), 0, 0, 255);viewer.addPointCloud<pcl::PointXYZ>(boundary_cloud->makeShared(), boundary_color_handler, "boundary_cloud");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "boundary_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;
}总结
- 本文介绍点云轮廓提取的原理（论文解读
Detecting Holes in Point Set Surfaces）和pcl::BoundaryEstimation类的使用，并以代码示例展示了如何将 3D 点云轮廓信息投射到 2D 平面上。 - 这个系列也是完结辣，感谢大家的支持，如有错误，欢迎指出！！！
- 一把无敌的椅子
