一、概念

1、点云的结构公共字段

2、点云的类型

3、ROS的PCL接口

二、创建点云

三、转PCD

四、滤波采样

五、点云配准ICP

六、建立KD树

七、点云分割

八、可视化点云

一、概念

1、点云的结构公共字段

PCL包含一个重要的数据结构，被设计成一个模板类，把点的类型当做模板类的参数。
header：pcl::PCLHeader 记录了点云的获取时间
points：std::vector<PointT,...>储存所有点的容器
width：指定点云组织成图像时的宽度
height：指定点云组成图像时的高度
is_dense: 指定点云中是否有无效值
sensor_origin_:是Eigen::Vector4f类型，传感器相对于原点平移所得的位姿
sensor_orientation_:是Eigen::Quaternionf类型，定义传感器旋转所得的位姿

2、点云的类型

PointT是pcl::PointCloud类的模板参数，定义点云的类型

pcl::PointXYZ 位置
pcl::PointXYZI 位置+亮度
pcl::PointXYZRGBA 位置+颜色+透明度
pcl::PointXYZRGB 位置+颜色
pcl::Normal 表示曲面上给定点处的法线以及测量的曲率
pcl::PointNormal 曲率信息+位置

3、ROS的PCL接口

定义不同的消息类型去处理点云的数据

std_msgs::Header 不是真的消息类型，它包含发送的时间、序列号等

sensor_msgs::PointCloud2 用来转换pcl::PointCloud类型

pcl_msgs::PointIndices 储存点云的索引

pcl_msgs::PolygonMesh 保存了描绘网格、定点和多边形

pcl_msgs::Vertices 将一组定点的索引保存在数组中

pcl_msgs::ModelCoefficients 储存一个模型的不同系数，如描述一个平面需要四个参数

用函数转换消息

void fromPCL(const <PCL Type> &,<ROS Message type> &)

void fromPCL(const pcl::PointCloud<T> &, sensor_msgs::PointCloud2 &)

二、创建点云

三、转PCD

在工作空间创建一个ROS软件包

catkin_create_pkg chapter6_tutorials pcl_conversions pcl_ros pcl_msgs sensor_msgs
roscd chapter6_tutorials
mkdir src

写C++程序

//创建一个ROS节点发布100个元素的点云
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>main(int argc , char** argv)
{//节点初始化 创建对象ros::init(argc,argv,"pcl_sample");ros::NodeHandle nh;ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_output",1);//在pcl_output这个话题上发布sensor_msgs::PointCloud2类型的消息。pcl::PointCloud<pcl::PointXYZ> cloud;sensor_msgs::PointCloud2 output;//创建一个消息对象 output//定义点云空间cloud.width = 100;cloud.height = 1;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.0f);cloud.points[i].y=1024*rand()/(RAND_MAX+1.0f);cloud.points[i].z=1024*rand()/(RAND_MAX+1.0f);}//转换成ROS信息pcl::toROSMsg(cloud,output);output.header.frame_id="odom";//发布消息ros::Rate loop_rate(1);while(ros::ok()){pcl_pub.publish(output);ros::spinOnce();loop_rate.sleep();}return 0;
}

更改CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(chapter6_tutorials)
find_package(catkin REQUIRED COMPONENTSpcl_conversionspcl_msgspcl_rossensor_msgs)
find_package(PCL REQUIRED)
include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_executable(pcl_sample src/pcl_sample.cpp)
target_link_libraries(pcl_sample ${catkin_LIBRARIES} ${PCL_LIBRARIES})

编译运行

roscore
rosrun chapter6_tutorals pcl_create
rosrun rviz rviz

1、读取PCD文件

//加载PCD文件并且将点云发布为ROS消息
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>main(int argc, char **argv)
{ros::init (argc,argv,"pcl_read");//定义节点ros::NodeHandle nh;ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_output",1);sensor_msgs::PointCloud2 output;pcl::PointCloud<pcl::PointXYZ> cloud;pcl::io::loadPCDFile("pcl_regfinal.pcd",cloud);//加载pcd文件 为cloudpcl::toROSMsg(cloud,output);output.header.frame_id="odom";//发布消息ros::Rate loop_rate(1);while(ros::ok()){pcl_pub.publish(output);ros::spinOnce();loop_rate.sleep();}return 0;
}在CMAKE文件中添加该文件
之后将pcd文件放在chapter6_tutorials/data文件夹中，在这个文件夹运行
rosrun chapter6_tutorials pcl_read

2.保存为PCD文件

//将接收的点云保存在PCD文件中
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>void cloudCB(const sensor_msgs::PointCloud2 &input)
{pcl::PointCloud<pcl::PointXYZ> cloud;pcl::fromROSMsg(input,cloud);pcl::io::savePCDFileASCII ("write_pcd_test.pcd",cloud);
}main (int argc,char **argv)
{ros::init(argc,argv,"pcl_write");ros::NodeHandle nh;//定义接受者ros::Subscriber bat_sub = nh.subscribe("pcl_output",10,cloudCB);//cloudCB为回调函数ros::spin();return 0;
}

四、滤波采样

滤波和缩减采样

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/statistical_outlier_removal.h>class cloudHandler
{
public:cloudHandler(){//接收pcl_output节点的数据，在cloudCB回调函数进行处理，以pcl_filtered节点发出去pcl_sub = nh.subscribe("pcl_output",10,&cloudHandler::cloudCB, this);//定义接收者pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_filtered",1);//定义发布者}void cloudCB(const sensor_msgs::PointCloud2& input){//创建接收点云 发出点云 发出消息的变量pcl::PointCloud<pcl::PointXYZ> cloud;pcl::PointCloud<pcl::PointXYZ> cloud_filtered;sensor_msgs::PointCloud2 output;//把ROS消息转化为pclpcl::fromROSMsg(input,cloud);//定义一个滤波分析算法pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;statFilter.setInputCloud(cloud.makeShared()); //cloud传入statFilter.setMeanK(10);statFilter.setStddevMulThresh(0.2);statFilter.filter(cloud_filtered); //cloud_filtered传出//将PCL类型 cloud_filtered的数据 转化成ROS类型 output的数据pcl::toROSMsg(cloud_filtered,output);pcl_pub.publish(output);}
protected://创建节点 接受者 发布者ros::NodeHandle nh;ros::Subscriber pcl_sub;ros::Publisher pcl_pub;};main (int argc,char** argv)
{ros::init(argc,argv,"pcl_filter");cloudHandler handler;ros::spin();return 0;
}

五、点云配准ICP

//迭代最近点ICP算法
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/registration/icp.h>
#include <pcl/io/pcd_io.h>class cloudHandler
{
public:cloudHandler(){pcl_sub=nh.subscribe("pcl_downsampled",10,&cloudHandler::cloudCB,this);pcl_pub=nh.advertise<sensor_msgs::PointCloud2>("pcl_matched",1);}void cloudCB(const sensor_msgs::PointCloud2 &input){pcl::PointCloud<pcl::PointXYZ> cloud_in; //要转换的点云pcl::PointCloud<pcl::PointXYZ> cloud_out; //与点云对齐的点云pcl::PointCloud<pcl::PointXYZ> cloud_aligned; //最终点云sensor_msgs::PointCloud2 output;pcl::fromROSMsg(input,cloud_in);  //ROS转换pclcloud_out = cloud_in;for(size_t i=0; i<cloud_in.points.size();i++){cloud_out.points[i].x = cloud_in.points[i].x+0.7f;//点云平移}//对两个点云进行配准pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ>icp;//in和移动的out进行配准icp.setInputSource(cloud_in.makeShared());icp.setInputTarget(cloud_out.makeShared());icp.setMaxCorrespondenceDistance(5);icp.setMaximumIterations(100);icp.setTransformationEpsilon(1e-12);icp.setEuclideanFitnessEpsilon(0.1);icp.align(cloud_aligned);//发布pcl::toROSMsg(cloud_aligned,output);pcl_pub.publish(output);}
protected:ros::NodeHandle nh;ros::Subscriber pcl_sub;ros::Publisher pcl_pub;
};main (int argc, char **argv)
{ros::init(argc,argv,"pcl_matching");cloudHandler handler;ros::spin();return 0;
}

六、建立KD树

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/registration/icp.h>
#include <pcl/io/pcd_io.h>
#include <pcl/octree/octree.h>class cloudHandler
{
public:cloudHandler(){pcl_sub=nh.subscribe("pcl_downsampled",10,&cloudHandler::cloudCB,this);pcl_pub=nh.advertise<sensor_msgs::PointCloud2>("pcl_part",1);}void cloudCB(const sensor_msgs::PointCloud2 &input){pcl::PointCloud<pcl::PointXYZ> cloud;pcl::PointCloud<pcl::PointXYZ> cloud_part; sensor_msgs::PointCloud2 output;pcl::fromROSMsg(input,cloud);  //ROS转换pcl//创建八叉树float resolution = 128.0f;pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);octree.setInputCloud(cloud.makeShared());octree.addPointsFromInputCloud();//定义分区一个中心点pcl::PointXYZ center_point;center_point.x = -2.9;center_point.y = -2.7;center_point.z = -0.5;//在指定半径内使用八叉树搜寻float radius = 0.5;std::vector<int>radiusIdx;std::vector<float>radiusSQDist;if(octree.radiusSearch (center_point,radius,radiusIdx,radiusSQDist)>0){for(size_t i = 0;i<radiusIdx.size();++i){cloud_part.points.push_back(cloud.points[radiusIdx[i]]);}}//发布pcl::toROSMsg(cloud_part,output);output.header.frame_id = "odom";pcl_pub.publish(output);}
protected:ros::NodeHandle nh;ros::Subscriber pcl_sub;ros::Publisher pcl_pub;
};main (int argc, char **argv)
{ros::init(argc,argv,"pcl_part");cloudHandler handler;ros::spin();return 0;
}

七、点云分割

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <sensor_msgs/PointCloud2.h>class cloudHandler
{
public:cloudHandler(){//接收pcl_output节点的数据，在cloudCB回调函数进行处理，以pcl_filtered节点发出去pcl_sub = nh.subscribe("pcl_downsampled",10,&cloudHandler::cloudCB, this);//定义接收者pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_segmented",1);//定义发布者ind_pub = nh.advertise<pcl_msgs::PointIndices>("point_indices",1);//PointIndices消息储存一个点云中心点的索引coef_pub = nh.advertise<pcl_msgs::ModelCoefficients>("planar_coef",1);//ModeCoefficients消息储存一个数学模型的系数}void cloudCB(const sensor_msgs::PointCloud2& input){//创建接收点云 发出点云 发出消息的变量pcl::PointCloud<pcl::PointXYZ> cloud;pcl::PointCloud<pcl::PointXYZ> cloud_segmented;//把传入的ROS消息转化为pclpcl::fromROSMsg(input,cloud);//创建两个消息对象pcl::ModelCoefficients coefficients;pcl::PointIndices::Ptr inliers(new pcl::PointIndices());//定义分割算法模型pcl::SACSegmentation<pcl::PointXYZ> segmentation;//创建算法对象segmentation.setModelType(pcl::SACMODEL_PLANE);//期望匹配的数学模型segmentation.setMethodType(pcl::SAC_RANSAC);//用到的算法segmentation.setMaxIterations(1000);//定义最大迭代segmentation.setDistanceThreshold(0.01);//到模型最大距离segmentation.setInputCloud(cloud.makeShared());//输入segmentation.segment(*inliers, coefficients);//分割//将内点转化成ROS消息pcl_msgs::ModelCoefficients ros_coefficients;pcl_conversions::fromPCL(coefficients, ros_coefficients);ros_coefficients.header.stamp = input.header.stamp;coef_pub.publish(ros_coefficients);//将模型系数转化成ROS消息pcl_msgs::PointIndices ros_inliers;pcl_conversions::fromPCL(*inliers,ros_inliers);ros_inliers.header.stamp = input.header.stamp;ind_pub.publish(ros_inliers);//提取分割点云pcl::ExtractIndices<pcl::PointXYZ> extract;extract.setInputCloud(cloud.makeShared());extract.setIndices(inliers);extract.setNegative(false);extract.filter(cloud_segmented);//分割后储存在cloud_segmentedsensor_msgs::PointCloud2 output;//将PCL类型 cloud_filtered的数据 转化成ROS类型 output的数据pcl::toROSMsg(cloud_segmented,output);pcl_pub.publish(output);}
protected://创建节点 接受者 发布者ros::NodeHandle nh;ros::Subscriber pcl_sub;ros::Publisher pcl_pub, ind_pub, coef_pub;};main (int argc,char** argv)
{ros::init(argc,argv,"pcl_segment");cloudHandler handler;ros::spin();return 0;
}

八、可视化点云

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <sensor_msgs/PointCloud2.h>
#include <iostream>
#include <pcl/visualization/cloud_viewer.h>class cloudHandler
{
public:cloudHandler(): viewer("Cloud Viewer"){//接收pcl_output节点的数据，在cloudCB回调函数进行处理，以pcl_filtered节点发出去pcl_sub = nh.subscribe("pcl_output",10,&cloudHandler::cloudCB, this);//定义接收者viewer_timer = nh.createTimer(ros::Duration(0.1),&cloudHandler::timerCB,this);//创建定时器}void cloudCB(const sensor_msgs::PointCloud2& input){pcl::PointCloud<pcl::PointXYZ> cloud;pcl::fromROSMsg(input,cloud);viewer.showCloud(cloud.makeShared());    }//定时器  如果窗口关闭，节点关闭void timerCB(const ros::TimerEvent&){if (viewer.wasStopped()){ros::shutdown();}}
protected://创建节点 接受者 发布者ros::NodeHandle nh;ros::Subscriber pcl_sub;pcl::visualization::CloudViewer viewer;ros::Timer viewer_timer;
};main (int argc,char** argv)
{ros::init(argc,argv,"pcl_visualize");cloudHandler handler;ros::spin();return 0;
}

C++点云PCL基础ROS代码相关推荐

PCL学习一：点云与PCL基础
参考引用黑马机器人 | PCL-3D点云 PCL(Point Cloud Library)学习记录 PCL点云库学习笔记(文章链接汇总) 1. 点云概述点云(Point Cloud)是三维空间中, ...
ROS机器人程序设计（原书第2版）补充资料（陆）第六章点云 PCL
ROS机器人程序设计(原书第2版)补充资料 (陆) 第六章点云 PCL 书中,大部分出现hydro的地方,直接替换为indigo或jade或kinetic,即可在对应版本中使用. RGBD深度摄像头 ...
html代码大全百度云,html代码大全(基础使用代码)(颜色代码完整版)
该楼层疑似违规已被系统折叠隐藏此楼查看此楼您在使用以下基础使用代码时请把{}换成<>!!!) (这样这个命令才成立!!!) 基础使用代码: 1)贴图:{img src="图片 ...
爆肝5万字❤️Open3D 点云数据处理基础（Python版）
Open3D 点云数据处理基础(Python版) 文章目录 1 概述 2 安装 2.1 PyCharm 与 Python 安装 2.3 Anaconda 安装 2.4 Open3D 0.13.0 安装 ...
无人驾驶汽车系统入门（二十五）——基于欧几里德聚类的激光雷达点云分割及ROS实现
无人驾驶汽车系统入门(二十五)--基于欧几里德聚类的激光雷达点云分割及ROS实现上一篇文章中我们介绍了一种基于射线坡度阈值的地面分割方法,并且我们使用pcl_ros实现了一个简单的节点,在完成了点云 ...
云原生基础架构的最佳状态，就是没有架构？
云栖号资讯:[点击查看更多行业资讯] 在这里您可以找到不同行业的第一手的上云资讯,还在等什么,快来! 云原生基础架构是通向云原生时代的基石,对于很多架构师来说,上云之后,架构为什么成为了云原生架构而不 ...
bresenham算法画圆mfc实现_kd-tree理论以及在PCL 中的代码的实现
通过雷达,激光扫描,立体摄像机等三维测量设备获取的点云数据,具有数据量大,分布不均匀等特点,作为三维领域中一个重要的数据来源,点云主要是表征目标表面的海量点的集合,并不具备传统网格数据的几何拓扑信息, ...
服务器测速文件,云服务器测速代码
云服务器测速代码内容精选换一换基于云服务器访问安全的考虑,云服务器的访问密钥需定期更换,有时需要禁用SSH密码登录方式.本节操作介绍禁用SSH密码方式连接云服务器的操作步骤.该设置方法仅对SSH ...
国内代码托管中心-码云、自建代码托管平台-GitLab
文章目录第 9 章国内代码托管中心-码云 9.1 简介 9.2 码云帐号注册和登录 9.3 码云创建远程库 9.4 IDEA 集成码云 9.4.1 IDEA 安装码云插件 9.4.2 IDEA 连 ...

C++点云PCL基础ROS代码

一、概念

1、点云的结构公共字段

2、点云的类型

3、ROS的PCL接口

二、创建点云

三、转PCD

四、滤波采样

五、点云配准ICP

六、建立KD树

七、点云分割

八、可视化点云

C++点云PCL基础ROS代码相关推荐

最新文章

热门文章