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