目录

一、概念

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

  1.  在工作空间创建一个ROS软件包 
    catkin_create_pkg chapter6_tutorials pcl_conversions pcl_ros pcl_msgs sensor_msgs
roscd chapter6_tutorials
mkdir src
  2. 写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;
}
  3. 更改CMakeLists.txt 
    cmake_minimum_required(VERSION 3.0.2)
project(chapter6_tutorials)
find_package(catkin REQUIRED COMPONENTS
  pcl_conversions
  pcl_msgs
  pcl_ros
  sensor_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})
  4. 编译运行 
    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文件 为cloud
	
	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;
}


在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消息转化为pcl
		pcl::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转换pcl
		cloud_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消息转化为pcl
		pcl::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_segmented
		
		sensor_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++ # 开发语言
Logo
向您推荐>>Eolink开发者社区

权威｜前沿｜技术｜干货｜国内首个API全生命周期开发者社区

更多推荐

深入理解 Mocha 测试框架：从零实现一个 Mocha

前言什么是自动化测试自动化测试在很多团队中都是Devops环节中很难执行起来的一个环节，主要原因在于测试代码的编写工作很难抽象，99%的场景都需要和业务强绑定，而且写测试代码的编写工作量往往比编写实际业务代码的工作量更多。在一些很多业务场景中投入产出比很低，适合写自动化测试的应该是那些中长期业务以及一些诸如组件一样的基础库。自动化测试是个比较大的概念，其中分类也比较多，比如单元测试，端对端测试，集

avatar 云原生

ELK实现containerd的容器日志采集展示【基于logging的全栈监测】

企业级ELK Stack构建介绍

avatar 云原生

(20200916 Solved)docker-compose up创建容器自动退出

问题描述如题，创建容器后自动退出了。并且docker start container无效解决方案原因是缺失了控制终端的配置，需要在docker-compose.yml中增加tty:true ，有时候这样也不行，需要再增加一个command:/bin/bash，命令不一定是这个，需要是一个不会退出的命令，然后用-d后台启动容器。Referencesdocker-compose启动容器后自动退出...

avatar 云原生