| 90 | } |
| 91 | |
| 92 | void |
| 93 | compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output, |
| 94 | int max_iterations = 1000, double threshold = 0.05, bool negative = false) |
| 95 | { |
| 96 | // Convert data to PointCloud<T> |
| 97 | PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>); |
| 98 | fromPCLPointCloud2 (*input, *xyz); |
| 99 | |
| 100 | // Estimate |
| 101 | TicToc tt; |
| 102 | print_highlight (stderr, "Computing "); |
| 103 | |
| 104 | tt.tic (); |
| 105 | |
| 106 | // Refine the plane indices |
| 107 | using SampleConsensusModelPlanePtr = SampleConsensusModelPlane<PointXYZ>::Ptr; |
| 108 | SampleConsensusModelPlanePtr model (new SampleConsensusModelPlane<PointXYZ> (xyz)); |
| 109 | RandomSampleConsensus<PointXYZ> sac (model, threshold); |
| 110 | sac.setMaxIterations (max_iterations); |
| 111 | bool res = sac.computeModel (); |
| 112 | |
| 113 | auto inliers = sac.getInliers (); |
| 114 | Eigen::VectorXf coefficients = sac.getModelCoefficients (); |
| 115 | |
| 116 | if (!res || inliers.empty ()) |
| 117 | { |
| 118 | PCL_ERROR ("No planar model found. Relax thresholds and continue.\n"); |
| 119 | return; |
| 120 | } |
| 121 | sac.refineModel (2, 50); |
| 122 | inliers = sac.getInliers (); |
| 123 | coefficients = sac.getModelCoefficients (); |
| 124 | |
| 125 | print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms, plane has : "); print_value ("%lu", inliers.size ()); print_info (" points]\n"); |
| 126 | |
| 127 | print_info ("Model coefficients: ["); |
| 128 | print_value ("%g %g %g %g", coefficients[0], coefficients[1], coefficients[2], coefficients[3]); print_info ("]\n"); |
| 129 | |
| 130 | // Instead of returning the planar model as a set of inliers, return the outliers, but perform a cluster segmentation first |
| 131 | if (negative) |
| 132 | { |
| 133 | // Remove the plane indices from the data |
| 134 | PointIndices::Ptr everything_but_the_plane (new PointIndices); |
| 135 | std::vector<int> indices_fullset (xyz->size ()); |
| 136 | for (int p_it = 0; p_it < static_cast<int> (indices_fullset.size ()); ++p_it) |
| 137 | indices_fullset[p_it] = p_it; |
| 138 | |
| 139 | std::sort (inliers.begin (), inliers.end ()); |
| 140 | set_difference (indices_fullset.begin (), indices_fullset.end (), |
| 141 | inliers.begin (), inliers.end (), |
| 142 | inserter (everything_but_the_plane->indices, everything_but_the_plane->indices.begin ())); |
| 143 | |
| 144 | // Extract largest cluster minus the plane |
| 145 | std::vector<PointIndices> cluster_indices; |
| 146 | EuclideanClusterExtraction<PointXYZ> ec; |
| 147 | ec.setClusterTolerance (0.02); // 2cm |
| 148 | ec.setMinClusterSize (100); |
| 149 | ec.setInputCloud (xyz); |
no test coverage detected