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"); }