Filter

Removing Points

• Filtering a PointCloud using a PassThrough filter
• Downsampling a PointCloud using a VoxelGrid filter
• Removing sparse outliers using StatisticalOutlierRemoval
• Projecting points using a parametric model
• Extracting indices from a PointCloud
• Removing outliers using a Conditional or RadiusOutlier removal

Passthrough filter

Reference : http://pointclouds.org/documentation/tutorials/passthrough.php#passthrough

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

/*建立過濾物件*/
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (cloud);
/*設定根據哪個attribute來濾掉點*/
pass.setFilterFieldName ("z");
/*設定不濾掉的attribute值範圍*/
pass.setFilterLimits (0.0, 1.0);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered);

Voxel grid filter

#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());
pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());

//Fill in the cloud data
pcl::PCDReader reader;
reader.read ("table_scene_lms400.pcd", *cloud); 

/*建立VoxelGrid過濾物件*/
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (0.01f, 0.01f, 0.01f); /*x,y,z軸每隔1cm取樣*/
sor.filter (*cloud_filtered);

std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height 
       << " data points (" << pcl::getFieldsList (*cloud_filtered) << ").";

pcl::PCDWriter writer;
writer.write("table_scene_lms400_downsampled.pcd", *cloud_filtered, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);

Statistical Outlier Removal

#include <pcl/io/pcd_io.h>
#include <pcl/filters/statistical_outlier_removal.h>

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

// Fill in the cloud data
pcl::PCDReader reader;
reader.read<pcl::PointXYZ> ("table_scene_lms400.pcd", *cloud);

/*建立StatisticalOutlierRemoval過濾物件*/
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud (cloud);
sor.setMeanK (50);  //針對每個點找鄰近50個點做運算
sor.setStddevMulThresh (1.0);  //比平均距離大於1.0倍標準差的點會被濾除
sor.filter (*cloud_filtered);

pcl::PCDWriter writer;
writer.write<pcl::PointXYZ> ("table_scene_lms400_inliers.pcd", *cloud_filtered, false);

sor.setNegative (true);  //指定cloud_filtered為被排除的點群(outliers)
sor.filter (*cloud_filtered); //重新濾除
writer.write<pcl::PointXYZ> ("table_scene_lms400_outliers.pcd", *cloud_filtered, false);

Projecting points using a parametric model

In this tutorial we will learn how to project points onto a parametric model (e.g., plane, sphere, etc). The parametric model is given through a set of coefficients – in the case of a plane, through its equation: ax + by + cz + d = 0.

#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);

cloud->width  = 5;
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);
}

// Create a set of planar coefficients with X=Y=0,Z=1, ax+by+cz+d=0, where a=b=d=0, and c=1, or said differently, the X-Y plane

pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
coefficients->values.resize (4);
coefficients->values[0] = coefficients->values[1] = 0;
coefficients->values[2] = 1.0;
coefficients->values[3] = 0;

/*建立投影濾除物件*/
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE); //指定模型為平面
proj.setInputCloud (cloud);
proj.setModelCoefficients (coefficients); //指定平面模型的參數值
proj.filter (*cloud_projected);  //將濾除後的結果存於cloud_projected

Extracting indices from a PointCloud

learn how to use an ExtractIndices filter to extract a subset of points from a point cloud based on the indices output by a segmentation algorithm.

#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>


pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), cloud_filtered_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);

// Fill in the cloud data
pcl::PCDReader reader;
reader.read ("table_scene_lms400.pcd", *cloud_blob);

// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud_blob);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered_blob);

// Convert to the templated PointCloud
pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);

// Write the downsampled version to disk
pcl::PCDWriter writer;
writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);

pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());  //模型參數
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());  //在模型內的點群
/*建立segmentation物件*/
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);  //指定平面模型
seg.setMethodType (pcl::SAC_RANSAC); 以隨機segmentation方法找模型
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);

// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;

int i = 0, nr_points = (int) cloud_filtered->points.size ();
// While 30% of the original cloud is still there
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
  // Segment the largest planar component from the remaining cloud
  seg.setInputCloud (cloud_filtered);
  seg.segment (*inliers, *coefficients);  //找出存在平面模型其參數coefficients與在此平面模型內的點群inliners
  if (inliers->indices.size () == 0)
  { //無法找出平面模型就終止
    std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
    break;
  }

  // Extract the inliers
  extract.setInputCloud (cloud_filtered);
  extract.setIndices (inliers);
  extract.setNegative (false); //設定使用平面模型內的點群
  extract.filter (*cloud_p); //萃取出平面模型內的點

  std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;

  std::stringstream ss;
  ss << "table_scene_lms400_plane_" << i << ".pcd";
  writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);

  // Create the filtering object
  extract.setNegative (true);//設定使用平面模型外的點群
  extract.filter (*cloud_f); //萃取出平面模型外的點
  cloud_filtered.swap (cloud_f); //使目前平面模型外的點群成為新的cloud_filtered，以進入下一個迴圈找出下個平面
  i++;
}

Extract indices [pcl git]

// STL
#include <iostream>

// PCL
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>

int main (int, char**)
{
  typedef pcl::PointXYZ PointType;
  typedef pcl::PointCloud<PointType> CloudType;
  CloudType::Ptr cloud (new CloudType);
  cloud->is_dense = false;
  PointType p;
  for (unsigned int i = 0; i < 5; ++i)
  {
    p.x = p.y = p.z = static_cast<float> (i);
    cloud->push_back (p);
  }

  pcl::PointIndices indices;
  indices.indices.push_back (0);
  indices.indices.push_back (2);

  pcl::ExtractIndices<PointType> extract_indices;
  extract_indices.setIndices (boost::make_shared<const pcl::PointIndices> (indices));
  extract_indices.setInputCloud (cloud);
  pcl::PointCloud<pcl::PointXYZ>::Ptr output (new pcl::PointCloud<pcl::PointXYZ>);
  extract_indices.filter (*output);

  std::cout << "Output has " << output->points.size () << " points." << std::endl;
  return (0);
}

Removing outliers using a Conditional or RadiusOutlier removal

ConditionalRemoval filter : removes all indices in the given input cloud that do not satisfy one or more given conditions.

RadiusOutlierRemoval filter : removes all indices in it’s input cloud that don’t have at least some number of neighbors within a certain range.

#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
// build the filter
outrem.setInputCloud(cloud);
outrem.setRadiusSearch(0.8);  //設定半徑80cm
outrem.setMinNeighborsInRadius (2); //每個點半徑範圍內有至少2個點該點才不會被濾除
// apply filter
outrem.filter (*cloud_filtered);

/*建立條件物件range_cond*/
pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionAnd<pcl::PointXYZ> ());
/*加入z值 > 0.0條件 GreaterThan */
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::GT, 0.0)));
/*加入z值 < 0.8條件 LessThan */
range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::LT, 0.8)));

/*依據之前建立的條件物件range_cond，建立條件式過濾物件condrem*/
pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
condrem.setCondition (range_cond); //設定條件物件，z>0 且 z<0.8的點會被留下，其餘會濾除
condrem.setInputCloud (cloud);
condrem.setKeepOrganized(true); //濾除後點雲依然維持Orgnanized？
// apply filter
condrem.filter (*cloud_filtered);

Remove NaN from Point Cloud

// STL
#include <iostream>
#include <limits>

// PCL
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/filter.h>

int main (int, char**)
{
  typedef pcl::PointCloud<pcl::PointXYZ> CloudType;
  CloudType::Ptr cloud (new CloudType);
  cloud->is_dense = false;
  CloudType::Ptr output_cloud (new CloudType);

  CloudType::PointType p_nan;
  p_nan.x = std::numeric_limits<float>::quiet_NaN();
  p_nan.y = std::numeric_limits<float>::quiet_NaN();
  p_nan.z = std::numeric_limits<float>::quiet_NaN();
  cloud->push_back(p_nan);

  CloudType::PointType p_valid;
  p_valid.x = 1.0f;
  cloud->push_back(p_valid);

  std::cout << "size: " << cloud->points.size () << std::endl;

  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*cloud, *output_cloud, indices);
  std::cout << "size: " << output_cloud->points.size () << std::endl;

  return 0;
}