当前位置：首页 > article >正文

【PCL实现点云分割】ROS深度相机实践指南（上）:PCL库初识和ROS-PCL数据类型转换

article 2024/11/15 2:21:37

前言

本教程使用PCL对ROS深度相机捕获到的画面进行操场上锥桶的分割
本人相关的RGBD深度相机原理及其使用教程：
- [csdn 博客] 上手一个RGBD深度相机：从原理到实践–ROS noetic+Astra S（上）：解读深度测距原理和内外参推导
- [csdn 博客] 上手一个RGBD深度相机：从原理到实践–ROS noetic+Astra S（中）：RGB相机的标定和使用
关于PCL的学习教程强烈推荐双愚大大的文章：PCL(Point Cloud Library)学习指南&资料推荐（2024版）
本文使用的环境和设备
- ubuntu 20.04LTS
- ros1 noetic
- Astra S 深度相机
- pcl-1.10

1 PCL-PointCloudLibrary

1-1 简介

请添加图片描述

PCL（Point Cloud Library）是一个开源项目，旨在为三维点云处理提供一个全面的库。点云是由大量空间点组成的数据集，通常用于三维重建、三维扫描和三维测量等应用。PCL提供了丰富的算法和工具，用于点云数据的获取、处理、可视化以及分析。
PCL支持多种编程语言，包括C++和Python，并且可以在多种操作系统上运行，如Windows、Linux和Mac OS X。它是一个跨平台、可扩展的库，适用于多种三维数据处理任务。
PCL的主要功能包括：
- 数据获取：支持从多种传感器（如激光扫描仪、RGB-D相机等）获取点云数据。
- 数据预处理：提供滤波、采样、去噪等预处理算法，以优化点云数据的质量。
- 特征提取：提供关键点检测、表面法线估计、曲率估计等特征提取算法，用于后续的处理和分析。
- 表面重建：支持多种表面重建算法，如泊松重建、网格简化等，用于生成三维模型的表面。
- 物体识别：提供物体检测、分割、识别等算法，用于从点云数据中识别出特定的物体。
- 机器学习：集成了一些机器学习算法，如聚类、分类、回归等，用于对点云数据进行学习和分析。
- 可视化：提供强大的可视化工具，用于显示点云数据、三维模型和算法结果。
一句话简单概括就是PCL在3D点云数据处理的地位就像opencv在2D图像处理中的地位一样

1-2 安装与配置

ubuntu上安装PCL很简单，这里我们直接采用apt进行安装(源码安装过于复杂了，感觉不是很必要)

sudo apt-get install libpcl-dev
sudo apt-get install ros-noetic-pcl-ros

安装成功后我们需要获取PCL包PCLConfig.cmake的位置，以便后面我们在CmakeLists中使用,这里使用dpkg进行查找

dpkg -L libpcl-dev

请添加图片描述

我们在我们的工作空间中需要使用到的PCL的功能包的CMakeLists中加入下述代码

set(PCL_DIR "/usr/lib/x86_64-linux-gnu/cmake/pcl")
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  sensor_msgs
  OpenCV
  cv_bridge
  PCL
)

测试，我们新建一个cpp文件

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

int
main (int argc, char** argv)
{
  // 创建一个点云对象
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  // 填充点云数据
  cloud->width = 5;
  cloud->height = 1;
  cloud->is_dense = false;
  cloud->points.resize (cloud->width * cloud->height);

  for (size_t i = 0; i < cloud->points.size (); ++i)
  {
    cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
    cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
    cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
  }

  // 创建PCL可视化对象
  pcl::visualization::PCLVisualizer viewer ("3D Viewer");
  viewer.addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
  viewer.spin ();

  return 0;
}

catkin_make编译，运行，会得到如下窗口，可以看到窗口内新建了5个随机点云数据

2 pcl::PointCloud和sensor_msgs/PointCloud2的互相转换

2-1 sensor_msgs/PointCloud2

启动astra.launch启动我们的深度相机(不明白的朋友可以看看这篇文章[csdn 博客] 上手一个RGBD深度相机：从原理到实践–ROS noetic+Astra S（上）：解读深度测距原理和内外参推导)
我们对发布点云的话题进行查询，发现相机驱动发布的话题类型是sensor_msgs/PointCloud2
我们使用ROS内置的消息进行查询，得到如下数据类型

# rosmsg show sensor_msgs/PointCloud2 
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height
uint32 width
sensor_msgs/PointField[] fields
  uint8 INT8=1
  uint8 UINT8=2
  uint8 INT16=3
  uint8 UINT16=4
  uint8 INT32=5
  uint8 UINT32=6
  uint8 FLOAT32=7
  uint8 FLOAT64=8
  string name
  uint32 offset
  uint8 datatype
  uint32 count
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense

sensor_msgs/PointCloud2是用于在ROS中传输点云数据的标准消息类型。
- std_msgs/Header header:
  - uint32 seq: 消息序列号，用于消息排序。
  - time stamp: 时间戳，表示消息的生成时间。
  - string frame_id: 参考框架的ID，用于在三维空间中定位点云数据。
- uint32 height: 点云的高度，即行数。
- uint32 width: 点云的宽度，即列数。
- sensor_msgs/PointField[] fields:
  - 这是一个数组，定义了点云中每个点的数据字段。
  - uint8 INT8=1: 8位有符号整数。
  - uint8 UINT8=2: 8位无符号整数。
  - uint8 INT16=3: 16位有符号整数。
  - uint8 UINT16=4: 16位无符号整数。
  - uint8 INT32=5: 32位有符号整数。
  - uint8 UINT32=6: 32位无符号整数。
  - uint8 FLOAT32=7: 32位浮点数。
  - uint8 FLOAT64=8: 64位双精度浮点数。
  - string name: 字段名称，例如"x"、“y”、"z"表示空间坐标。
  - uint32 offset: 字段在点云数据中的偏移量。
  - uint8 datatype: 字段的数据类型，对应上面的枚举值。
  - uint32 count: 该字段在点云数据中的数量，例如，如果点云包含(x, y, z)坐标，那么"x"字段的计数为1。
- bool is_bigendian: 表示点云数据是否为大端字节序。
- uint32 point_step: 每个点的数据大小（以字节为单位）。
- uint32 row_step: 每行的数据大小（以字节为单位），如果点云是按行组织的。
- uint8[] data: 存储点云数据的字节数组。
- bool is_dense: 表示点云数据是否密集，即是否所有点都存在且没有空隙。
我们echo一次看看真实的数据长什么样（中间的data很多数据，输出重定向差点卡死我了）

header: 
seq: 11
stamp: 
secs: 1726826397
nsecs: 164749698
frame_id: "camera_depth_optical_frame"
height: 480
width: 640
fields: 
- 
name: "x"
offset: 0
datatype: 7
count: 1
- 
name: "y"
offset: 4
datatype: 7
count: 1
- 
name: "z"
offset: 8
datatype: 7
count: 1
is_bigendian: False
point_step: 16
row_step: 10240

"\ndata: [0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 0, 0, 0, 0, 192,...
92, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 0, 0, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 0, 0, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 0, 0, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 0, 0, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 0, 0]
is_dense: False

2-2 pcl::PointCloud

我们看数据类型

struct PCL_EXPORTS PCLPointCloud2
  {
    ::pcl::PCLHeader header;

    std::uint32_t height = 0;
    std::uint32_t width = 0;

    std::vector<::pcl::PCLPointField>  fields;

    static_assert(BOOST_ENDIAN_BIG_BYTE || BOOST_ENDIAN_LITTLE_BYTE, "unable to determine system endianness");
    std::uint8_t is_bigendian = BOOST_ENDIAN_BIG_BYTE;
    std::uint32_t point_step = 0;
    std::uint32_t row_step = 0;

    std::vector<std::uint8_t> data;

    std::uint8_t is_dense = 0;

  public:
    using Ptr = shared_ptr< ::pcl::PCLPointCloud2>;
    using ConstPtr = shared_ptr<const ::pcl::PCLPointCloud2>;
    ....

PCL（Point Cloud Library）中的一个结构体PCLPointCloud2，它是用于在PCL中处理和传输点云数据的一种数据结构。
成员变量：
- header：PCLHeader类型的成员变量，包含时间戳、帧ID等信息。
- height：点云的高度，即行数。
- width：点云的宽度，即列数。
- fields：PCLPointField类型的向量，描述了点云中每个点的数据字段。
- is_bigendian：表示点云数据是否为大端字节序。
- point_step：每个点的数据大小（以字节为单位）。
- row_step：每行的数据大小（以字节为单位），如果点云是按行组织的。
- data：存储点云数据的字节数组。
- is_dense：表示点云数据是否密集，即是否所有点都存在且没有空隙。

2-3 数据转换

要将这两种数据结构互相转换，可以使用PCL提供的转换函数。
1.从 sensor_msgs/PointCloud2 转换为 pcl::PointCloud<pcl::PointXYZ>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

void convertSensorMsgsToPCL(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
  // 创建一个空的PCL点云
  pcl::PointCloud<pcl::PointXYZ> cloud;

  // 从sensor_msgs/PointCloud2消息转换为PCL点云
  pcl::fromROSMsg(*cloud_msg, cloud);

  // 现在可以使用PCL库处理cloud
}

2.从 pcl::PointCloud<pcl::PointXYZ> 转换为 sensor_msgs/PointCloud2

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

sensor_msgs::PointCloud2 convertPCLToSensorMsgs(const pcl::PointCloud<pcl::PointXYZ>& cloud)
{
  // 创建一个空的sensor_msgs/PointCloud2消息
  sensor_msgs::PointCloud2 output;

  // 从PCL点云转换为sensor_msgs/PointCloud2消息
  pcl::toROSMsg(cloud, output);

}

在这些示例中，pcl::fromROSMsg 函数用于将 sensor_msgs/PointCloud2 消息转换为 pcl::PointCloud 对象，而 pcl::toROSMsg 函数用于将 pcl::PointCloud 对象转换为 sensor_msgs/PointCloud2 消息。注意，这里假设点云类型是 pcl::PointXYZ。

3 可视化和数据转换实操

3-1 cv_bridge

请添加图片描述

cv_bridge 是ROS（Robot Operating System）中用于在ROS消息（如 sensor_msgs/Image）和OpenCV图像格式之间进行转换的工具。它允许你将ROS图像消息转换为OpenCV图像格式，以便使用OpenCV库进行图像处理，反之亦然。
cv_bridge 提供了以下主要功能：
- 图像消息转换：
  - 将ROS sensor_msgs/Image 消息转换为OpenCV cv::Mat 图像格式。
  - 将OpenCV cv::Mat 图像格式转换为ROS sensor_msgs/Image 消息。
- 编码和解码：
  - 支持ROS图像消息中常见的图像编码（如 “8UC1”, “8UC3”, “16UC1” 等）。
  - 支持从ROS图像消息中解码图像数据，并将其转换为OpenCV可以处理的格式。
我们写一下简单的cpp代码，订阅相机驱动发布的sensor_msgs/Image话题并转换为OpenCV支持的cv::Mat

#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>
using namespace cv;

class PCL_center
{
private:
    ros::NodeHandle nh;
    ros::Subscriber imageSub;
    ros::Subscriber depthImageSub;
    ros::Subscriber depthPCloudSub;

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

    imshow("Depth Window", depth_display);
    waitKey(1);
    }

    void depthPCloudCB(const sensor_msgs::PointCloud2ConstPtr& pc_msg)
    {
        std::cout << "depthPCloudCB()" << std::endl;
    }

public:
    PCL_center(ros::NodeHandle& nh_) : nh(nh_)
    {
        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);
    }
};

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "pcl_test");
    ros::NodeHandle nh;
    PCL_center pcl_center(nh);
    namedWindow("Input Window", WINDOW_AUTOSIZE);

    ros::spin();
    destroyWindow("Input Window");
    return 0;
}

请添加图片描述

3-2 点云数据转换和可视化

pcl::visualization::PCLVisualizer 是 PCL (Point Cloud Library) 库中用于可视化点云数据的一个类。它是基于 VTK (Visualization Toolkit) 库构建的，允许用户在 3D 环境中查看点云数据，并提供了多种自定义选项来增强可视化效果。
以下是一些 pcl::visualization::PCLVisualizer 的关键功能和特性：
- 添加点云：你可以将点云添加到可视化窗口中，并为每个点云分配一个唯一的名称。
```
viewer.addPointCloud<pcl::PointXYZ>(cloud.makeShared(), "sample cloud");
```
- 设置点云属性：可以设置点云的渲染属性，如点的大小、颜色等。
```
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
```
- 添加其他3D对象：除了点云，你还可以添加其他3D对象，如线、模型、文本等。
- 交互式操作：用户可以通过鼠标和键盘进行旋转、缩放、平移等操作来交互式地查看点云。
- 背景设置：可以设置可视化窗口的背景颜色。
```
viewer.setBackgroundColor(0, 0, 0); // 设置为黑色背景
```
- 坐标轴和网格：可以显示坐标轴和网格，以便更好地理解点云的空间位置。
- 回调函数：可以为特定事件（如键盘输入、鼠标点击）设置回调函数。
- 动画和定时器：支持动画和定时器，可以在特定时间间隔执行操作。
- 保存图像：可以将可视化窗口的当前视图保存为图像文件。
- 多窗口支持：可以创建多个可视化窗口，每个窗口可以显示不同的点云或对象。
我们需要包含具体的头文件，初始化viewer("3D Viewer")

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>

private:
    pcl::visualization::PCLVisualizer viewer;
     PCL_center(ros::NodeHandle& nh_) : nh(nh_),viewer("3D Viewer")
    {

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


            // 创建一个空的PCL点云
            pcl::PointCloud<pcl::PointXYZ> cloud;

            // 从sensor_msgs/PointCloud2消息转换为PCL点云
            pcl::fromROSMsg(pc_msg, cloud);

    
            viewer.removeAllPointClouds();  // 移除当前所有点云
            // 添加点云到可视化窗口
            viewer.addPointCloud<pcl::PointXYZ>(cloud.makeShared(), "sample cloud");
            viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");

            // 设置背景色
            viewer.setBackgroundColor(0, 0, 0);

            viewer.spinOnce(0.0000000000001);
            

        }

我们就拿到点云信息了

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>

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;

    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(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;


        // 创建一个空的PCL点云
        pcl::PointCloud<pcl::PointXYZ> cloud;

        // 从sensor_msgs/PointCloud2消息转换为PCL点云
        pcl::fromROSMsg(pc_msg, cloud);

 
        viewer.removeAllPointClouds();  // 移除当前所有点云
        // 添加点云到可视化窗口
        viewer.addPointCloud<pcl::PointXYZ>(cloud.makeShared(), "sample cloud");
        viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");

        // 设置背景色
        viewer.setBackgroundColor(0, 0, 0);

        viewer.spinOnce(0.0000000000001);
        

    }
public:
    PCL_center(ros::NodeHandle& nh_) : nh(nh_),viewer("3D Viewer")
    {
        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);
    }
};

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "pcl_test");
    ros::NodeHandle nh;
    PCL_center pcl_center(nh);
    namedWindow("Input Window", WINDOW_AUTOSIZE);

    ros::spin();
    destroyWindow("Input Window");
    return 0;
}

总结

本文介绍了PCL库的安装和基础使用，并介绍了如何使用PCL库对sensor_msgs/PointCloud2 消息转换为 pcl::PointCloud，并使用pcl::visualization::PCLVisualizer进行可视化
下一节我们讲讲如何使用PCL分割进行
如有错误，欢迎指出!!!谢谢支持！！

查看全文

http://www.kler.cn/a/313501.html