278 std::vector<ModelCoefficients>& model_coefficients,
279 std::vector<PointIndices>& inlier_indices,
281 std::vector<pcl::PointIndices>& label_indices,
282 std::vector<pcl::PointIndices>& boundary_indices)
285 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
286 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
287 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
288 refine (model_coefficients, inlier_indices, labels, label_indices);
289 regions.resize (model_coefficients.size ());
290 boundary_indices.resize (model_coefficients.size ());
292 for (std::size_t i = 0; i < model_coefficients.size (); i++)
294 boundary_cloud.
resize (0);
295 int max_inlier_idx =
static_cast<int> (inlier_indices[i].indices.size ()) - 1;
297 boundary_cloud.
resize (boundary_indices[i].indices.size ());
298 for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++)
299 boundary_cloud[j] = (*input_)[boundary_indices[i].indices[j]];
301 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
302 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
303 model_coefficients[i].values[1],
304 model_coefficients[i].values[2],
305 model_coefficients[i].values[3]);
307 Eigen::Vector3f vp (0.0, 0.0, 0.0);
308 if (project_points_ && !boundary_cloud.
empty ())
309 boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp);
313 static_cast<unsigned int> (inlier_indices[i].indices.size ()),
322 std::vector<PointIndices>& inlier_indices,
324 std::vector<pcl::PointIndices>& label_indices)
327 std::vector<bool> grow_labels;
328 std::vector<int> label_to_model;
329 grow_labels.resize (label_indices.size (),
false);
330 label_to_model.resize (label_indices.size (), 0);
332 for (std::size_t i = 0; i < model_coefficients.size (); i++)
334 int model_label = (*labels)[inlier_indices[i].indices[0]].label;
335 label_to_model[model_label] =
static_cast<int> (i);
336 grow_labels[model_label] =
true;
340 refinement_compare_->setInputCloud (input_);
341 refinement_compare_->setLabels (labels);
342 refinement_compare_->setModelCoefficients (model_coefficients);
343 refinement_compare_->setRefineLabels (grow_labels);
344 refinement_compare_->setLabelToModel (label_to_model);
347 unsigned int current_row = 0;
348 unsigned int next_row = labels->width;
349 for (std::size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = next_row, next_row += labels->width)
352 for (
unsigned colIdx = 0; colIdx < labels->width - 1; ++colIdx)
354 int current_label = (*labels)[current_row+colIdx].label;
355 int right_label = (*labels)[current_row+colIdx+1].label;
356 if (current_label < 0 || right_label < 0)
361 if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx+1))
364 (*labels)[current_row+colIdx+1].label = current_label;
365 label_indices[current_label].indices.push_back (current_row+colIdx+1);
366 inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx+1);
369 int lower_label = (*labels)[next_row+colIdx].label;
374 if (refinement_compare_->compare (current_row+colIdx, next_row+colIdx))
376 (*labels)[next_row+colIdx].label = current_label;
377 label_indices[current_label].indices.push_back (next_row+colIdx);
378 inlier_indices[label_to_model[current_label]].indices.push_back (next_row+colIdx);
385 current_row = labels->width * (labels->height - 1);
386 unsigned int prev_row = current_row - labels->width;
387 for (std::size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = prev_row, prev_row -= labels->width)
389 for (
int colIdx = labels->width - 1; colIdx >= 0; --colIdx)
391 int current_label = (*labels)[current_row+colIdx].label;
392 int left_label = (*labels)[current_row+colIdx-1].label;
393 if (current_label < 0 || left_label < 0)
397 if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx-1))
399 (*labels)[current_row+colIdx-1].label = current_label;
400 label_indices[current_label].indices.push_back (current_row+colIdx-1);
401 inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx-1);
404 int upper_label = (*labels)[prev_row+colIdx].label;
408 if (refinement_compare_->compare (current_row+colIdx, prev_row+colIdx))
410 (*labels)[prev_row+colIdx].label = current_label;
411 label_indices[current_label].indices.push_back (prev_row+colIdx);
412 inlier_indices[label_to_model[current_label]].indices.push_back (prev_row+colIdx);