| 328 | } |
| 329 | |
| 330 | void |
| 331 | OrganizedSegmentationDemo::cloud_cb(const CloudConstPtr& cloud) |
| 332 | { |
| 333 | if (!capture_) |
| 334 | return; |
| 335 | QMutexLocker locker(&mtx_); |
| 336 | FPS_CALC("computation"); |
| 337 | |
| 338 | // Estimate Normals |
| 339 | pcl::PointCloud<pcl::Normal>::Ptr normal_cloud(new pcl::PointCloud<pcl::Normal>); |
| 340 | ne.setInputCloud(cloud); |
| 341 | ne.compute(*normal_cloud); |
| 342 | float* distance_map = ne.getDistanceMap(); |
| 343 | pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal>::Ptr eapc = |
| 344 | pcl::dynamic_pointer_cast<pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal>>( |
| 345 | edge_aware_comparator_); |
| 346 | eapc->setDistanceMap(distance_map); |
| 347 | eapc->setDistanceThreshold(0.01f, false); |
| 348 | |
| 349 | // Segment Planes |
| 350 | double mps_start = pcl::getTime(); |
| 351 | std::vector<pcl::PlanarRegion<PointT>, |
| 352 | Eigen::aligned_allocator<pcl::PlanarRegion<PointT>>> |
| 353 | regions; |
| 354 | std::vector<pcl::ModelCoefficients> model_coefficients; |
| 355 | std::vector<pcl::PointIndices> inlier_indices; |
| 356 | pcl::PointCloud<pcl::Label>::Ptr labels(new pcl::PointCloud<pcl::Label>); |
| 357 | std::vector<pcl::PointIndices> label_indices; |
| 358 | std::vector<pcl::PointIndices> boundary_indices; |
| 359 | mps.setInputNormals(normal_cloud); |
| 360 | mps.setInputCloud(cloud); |
| 361 | if (use_planar_refinement_) { |
| 362 | mps.segmentAndRefine(regions, |
| 363 | model_coefficients, |
| 364 | inlier_indices, |
| 365 | labels, |
| 366 | label_indices, |
| 367 | boundary_indices); |
| 368 | } |
| 369 | else { |
| 370 | mps.segment(regions); |
| 371 | } |
| 372 | double mps_end = pcl::getTime(); |
| 373 | std::cout << "MPS+Refine took: " << double(mps_end - mps_start) << std::endl; |
| 374 | |
| 375 | // Segment Objects |
| 376 | pcl::PointCloud<PointT>::CloudVectorType clusters; |
| 377 | |
| 378 | if (use_clustering_ && !regions.empty()) { |
| 379 | pcl::EuclideanClusterComparator<PointT, pcl::Label>::ExcludeLabelSetPtr |
| 380 | plane_labels( |
| 381 | new pcl::EuclideanClusterComparator<PointT, pcl::Label>::ExcludeLabelSet); |
| 382 | for (std::size_t i = 0; i < label_indices.size(); ++i) |
| 383 | if (label_indices[i].indices.size() > mps.getMinInliers()) |
| 384 | plane_labels->insert(i); |
| 385 | |
| 386 | euclidean_cluster_comparator_->setInputCloud(cloud); |
| 387 | euclidean_cluster_comparator_->setLabels(labels); |
nothing calls this directly
no test coverage detected