非均勻體素采樣

SamplingSurfaceNormal,將輸入空間劃分為網(wǎng)格,直到每個網(wǎng)格中最多包含N個點(diǎn),并在每個網(wǎng)格中隨機(jī)采樣點(diǎn)。 使用每個網(wǎng)格的N個點(diǎn)計算法線。 在網(wǎng)格內(nèi)采樣的所有點(diǎ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. 半徑濾波器采樣

對整個輸入迭代一次，對于每個點(diǎn)進(jìn)行半徑R鄰域搜索，如果鄰域點(diǎn)的個數(shù)低于某一閾值，則該點(diǎn)將被視為噪聲點(diǎn)并被移除。

流程：讀入點(diǎn)云→創(chuàng)建半徑濾波器對象→設(shè)置離群點(diǎn)閾值→執(zhí)行下采樣→保存采樣結(jié)果

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();