ROS系统中实现点云聚类（realsense数据源）

本文主要介绍ROS系统中如何订阅并解码realsense点云数据，并对点云进行稀疏、去噪、聚类。

• 环境配置见《ROS系统中从零开始部署YoloV4目标检测算法（3种方式）》
• 需要安装的第三方库：PCL

package文件结构

程序结构：

• main.cpp（自己的聚类程序）
• CMakeLists.txt（创建package时自动生成的，需要改造内容）
• package.xml（创建package时自动生成的，需要改造内容）
• include 文件夹（创建package时自动生成的，空文件夹）
• src 文件夹（创建package时自动生成的，空文件夹）

实现一个初步的聚类package只要改动3个文件，分别是main.cpp, CMakeLists.txt, package.xml, 内容如下：

main.cpp

//参考：https://blog.csdn.net/weixin_42503785/article/details/110362740
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>

using namespace std;

#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_cloud.h>



#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/passthrough.h>

#include "std_msgs/Int8.h" 
#include "std_msgs/String.h"

ros::Publisher pub;


void pclCloudCallback(const sensor_msgs::PointCloud2ConstPtr& input) 
{
    //修改相机参数的方法。
    // //备注：如果提示 No module named 'rospkg'，请退出conda环境再执行launch启动。
    // system("rosrun dynamic_reconfigure dynparam set /camera/rgb_camera/ enable_auto_exposure 0");

    // 创建一个输出的数据格式
    sensor_msgs::PointCloud2 output;  //ROS中点云的数据格式
    //对数据进行处理
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    output = *input;
    pcl::fromROSMsg(output,*cloud);
    std::cout<<"direct_trans: "<<std::endl;


    //*********************** 1 点云稀疏化 ***********************//
    // 创建过滤对象：使用1cm（0.01）的叶大小对数据集进行下采样
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    vg.setInputCloud(cloud);
    //vg.setLeafSize(0.01f, 0.01f, 0.01f);
    vg.setLeafSize(0.1f, 0.1f, 0.1f);//单位：米
    vg.filter(*cloud_filtered);
    std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; //*


    //*********************** 2 筛除平面点云 ***********************//
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::SACSegmentation<pcl::PointXYZ> seg;    //实例化一个分割对象
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);//实例化一个索引
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);//实例化模型参数
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());//提取到的平面保存至cloud_plane

    pcl::PCDWriter writer;

    seg.setOptimizeCoefficients(true);//可选设置,设置模型系数需要优化
    seg.setModelType(pcl::SACMODEL_PLANE);//设置分割的模型类型
    seg.setMethodType(pcl::SAC_RANSAC);//设置分割的参数估计方法
    seg.setMaxIterations(100);//最大迭代次数
    seg.setDistanceThreshold(0.02);//设置内点到模型的距离允许最大值

    int i = 0, nr_points = (int)cloud_filtered->points.size();//计数变量i，记下提取的平面的个数
    while (cloud_filtered->points.size() > 0.3 * nr_points)
    {
        //从剩余的云中分割最大的平面组件
        seg.setInputCloud(cloud_filtered);//设置要分割的点云
        seg.segment(*inliers, *coefficients);   //分割，存储分割结果到点集合inliers及存储平面模型的系数coefficients
        if (inliers->indices.size() == 0)//如果平面点索引的数量为0
        {
            //std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
            break;
        }

        pcl::ExtractIndices<pcl::PointXYZ> extract;//实例化一个提取对象
        extract.setInputCloud(cloud_filtered);//设置要提取的点云
        extract.setIndices(inliers);//根据分割得到的平面的索引提取平面
        extract.setNegative(false);//false：提取内点

        // Write the planar inliers to disk
        extract.filter(*cloud_plane);//保存提取到的平面
        //std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;

        // Remove the planar inliers, extract the rest
        extract.setNegative(true);//提取外点（除第一个平面之外的点）
        extract.filter(*cloud_f);//保存除平面之外的剩余点
        cloud_filtered = cloud_f;//将剩余点作为下一次分割、提取的平面的输入点云
    }

    //*********************** 3 点云欧式聚类 ***********************//
    // 为提取的搜索方法创建KdTree对象
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(cloud_filtered); //将无法提取平面的点云作为 cloud_filtered

    std::vector<pcl::PointIndices> cluster_indices; //保存每一种聚类，每一种聚类下还有具体的聚类的点
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;//实例化一个欧式聚类提取对象

    //realsense点云XYZ值单位是：米m，Kinect点云XYZ值单位是：毫米mm
    // ec.setClusterTolerance(0.02);     //近邻搜索的搜索半径，重要参数, 单位：米 
    // ec.setMinClusterSize(50);         //设置一个聚类需要的最少点数目为100
    ec.setClusterTolerance(0.2);     //近邻搜索的搜索半径，重要参数, 单位：米 
    ec.setMinClusterSize(5);         //设置一个聚类需要的最少点数目为100
    ec.setMaxClusterSize(25000);      //设置一个聚类最大点数目为25000
    ec.setSearchMethod(tree);         //设置点云的搜索机制
    ec.setInputCloud(cloud_filtered); //设置输入点云
    ec.extract(cluster_indices);      //将聚类结果保存至 cluster_indices 中


    //*********************** 4 计算每一个聚类的bbox的中心点XYZ，及其bbox的length，width，height ***********************//
    // 改造自：https://blog.csdn.net/AdamShan/article/details/83015570
    double output_width;
    for (size_t i = 0; i < cluster_indices.size(); i++)
    {
        //质心的坐标
        pcl::PointXYZ centroid_;
        pcl::PointXYZ min_point_;
        pcl::PointXYZ max_point_;

        float min_x = std::numeric_limits<float>::max();
        float max_x = -std::numeric_limits<float>::max();
        float min_y = std::numeric_limits<float>::max();
        float max_y = -std::numeric_limits<float>::max();
        float min_z = std::numeric_limits<float>::max();
        float max_z = -std::numeric_limits<float>::max();

        for (auto pit = cluster_indices[i].indices.begin(); pit != cluster_indices[i].indices.end(); ++pit)
        {
            //fill new colored cluster point by point
            pcl::PointXYZ p;
            p.x = cloud_filtered->points[*pit].x;
            p.y = cloud_filtered->points[*pit].y;
            p.z = cloud_filtered->points[*pit].z;

            centroid_.x += p.x;
            centroid_.y += p.y;
            centroid_.z += p.z;

            if (p.x < min_x)
                min_x = p.x;
            if (p.y < min_y)
                min_y = p.y;
            if (p.z < min_z)
                min_z = p.z;
            if (p.x > max_x)
                max_x = p.x;
            if (p.y > max_y)
                max_y = p.y;
            if (p.z > max_z)
                max_z = p.z;
        }

        //min, max points
        min_point_.x = min_x;
        min_point_.y = min_y;
        min_point_.z = min_z;

        max_point_.x = max_x;
        max_point_.y = max_y;
        max_point_.z = max_z;

        //calculate centroid 计算质心
        if (cluster_indices[i].indices.size() > 0)
        {
            centroid_.x /= cluster_indices[i].indices.size();
            centroid_.y /= cluster_indices[i].indices.size();
            centroid_.z /= cluster_indices[i].indices.size();
        }

        //calculate bounding box
        double length_ = max_point_.x - min_point_.x;
        double width_ = max_point_.y - min_point_.y;
        double height_ = max_point_.z - min_point_.z;

        
        // std::cout << "聚类序号:" << i << std::endl;
        // std::cout << "center_x:" << min_point_.x + length_ / 2 << std::endl;
        // std::cout << "center_y:" << min_point_.y + width_ / 2 << std::endl;
        // std::cout << "center_z:" << min_point_.z + height_ / 2 << std::endl;

        // std::cout << "length:" << ((length_ < 0) ? -1 * length_ : length_) << std::endl;
        // std::cout << "width:" << ((width_ < 0) ? -1 * width_ : width_) << std::endl;
        // std::cout << "cheight:" << ((height_ < 0) ? -1 * height_ : height_) << std::endl;

        output_width = ((width_ < 0) ? -1 * width_ : width_); 
    }


    // 将某个结果信息实时发布出去
    std_msgs::String msg;
    std::stringstream ss;
    ss << std::to_string(output_width);
    msg.data = ss.str();
    pub.publish(msg);
}



int main(int argc, char **argv)
{
    //创建node第一步需要用到的函数
    ros::init(argc, argv, "jisuan_julei"); //第3个参数为本节点名，

    //ros::NodeHandle : 和topic、service、param等交互的公共接口
    //创建ros::NodeHandle对象，也就是节点的句柄，它可以用来创建Publisher、Subscriber以及做其他事情。
    //句柄(Handle)这个概念可以理解为一个“把手”，你握住了门把手，就可以很容易把整扇门拉开，而不必关心门是
    //什么样子。NodeHandle就是对节点资源的描述，有了它你就可以操作这个节点了，比如为程序提供服务、监听某个topic上的消息、访问和修改param等等。
    ros::NodeHandle nd; //实例化句柄，初始化node
    
     
    // Create a ROS subscriber for the input point cloud
    ros::Subscriber sub = nd.subscribe("/camera1/depth/color/points", 1, pclCloudCallback);
    std::cout << "sub:" << sub << std::endl;

    // Create a ROS publisher for the output point cloud
    pub = nd.advertise<std_msgs::String>("julei_out", 1);

    ros::spin();

    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(julei_pkg)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  cv_bridge
)


###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES julei_pkg
#  CATKIN_DEPENDS roscpp rospy std_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)



#此句要在下面target_link_libraries语句之前
add_executable(julei julei.cpp)


#PCL PCL PCL PCL PCL PCL PCL PCL PCL PCL
#如果你想把 PCL 里所有的模块都找到，那就这样写
find_package(PCL REQUIRED)
#为了让 CMake 寻找到 PCL 的头文件，需要以下三句
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
#把我们生成的可执行二进制文件链接到 PCL 的库
target_link_libraries(julei ${PCL_LIBRARIES})


target_link_libraries(julei ${catkin_LIBRARIES})

package.xml

<?xml version="1.0"?>
<package format="2">
  <name>julei_pkg</name>
  <version>0.0.0</version>
  <description>The julei_pkg package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="ym@todo.todo">ym</maintainer>


  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>TODO</license>


  <!-- Url tags are optional, but multiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <!-- <url type="website">http://wiki.ros.org/julei_pkg</url> -->


  <!-- Author tags are optional, multiple are allowed, one per tag -->
  <!-- Authors do not have to be maintainers, but could be -->
  <!-- Example: -->
  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->


  <!-- The *depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
  <!-- Examples: -->
  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
  <!--   <depend>roscpp</depend> -->
  <!--   Note that this is equivalent to the following: -->
  <!--   <build_depend>roscpp</build_depend> -->
  <!--   <exec_depend>roscpp</exec_depend> -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use build_export_depend for packages you need in order to build against this package: -->
  <!--   <build_export_depend>message_generation</build_export_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use exec_depend for packages you need at runtime: -->
  <!--   <exec_depend>message_runtime</exec_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  <!-- Use doc_depend for packages you need only for building documentation: -->
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>cv_bridge</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <build_export_depend>cv_bridge</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
   <exec_depend>cv_bridge</exec_depend>


  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>