307 response->
reserve (surface_->size ());
310 label_idx_ = pcl::getFieldIndex<PointOutT> (
"label", out_fields_);
312 const auto input_size =
static_cast<pcl::index_t> (input_->size ());
313 for (
pcl::index_t point_index = 0; point_index < input_size; ++point_index)
315 const PointInT& point_in = input_->points [point_index];
316 const NormalT& normal_in = normals_->points [point_index];
320 Eigen::Vector3f nucleus = point_in.getVector3fMap ();
321 Eigen::Vector3f nucleus_normal = normals_->points [point_index].getNormalVector3fMap ();
322 float nucleus_intensity = intensity_ (point_in);
324 std::vector<float> nn_dists;
325 tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_dists);
327 Eigen::Vector3f centroid = Eigen::Vector3f::Zero ();
329 std::vector<int> usan; usan.reserve (nn_indices.size () - 1);
330 for (
const auto& index : nn_indices)
332 if ((index != point_index) && std::isfinite ((*normals_)[index].normal_x))
335 if ((std::abs (nucleus_intensity - intensity_ ((*input_)[index])) <= intensity_threshold_) ||
336 (1 - nucleus_normal.dot ((*normals_)[index].getNormalVector3fMap ()) <= angular_threshold_))
339 centroid += (*input_)[index].getVector3fMap ();
340 usan.push_back (index);
345 float geometric_threshold = 0.5f * (
static_cast<float> (nn_indices.size () - 1));
346 if ((area > 0) && (area < geometric_threshold))
349 if (!geometric_validation_)
352 point_out.getVector3fMap () = point_in.getVector3fMap ();
354 intensity_out_.set (point_out, geometric_threshold - area);
356 if (label_idx_ != -1)
359 auto label =
static_cast<std::uint32_t
> (point_index);
360 memcpy (
reinterpret_cast<char*
> (&point_out) + out_fields_[label_idx_].offset,
361 &label,
sizeof (std::uint32_t));
368 Eigen::Vector3f dist = nucleus - centroid;
370 if (dist.norm () >= distance_threshold_)
373 Eigen::Vector3f nc = centroid - nucleus;
375 auto usan_it = usan.cbegin ();
376 for (; usan_it != usan.cend (); ++usan_it)
378 if (!isWithinNucleusCentroid (nucleus, centroid, nc, (*input_)[*usan_it]))
382 if (usan_it == usan.end ())
385 point_out.getVector3fMap () = point_in.getVector3fMap ();
387 intensity_out_.set (point_out, geometric_threshold - area);
389 if (label_idx_ != -1)
392 auto label =
static_cast<std::uint32_t
> (point_index);
393 memcpy (
reinterpret_cast<char*
> (&point_out) + out_fields_[label_idx_].offset,
394 &label,
sizeof (std::uint32_t));
409 for (std::size_t i = 0; i < response->
size (); ++i)
410 keypoints_indices_->indices.
push_back (i);
412 output.is_dense = input_->is_dense;
417 output.reserve (response->
size());
419 for (
pcl::index_t idx = 0; idx < static_cast<pcl::index_t> (response->
size ()); ++idx)
421 const PointOutT& point_in = response->
points [idx];
422 const NormalT& normal_in = normals_->points [idx];
424 const float intensity = intensity_out_ ((*response)[idx]);
428 std::vector<float> nn_dists;
429 tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
430 bool is_minima =
true;
431 for (
const auto& nn_index : nn_indices)
434 if (intensity > intensity_out_ ((*response)[nn_index]))
442 output.push_back ((*response)[idx]);
443 keypoints_indices_->indices.push_back (idx);
448 output.width = output.size();
449 output.is_dense =
true;