C++点云PCL基础ROS代码
目录
一、概念
1、点云的结构公共字段
2、点云的类型
3、ROS的PCL接口
二、创建点云
三、转PCD
四、滤波采样
五、点云配准ICP
六、建立KD树
七、点云分割
八、可视化点云
一、概念
1、点云的结构公共字段
- PCL包含一个重要的数据结构，被设计成一个模板类，把点的类型当做模板类的参数。
- header：pcl::PCLHeader 记录了点云的获取时间
- points：std::vector
- 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
void fromPCL(const pcl::PointCloud
二、创建点云
三、转PCD
- 在工作空间创建一个ROS软件包
catkin_create_pkg chapter6_tutorials pcl_conversions pcl_ros pcl_msgs sensor_msgs roscd chapter6_tutorials mkdir src - 写C++程序
//创建一个ROS节点发布100个元素的点云 #include#include #include #include #include main(int argc , char** argv) { //节点初始化 创建对象 ros::init(argc,argv,"pcl_sample"); ros::NodeHandle nh; ros::Publisher pcl_pub = nh.advertise - 更改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}) - 编译运行
roscore rosrun chapter6_tutorals pcl_create rosrun rviz rviz
1、 读取PCD文件
//加载PCD文件并且将点云发布为ROS消息
#include
#include
#include
#include
#include
#include
main(int argc, char **argv)
{
ros::init (argc,argv,"pcl_read");//定义节点
ros::NodeHandle nh;
ros::Publisher pcl_pub = nh.advertise("pcl_output",1);
sensor_msgs::PointCloud2 output;
pcl::PointCloud 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
#include
#include
#include
#include
#include
void cloudCB(const sensor_msgs::PointCloud2 &input)
{
pcl::PointCloud 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
#include
#include
#include
#include
#include
#include
class cloudHandler
{
public:
cloudHandler()
{
//接收pcl_output节点的数据，在cloudCB回调函数进行处理，以pcl_filtered节点发出去
pcl_sub = nh.subscribe("pcl_output",10,&cloudHandler::cloudCB, this);//定义接收者
pcl_pub = nh.advertise("pcl_filtered",1);//定义发布者
}
void cloudCB(const sensor_msgs::PointCloud2& input)
{
//创建接收点云 发出点云 发出消息的变量
pcl::PointCloud cloud;
pcl::PointCloud cloud_filtered;
sensor_msgs::PointCloud2 output;
//把ROS消息转化为pcl
pcl::fromROSMsg(input,cloud);
//定义一个滤波分析算法
pcl::StatisticalOutlierRemoval 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
#include
#include
#include
#include
#include
#include
class cloudHandler
{
public:
cloudHandler()
{
pcl_sub=nh.subscribe("pcl_downsampled",10,&cloudHandler::cloudCB,this);
pcl_pub=nh.advertise("pcl_matched",1);
}
void cloudCB(const sensor_msgs::PointCloud2 &input)
{
pcl::PointCloud cloud_in; //要转换的点云
pcl::PointCloud cloud_out; //与点云对齐的点云
pcl::PointCloud cloud_aligned; //最终点云
sensor_msgs::PointCloud2 output;
pcl::fromROSMsg(input,cloud_in); //ROS转换pcl
cloud_out = cloud_in;
for(size_t i=0; iicp;
//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
#include
#include
#include
#include
#include
#include
#include
class cloudHandler
{
public:
cloudHandler()
{
pcl_sub=nh.subscribe("pcl_downsampled",10,&cloudHandler::cloudCB,this);
pcl_pub=nh.advertise("pcl_part",1);
}
void cloudCB(const sensor_msgs::PointCloud2 &input)
{
pcl::PointCloud cloud;
pcl::PointCloud cloud_part;
sensor_msgs::PointCloud2 output;
pcl::fromROSMsg(input,cloud); //ROS转换pcl
//创建八叉树
float resolution = 128.0f;
pcl::octree::OctreePointCloudSearch 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::vectorradiusIdx;
std::vectorradiusSQDist;
if(octree.radiusSearch (center_point,radius,radiusIdx,radiusSQDist)>0)
{
for(size_t i = 0;i 七、点云分割
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
class cloudHandler
{
public:
cloudHandler()
{
//接收pcl_output节点的数据，在cloudCB回调函数进行处理，以pcl_filtered节点发出去
pcl_sub = nh.subscribe("pcl_downsampled",10,&cloudHandler::cloudCB, this);//定义接收者
pcl_pub = nh.advertise("pcl_segmented",1);//定义发布者
ind_pub = nh.advertise("point_indices",1);//PointIndices消息储存一个点云中心点的索引
coef_pub = nh.advertise("planar_coef",1);//ModeCoefficients消息储存一个数学模型的系数
}
void cloudCB(const sensor_msgs::PointCloud2& input)
{
//创建接收点云 发出点云 发出消息的变量
pcl::PointCloud cloud;
pcl::PointCloud cloud_segmented;
//把传入的ROS消息转化为pcl
pcl::fromROSMsg(input,cloud);
//创建两个消息对象
pcl::ModelCoefficients coefficients;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
//定义分割算法模型
pcl::SACSegmentation 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 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
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
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 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;
}