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 ());
315 for (
int point_index = 0; point_index < input_size; ++point_index)
317 const PointInT& point_in = input_->points [point_index];
318 const NormalT& normal_in = normals_->points [point_index];
322 Eigen::Vector3f nucleus = point_in.getVector3fMap ();
323 Eigen::Vector3f nucleus_normal = normals_->points [point_index].getNormalVector3fMap ();
324 float nucleus_intensity = intensity_ (point_in);
325 std::vector<int> nn_indices;
326 std::vector<float> nn_dists;
327 tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_dists);
329 Eigen::Vector3f centroid = Eigen::Vector3f::Zero ();
331 std::vector<int> usan; usan.reserve (nn_indices.size () - 1);
332 for (std::vector<int>::const_iterator index = nn_indices.begin(); index != nn_indices.end(); ++index)
334 if ((*index != point_index) && std::isfinite (normals_->points[*index].normal_x))
337 if ((std::abs (nucleus_intensity - intensity_ (input_->points[*index])) <= intensity_threshold_) ||
338 (1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_))
341 centroid += input_->points[*index].getVector3fMap ();
342 usan.push_back (*index);
347 float geometric_threshold = 0.5f * (static_cast<float> (nn_indices.size () - 1));
348 if ((area > 0) && (area < geometric_threshold))
351 if (!geometric_validation_)
354 point_out.getVector3fMap () = point_in.getVector3fMap ();
356 intensity_out_.set (point_out, geometric_threshold - area);
358 if (label_idx_ != -1)
361 std::uint32_t label = static_cast<std::uint32_t> (point_index);
362 memcpy (reinterpret_cast<char*> (&point_out) + out_fields_[label_idx_].offset,
373 Eigen::Vector3f dist = nucleus - centroid;
375 if (dist.norm () >= distance_threshold_)
378 Eigen::Vector3f nc = centroid - nucleus;
380 auto usan_it = usan.cbegin ();
381 for (; usan_it != usan.cend (); ++usan_it)
383 if (!isWithinNucleusCentroid (nucleus, centroid, nc, input_->points[*usan_it]))
387 if (usan_it == usan.end ())
390 point_out.getVector3fMap () = point_in.getVector3fMap ();
392 intensity_out_.set (point_out, geometric_threshold - area);
394 if (label_idx_ != -1)
397 std::uint32_t label = static_cast<std::uint32_t> (point_index);
398 memcpy (reinterpret_cast<char*> (&point_out) + out_fields_[label_idx_].offset,
412 response->
width = static_cast<std::uint32_t> (response->
size ());
417 for (std::size_t i = 0; i < response->
size (); ++i)
418 keypoints_indices_->indices.
push_back (i);
430 for (
int idx = 0; idx < static_cast<int> (response->
points.size ()); ++idx)
432 const PointOutT& point_in = response->
points [idx];
433 const NormalT& normal_in = normals_->points [idx];
435 const float intensity = intensity_out_ (response->
points[idx]);
438 std::vector<int> nn_indices;
439 std::vector<float> nn_dists;
440 tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
441 bool is_minima =
true;
442 for (std::vector<int>::const_iterator nn_it = nn_indices.begin(); nn_it != nn_indices.end(); ++nn_it)
445 if (intensity > intensity_out_ (response->
points[*nn_it]))
457 keypoints_indices_->indices.push_back (idx);
462 output.
width = static_cast<std::uint32_t> (output.
points.size());
467 #define PCL_INSTANTIATE_SUSAN(T,U,N) template class PCL_EXPORTS pcl::SUSANKeypoint<T,U,N>;
468 #endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_