PointCloud<PointXYZI>::Ptr PointCloudFunctions::downSampleCloud(pcl::PointCloud<PointXYZI>::Ptr inputCloud, float leafSize, bool save, string fileNameToSave)
{
PointCloud<PointXYZI>::Ptr downsampled(new PointCloud<PointXYZI> ());
VoxelGrid<PointXYZI> sor;
sor.setInputCloud (inputCloud);
sor.setFilterLimits(0, 2000);
sor.setLeafSize (leafSize, leafSize, leafSize);
sor.filter (*downsampled);
if (save)
{
savePCDFileASCII (fileNameToSave, *downsampled);
}
return downsampled;
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
float leaf_x, float leaf_y, float leaf_z, const std::string &field, double fmin, double fmax)
{
TicToc tt;
tt.tic ();
print_highlight ("Computing ");
VoxelGrid<sensor_msgs::PointCloud2> grid;
grid.setInputCloud (input);
grid.setFilterFieldName (field);
grid.setFilterLimits (fmin, fmax);
grid.setLeafSize (leaf_x, leaf_y, leaf_z);
grid.filter (output);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
}
