PCL中非均匀体素采样介绍

非均匀体素采样

SamplingSurfaceNormal,将输入空间划分为网格,直到每个网格中最多包含N个点,并在每个网格中随机采样点。 使用每个网格的N个点计算法线。 在网格内采样的所有点都分配有相同的法线。

PointCloud < PointNormal >::Ptr incloud (new PointCloud < PointNormal > ());
PointCloud < PointNormal > outcloud;


//Creating a point cloud on the XY plane
for (float i = 0.0f; i < 5.0f; i += 0.1f)
{
  for (float j = 0.0f; j < 5.0f; j += 0.1f)
  {
    PointNormal pt;
    pt.x = i;
    pt.y = j;
    pt.z = 1;
    incloud- >points.push_back (pt);
  }
}
incloud- >width = 1;
incloud- >height = uint32_t (incloud- >points.size ());


pcl::SamplingSurfaceNormal < pcl::PointNormal > ssn_filter;
ssn_filter.setInputCloud (incloud);
ssn_filter.setRatio (0.3f);
ssn_filter.filter (outcloud);


// All the sampled points should have normals along the direction of Z axis
for (unsigned int i = 0; i < outcloud.points.size (); i++)
{
  EXPECT_NEAR (outcloud.points[i].normal[0], 0, 1e-3);
  EXPECT_NEAR (outcloud.points[i].normal[1], 0, 1e-3);
  EXPECT_NEAR (outcloud.points[i].normal[2], 1, 1e-3);
}

6. 半径滤波器采样

对整个输入迭代一次，对于每个点进行半径R邻域搜索，如果邻域点的个数低于某一阈值，则该点将被视为噪声点并被移除。

流程：读入点云→创建半径滤波器对象→设置离群点阈值→执行下采样→保存采样结果

pcl::PointCloud< pcl::PointXYZ >::Ptr pcl_cloud_ptr(pcl_cloud);


boost::shared_ptr< pcl::PointCloud< pcl::PointXYZ >> pcl_vg_cloud(new pcl::PointCloud< pcl::PointXYZ >());
pcl::PointCloud< pcl::PointXYZ >::Ptr pcl_vg_cloud_ptr(pcl_vg_cloud);


boost::shared_ptr< pcl::PointCloud< pcl::PointXYZ >> pcl_ror_cloud(new pcl::PointCloud< pcl::PointXYZ >());
pcl::PointCloud< pcl::PointXYZ >::Ptr pcl_ror_cloud_ptr(pcl_ror_cloud);


//Use VoxelGrid to make points sparse
pcl::VoxelGrid< pcl::PointXYZ > sor;
sor.setInputCloud (pcl_cloud_ptr);
sor.setLeafSize (0.08, 0.1, 0.1); 
sor.filter (*pcl_vg_cloud_ptr);


//Use RadiusOutlierRemoval to remove the point whitch is far away to others
pcl::RadiusOutlierRemoval< pcl::PointXYZ > outrem;
outrem.setInputCloud(pcl_vg_cloud_ptr);
outrem.setRadiusSearch(0.5);
outrem.setMinNeighborsInRadius (3);
outrem.filter (*pcl_ror_cloud_ptr); 


//transfrom and publish
sensor_msgs::PointCloud2Ptr msg_pointcloud(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*pcl_ror_cloud, *msg_pointcloud);
msg_pointcloud- >header.frame_id = optical_frame_id_[RS_STREAM_DEPTH];;  
msg_pointcloud- >header.stamp = ros::Time::now();