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