| 82 | } |
| 83 | |
| 84 | void |
| 85 | compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output, |
| 86 | double radius) |
| 87 | { |
| 88 | // Convert data to PointCloud<T> |
| 89 | PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>); |
| 90 | fromPCLPointCloud2 (*input, *xyz); |
| 91 | |
| 92 | // Estimate |
| 93 | TicToc tt; |
| 94 | tt.tic (); |
| 95 | |
| 96 | print_highlight (stderr, "Computing "); |
| 97 | |
| 98 | UniformSampling<PointXYZ> us(true); // extract removed indices |
| 99 | us.setInputCloud(xyz); |
| 100 | us.setRadiusSearch(radius); |
| 101 | PointCloud<PointXYZ> temp; |
| 102 | us.filter(temp); |
| 103 | |
| 104 | pcl::PointIndices removed_indices; |
| 105 | // These are "fake" indices generated by initCompute(), already sorted |
| 106 | auto input_indices = us.getIndices(); |
| 107 | // Removed indices, not guaranteed to be sorted |
| 108 | us.getRemovedIndices(removed_indices); |
| 109 | std::sort(removed_indices.indices.begin(), removed_indices.indices.end()); |
| 110 | // Compute retained indices as a set difference between all and removed |
| 111 | pcl::Indices retained; |
| 112 | std::set_difference(input_indices->begin(), |
| 113 | input_indices->end(), |
| 114 | removed_indices.indices.begin(), |
| 115 | removed_indices.indices.end(), |
| 116 | std::inserter(retained, retained.begin())); |
| 117 | pcl::copyPointCloud(*input, retained, output); |
| 118 | |
| 119 | print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); |
| 120 | print_value ("%d", retained.size()); print_info (" points]\n"); |
| 121 | } |
| 122 | |
| 123 | void |
| 124 | saveCloud (const std::string &filename, const pcl::PCLPointCloud2 &output) |
no test coverage detected