//////////////////////////////////////////////////////////////////// \brief * Given a plane, and the set of inlier indices representing it, * segment out the object of intererest supported by it. * * \param[in] picked_idx the index of a point on the object * \param[in] cloud the full point cloud dataset * \param[in] plane_indices a set of indices representing the plane supporting
| 149 | * \param[out] object the segmented resultant object |
| 150 | */ |
| 151 | void |
| 152 | segmentObject(pcl::index_t picked_idx, |
| 153 | const typename PointCloud<PointT>::ConstPtr& cloud, |
| 154 | const PointIndices::Ptr& plane_indices, |
| 155 | PointCloud<PointT>& object) |
| 156 | { |
| 157 | typename PointCloud<PointT>::Ptr plane_hull(new PointCloud<PointT>); |
| 158 | |
| 159 | // Compute the convex hull of the plane |
| 160 | ConvexHull<PointT> chull; |
| 161 | chull.setDimension(2); |
| 162 | chull.setInputCloud(cloud); |
| 163 | chull.setIndices(plane_indices); |
| 164 | chull.reconstruct(*plane_hull); |
| 165 | |
| 166 | // Remove the plane indices from the data |
| 167 | typename PointCloud<PointT>::Ptr plane(new PointCloud<PointT>); |
| 168 | ExtractIndices<PointT> extract(true); |
| 169 | extract.setInputCloud(cloud); |
| 170 | extract.setIndices(plane_indices); |
| 171 | extract.setNegative(false); |
| 172 | extract.filter(*plane); |
| 173 | PointIndices::Ptr indices_but_the_plane(new PointIndices); |
| 174 | extract.getRemovedIndices(*indices_but_the_plane); |
| 175 | |
| 176 | // Extract all clusters above the hull |
| 177 | PointIndices::Ptr points_above_plane(new PointIndices); |
| 178 | ExtractPolygonalPrismData<PointT> exppd; |
| 179 | exppd.setInputCloud(cloud); |
| 180 | exppd.setIndices(indices_but_the_plane); |
| 181 | exppd.setInputPlanarHull(plane_hull); |
| 182 | exppd.setViewPoint( |
| 183 | (*cloud)[picked_idx].x, (*cloud)[picked_idx].y, (*cloud)[picked_idx].z); |
| 184 | exppd.setHeightLimits(0.001, 0.5); // up to half a meter |
| 185 | exppd.segment(*points_above_plane); |
| 186 | |
| 187 | std::vector<PointIndices> euclidean_label_indices; |
| 188 | // Prefer a faster method if the cloud is organized, over EuclidanClusterExtraction |
| 189 | if (cloud_->isOrganized()) { |
| 190 | // Use an organized clustering segmentation to extract the individual clusters |
| 191 | typename EuclideanClusterComparator<PointT, Label>::Ptr |
| 192 | euclidean_cluster_comparator(new EuclideanClusterComparator<PointT, Label>); |
| 193 | euclidean_cluster_comparator->setInputCloud(cloud); |
| 194 | euclidean_cluster_comparator->setDistanceThreshold(0.03f, false); |
| 195 | // Set the entire scene to false, and the inliers of the objects located on top of |
| 196 | // the plane to true |
| 197 | Label l; |
| 198 | l.label = 0; |
| 199 | PointCloud<Label>::Ptr scene( |
| 200 | new PointCloud<Label>(cloud->width, cloud->height, l)); |
| 201 | // Mask the objects that we want to split into clusters |
| 202 | for (const auto& index : points_above_plane->indices) |
| 203 | (*scene)[index].label = 1; |
| 204 | euclidean_cluster_comparator->setLabels(scene); |
| 205 | |
| 206 | typename EuclideanClusterComparator<PointT, Label>::ExcludeLabelSetPtr |
| 207 | exclude_labels( |
| 208 | new typename EuclideanClusterComparator<PointT, Label>::ExcludeLabelSet); |
nothing calls this directly
no test coverage detected