38 #ifndef PCL_SUSAN_IMPL_HPP_
39 #define PCL_SUSAN_IMPL_HPP_
41 #include <pcl/common/io.h>
42 #include <pcl/keypoints/susan.h>
43 #include <pcl/features/normal_3d.h>
44 #include <pcl/features/integral_image_normal.h>
47 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
54 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
57 geometric_validation_ = validate;
61 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
64 search_radius_ = radius;
68 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
71 distance_threshold_ = distance_threshold;
75 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
78 angular_threshold_ = angular_threshold;
82 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
85 intensity_threshold_ = intensity_threshold;
89 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
96 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
104 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
107 threads_ = nr_threads;
215 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
bool
220 PCL_ERROR (
"[pcl::%s::initCompute] init failed!\n", name_.c_str ());
224 if (normals_->empty ())
227 normals->reserve (normals->size ());
228 if (!surface_->isOrganized ())
233 normal_estimation.
compute (*normals);
241 normal_estimation.
compute (*normals);
245 if (normals_->size () != surface_->size ())
247 PCL_ERROR (
"[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ());
255 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
bool
257 const Eigen::Vector3f& centroid,
258 const Eigen::Vector3f& nc,
259 const PointInT& point)
const
261 Eigen::Vector3f pc = centroid - point.getVector3fMap ();
262 Eigen::Vector3f pn = nucleus - point.getVector3fMap ();
263 Eigen::Vector3f pc_cross_nc = pc.cross (nc);
264 return ((pc_cross_nc.norm () <= tolerance_) && (pc.dot (nc) >= 0) && (pn.dot (nc) <= 0));
303 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
307 response->
reserve (surface_->size ());
310 label_idx_ = pcl::getFieldIndex<PointOutT> (
"label", out_fields_);
312 const int input_size =
static_cast<int> (input_->size ());
313 for (
int 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);
323 std::vector<int> nn_indices;
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 std::uint32_t 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 std::uint32_t 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 (
int idx = 0; idx < static_cast<int> (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]);
427 std::vector<int> nn_indices;
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;
453 #define PCL_INSTANTIATE_SUSAN(T,U,N) template class PCL_EXPORTS pcl::SUSANKeypoint<T,U,N>;
454 #endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_