PointCloud 点云处理方法总结(代码案例版)
本文将自己在点云处理过程中,遇到的一些常用的具体方法进行总结,不介绍点云数据处理的基本概念,主要是处理过程中的代码总结,以及参考案例。
1. 点云数据类型转换:
ROS msg
, PCLPointCloud2
, PointXYZ
三种数据类型之间的转换。 ROS msg to PCLPointCloud2
const sensor_msgs::PointCloud2ConstPtr& cloud_msg
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl_conversions::toPCL(*cloud_msg, *cloud);
PCLPointCloud2 to ROS msg
sensor_msgs::PointCloud2 output;
pcl::PCLPointCloud2 cloud_filtered;
pcl_conversions::moveFromPCL(cloud_filtered, output);
PointXYZ to PCLPointCloud2
pcl::PointCloud<pcl::PointXYZ> local_map;
pcl::PCLPointCloud2* local_map_pc2_ptr = new pcl::PCLPointCloud2;
pcl::toPCLPointCloud2(local_map, *local_map_pc2_ptr);
PCLPointCloud2 to PointXYZ
pcl::PCLPointCloud2 local_map_pc2;
pcl::fromPCLPointCloud2(local_map_pc2, local_map);
ROS msg to PointXYZ
sensor_msgs::PointCloud2 output;
pcl::PointCloud<pcl::PointXYZ> icp_cloud_;
pcl::fromROSMsg(output, icp_cloud_);
ROS msg to PointXYZ
sensor_msgs::PointCloud2 output;
pcl::PointCloud<pcl::PointXYZ> icp_cloud_;
pcl::fromROSMsg(output, icp_cloud_);
PointXYZ to ROS msg
pcl::toROSMsg(local_map,output);
pointer to const pointer 特别的,有时需要将指针转为常量指针
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
2. 降采样点云:
点云常常占用较多计算资源,为了实时处理,需要进行稀疏化,这里 使用VoxelGrid滤波器进行降采样。
pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());
pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
//创建滤波器
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud);
//设置点云稀疏程度
sor.setLeafSize (0.01f, 0.01f, 0.01f);
//降采样后输出到*cloud_filtered
sor.filter (*cloud_filtered);
详情见降采样点云Downsampling a PointCloud using a VoxelGrid filter 3. 剪裁点云: 机器人在运行的过程中,会一直采集点云加入到地图中,而那些较远处或者处于机器人后面的部分,通常不会用到,为了加快处理速度,需要对点云进行剪裁。 沿着某一个轴进行裁剪
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (cloud);
//设置裁剪轴
pass.setFilterFieldName ("z");
//设置裁剪范围
pass.setFilterLimits (0.0, 1.0);
//设置输出
pass.filter (*cloud_filtered);
那么,裁剪三个轴呢?注意,不能一次设置裁剪三个轴(我测试过程中是这样),需要分成三步 三个轴同时裁剪
{
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(crop_cloud_ptr);
*crop_cloud_ptr = crop_cloud_;
// Crop horizontally
pass.setFilterFieldName("x");
pass.setFilterLimits(x_limit_left, x_limit_right);
pass.filter(crop_cloud_);
}
{
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(crop_cloud_ptr);
*crop_cloud_ptr = crop_cloud_;
// Crop vertically
pass.setFilterFieldName("y");
pass.setFilterLimits(y_limit_above, y_limit_below);
pass.filter(crop_cloud_);
}
{
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(crop_cloud_ptr);
*crop_cloud_ptr = crop_cloud_;
// Crop depth-wise
pass.setFilterFieldName("z");
pass.setFilterLimits(z_limit_behind, z_limit_ahead);
pass.filter(crop_cloud_);
}
发现另一个pcl::CropBox可以直接进行裁剪 CropBox裁剪
pcl::CropBox<pcl::PointXYZRGBA> boxFilter;
boxFilter.setMin(Eigen::Vector4f(minX, minY, minZ, 1.0));
boxFilter.setMax(Eigen::Vector4f(maxX, maxY, maxZ, 1.0));
boxFilter.setInputCloud(body);
boxFilter.filter(*bodyFiltered);
passthrough滤波器例子 CropBox裁剪函数查询
4. 迭代最近点法ICP match:
使用迭代最近点法可以将两个具有相同特征的点云进行配准。
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
迭代最近点使用案例,How to use iterative closest point,How to incrementally register pairs of clouds 5. 随机抽样一致算法Random Sample Consensus: 随机抽样一致算法可以在给定正确数据的情况下,去除点云中的由于错误的测量引起的离群点云数据。
//针对点云cloud,建立随机抽样一致的模型
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
ransac.setDistanceThreshold (.01);
ransac.computeModel();
//去除离群点后输出
ransac.getInliers(inliers);
具体例子见How to use Random Sample Consensus model 相关开源代码推荐 maplab https://github.com/ethz-asl/maplab ethzasl_icp_mapping https://github.com/ethz-asl/ethzasl_icp_mapping libpointmatcher https://github.com/ethz-asl/libpointmatcher