38 #ifndef PCL_SUSAN_IMPL_HPP_
39 #define PCL_SUSAN_IMPL_HPP_
41 #include <pcl/keypoints/susan.h>
42 #include <pcl/features/normal_3d.h>
43 #include <pcl/features/integral_image_normal.h>
46 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
53 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
56 geometric_validation_ = validate;
60 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
63 search_radius_ = radius;
67 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
70 distance_threshold_ = distance_threshold;
74 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
77 angular_threshold_ = angular_threshold;
81 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
84 intensity_threshold_ = intensity_threshold;
88 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
95 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
103 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
106 threads_ = nr_threads;
214 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
bool
219 PCL_ERROR (
"[pcl::%s::initCompute] init failed!\n", name_.c_str ());
223 if (normals_->empty ())
226 normals->reserve (normals->size ());
227 if (!surface_->isOrganized ())
232 normal_estimation.
compute (*normals);
240 normal_estimation.
compute (*normals);
244 if (normals_->size () != surface_->size ())
246 PCL_ERROR (
"[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ());
254 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
bool
256 const Eigen::Vector3f& centroid,
257 const Eigen::Vector3f& nc,
258 const PointInT& point)
const
260 Eigen::Vector3f pc = centroid - point.getVector3fMap ();
261 Eigen::Vector3f pn = nucleus - point.getVector3fMap ();
262 Eigen::Vector3f pc_cross_nc = pc.cross (nc);
263 return ((pc_cross_nc.norm () <= tolerance_) && (pc.dot (nc) >= 0) && (pn.dot (nc) <= 0));
302 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
306 response->
reserve (surface_->size ());
309 label_idx_ = pcl::getFieldIndex<PointOutT> (
"label", out_fields_);
311 const int input_size =
static_cast<int> (input_->size ());
312 for (
int point_index = 0; point_index < input_size; ++point_index)
314 const PointInT& point_in = input_->points [point_index];
315 const NormalT& normal_in = normals_->points [point_index];
319 Eigen::Vector3f nucleus = point_in.getVector3fMap ();
320 Eigen::Vector3f nucleus_normal = normals_->points [point_index].getNormalVector3fMap ();
321 float nucleus_intensity = intensity_ (point_in);
322 std::vector<int> nn_indices;
323 std::vector<float> nn_dists;
324 tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_dists);
326 Eigen::Vector3f centroid = Eigen::Vector3f::Zero ();
328 std::vector<int> usan; usan.reserve (nn_indices.size () - 1);
329 for (std::vector<int>::const_iterator index = nn_indices.begin(); index != nn_indices.end(); ++index)
331 if ((*index != point_index) && std::isfinite ((*normals_)[*index].normal_x))
334 if ((std::abs (nucleus_intensity - intensity_ ((*input_)[*index])) <= intensity_threshold_) ||
335 (1 - nucleus_normal.dot ((*normals_)[*index].getNormalVector3fMap ()) <= angular_threshold_))
338 centroid += (*input_)[*index].getVector3fMap ();
339 usan.push_back (*index);
344 float geometric_threshold = 0.5f * (
static_cast<float> (nn_indices.size () - 1));
345 if ((area > 0) && (area < geometric_threshold))
348 if (!geometric_validation_)
351 point_out.getVector3fMap () = point_in.getVector3fMap ();
353 intensity_out_.set (point_out, geometric_threshold - area);
355 if (label_idx_ != -1)
359 memcpy (
reinterpret_cast<char*
> (&point_out) + out_fields_[label_idx_].offset,
367 Eigen::Vector3f dist = nucleus - centroid;
369 if (dist.norm () >= distance_threshold_)
372 Eigen::Vector3f nc = centroid - nucleus;
374 auto usan_it = usan.cbegin ();
375 for (; usan_it != usan.cend (); ++usan_it)
377 if (!isWithinNucleusCentroid (nucleus, centroid, nc, (*input_)[*usan_it]))
381 if (usan_it == usan.end ())
384 point_out.getVector3fMap () = point_in.getVector3fMap ();
386 intensity_out_.set (point_out, geometric_threshold - area);
388 if (label_idx_ != -1)
392 memcpy (
reinterpret_cast<char*
> (&point_out) + out_fields_[label_idx_].offset,
408 for (std::size_t i = 0; i < response->
size (); ++i)
409 keypoints_indices_->indices.
push_back (i);
411 output.is_dense = input_->is_dense;
415 output.points.clear ();
416 output.points.reserve (response->
size());
418 for (
int idx = 0; idx < static_cast<int> (response->
size ()); ++idx)
420 const PointOutT& point_in = response->
points [idx];
421 const NormalT& normal_in = normals_->points [idx];
423 const float intensity = intensity_out_ ((*response)[idx]);
426 std::vector<int> nn_indices;
427 std::vector<float> nn_dists;
428 tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
429 bool is_minima =
true;
430 for (std::vector<int>::const_iterator nn_it = nn_indices.begin(); nn_it != nn_indices.end(); ++nn_it)
433 if (intensity > intensity_out_ ((*response)[*nn_it]))
441 output.points.push_back ((*response)[idx]);
442 keypoints_indices_->indices.push_back (idx);
447 output.width = output.size();
448 output.is_dense =
true;
452 #define PCL_INSTANTIATE_SUSAN(T,U,N) template class PCL_EXPORTS pcl::SUSANKeypoint<T,U,N>;
453 #endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_