本文主要介紹ROS系統(tǒng)中如何訂閱并解碼realsense點云數(shù)據，并對點云進行稀疏、去噪、聚類。

• 環(huán)境配置見《ROS系統(tǒng)中從零開始部署YoloV4目標檢測算法（3種方式）》
• 需要安裝的第三方庫：PCL

package文件結構

程序結構：

• main.cpp（自己的聚類程序）
• CMakeLists.txt（創(chuàng)建package時自動生成的，需要改造內容）
• package.xml（創(chuàng)建package時自動生成的，需要改造內容）
• include 文件夾（創(chuàng)建package時自動生成的，空文件夾）
• src 文件夾（創(chuàng)建package時自動生成的，空文件夾）

實現(xiàn)一個初步的聚類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) 
{//修改相機參數(shù)的方法。// //備注：如果提示 No module named 'rospkg'，請退出conda環(huán)境再執(zhí)行l(wèi)aunch啟動。// system("rosrun dynamic_reconfigure dynparam set /camera/rgb_camera/ enable_auto_exposure 0");// 創(chuàng)建一個輸出的數(shù)據格式sensor_msgs::PointCloud2 output;  //ROS中點云的數(shù)據格式//對數(shù)據進行處理pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);output = *input;pcl::fromROSMsg(output,*cloud);std::cout<<"direct_trans: "<<std::endl;//*********************** 1 點云稀疏化 ***********************//// 創(chuàng)建過濾對象：使用1cm（0.01）的葉大小對數(shù)據集進行下采樣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);//實例化模型參數(shù)pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());//提取到的平面保存至cloud_planepcl::PCDWriter writer;seg.setOptimizeCoefficients(true);//可選設置,設置模型系數(shù)需要優(yōu)化seg.setModelType(pcl::SACMODEL_PLANE);//設置分割的模型類型seg.setMethodType(pcl::SAC_RANSAC);//設置分割的參數(shù)估計方法seg.setMaxIterations(100);//最大迭代次數(shù)seg.setDistanceThreshold(0.02);//設置內點到模型的距離允許最大值int i = 0, nr_points = (int)cloud_filtered->points.size();//計數(shù)變量i，記下提取的平面的個數(shù)while (cloud_filtered->points.size() > 0.3 * nr_points){//從剩余的云中分割最大的平面組件seg.setInputCloud(cloud_filtered);//設置要分割的點云seg.segment(*inliers, *coefficients);   //分割，存儲分割結果到點集合inliers及存儲平面模型的系數(shù)coefficientsif (inliers->indices.size() == 0)//如果平面點索引的數(shù)量為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 diskextract.filter(*cloud_plane);//保存提取到的平面//std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;// Remove the planar inliers, extract the restextract.setNegative(true);//提取外點（除第一個平面之外的點）extract.filter(*cloud_f);//保存除平面之外的剩余點cloud_filtered = cloud_f;//將剩余點作為下一次分割、提取的平面的輸入點云}//*********************** 3 點云歐式聚類 ***********************//// 為提取的搜索方法創(chuàng)建KdTree對象pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);tree->setInputCloud(cloud_filtered); //將無法提取平面的點云作為 cloud_filteredstd::vector<pcl::PointIndices> cluster_indices; //保存每一種聚類，每一種聚類下還有具體的聚類的點pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;//實例化一個歐式聚類提取對象//realsense點云XYZ值單位是：米m，Kinect點云XYZ值單位是：毫米mm// ec.setClusterTolerance(0.02);     //近鄰搜索的搜索半徑，重要參數(shù), 單位：米 // ec.setMinClusterSize(50);         //設置一個聚類需要的最少點數(shù)目為100ec.setClusterTolerance(0.2);     //近鄰搜索的搜索半徑，重要參數(shù), 單位：米 ec.setMinClusterSize(5);         //設置一個聚類需要的最少點數(shù)目為100ec.setMaxClusterSize(25000);      //設置一個聚類最大點數(shù)目為25000ec.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/83015570double 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 pointpcl::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 pointsmin_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 boxdouble 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_); }// 將某個結果信息實時發(fā)布出去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)
{//創(chuàng)建node第一步需要用到的函數(shù)ros::init(argc, argv, "jisuan_julei"); //第3個參數(shù)為本節(jié)點名，//ros::NodeHandle : 和topic、service、param等交互的公共接口//創(chuàng)建ros::NodeHandle對象，也就是節(jié)點的句柄，它可以用來創(chuàng)建Publisher、Subscriber以及做其他事情。//句柄(Handle)這個概念可以理解為一個“把手”，你握住了門把手，就可以很容易把整扇門拉開，而不必關心門是//什么樣子。NodeHandle就是對節(jié)點資源的描述，有了它你就可以操作這個節(jié)點了，比如為程序提供服務、監(jiān)聽某個topic上的消息、訪問和修改param等等。ros::NodeHandle nd; //實例化句柄，初始化node// Create a ROS subscriber for the input point cloudros::Subscriber sub = nd.subscribe("/camera1/depth/color/points", 1, pclCloudCallback);std::cout << "sub:" << sub << std::endl;// Create a ROS publisher for the output point cloudpub = 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 COMPONENTSroscpprospystd_msgscv_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})
#把我們生成的可執(zhí)行二進制文件鏈接到 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>

總結

以上是生活随笔為你收集整理的ROS系统中实现点云聚类（realsense数据源）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。