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> (output,
"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];
323 Eigen::Vector3f nucleus = point_in.getVector3fMap ();
324 Eigen::Vector3f nucleus_normal = normals_->points [point_index].getNormalVector3fMap ();
325 float nucleus_intensity = intensity_ (point_in);
326 std::vector<int> nn_indices;
327 std::vector<float> nn_dists;
328 tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_dists);
330 Eigen::Vector3f centroid = Eigen::Vector3f::Zero ();
332 std::vector<int> usan; usan.reserve (nn_indices.size () - 1);
333 for (std::vector<int>::const_iterator index = nn_indices.begin(); index != nn_indices.end(); ++index)
335 if ((*index != point_index) && pcl_isfinite (normals_->points[*index].normal_x))
338 if ((fabs (nucleus_intensity - intensity_ (input_->points[*index])) <= intensity_threshold_) ||
339 (1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_))
342 centroid += input_->points[*index].getVector3fMap ();
343 usan.push_back (*index);
348 float geometric_threshold = 0.5f * (
static_cast<float> (nn_indices.size () - 1));
349 if ((area > 0) && (area < geometric_threshold))
352 if (!geometric_validation_)
355 point_out.getVector3fMap () = point_in.getVector3fMap ();
357 intensity_out_.set (point_out, geometric_threshold - area);
359 if (label_idx_ != -1)
362 uint32_t label =
static_cast<uint32_t
> (point_index);
363 memcpy (
reinterpret_cast<char*
> (&point_out) + out_fields_[label_idx_].offset,
364 &label,
sizeof (uint32_t));
369 response->push_back (point_out);
374 Eigen::Vector3f dist = nucleus - centroid;
376 if (dist.norm () >= distance_threshold_)
379 Eigen::Vector3f nc = centroid - nucleus;
381 std::vector<int>::const_iterator usan_it = usan.begin ();
382 for (; usan_it != usan.end (); ++usan_it)
384 if (!isWithinNucleusCentroid (nucleus, centroid, nc, input_->points[*usan_it]))
388 if (usan_it == usan.end ())
391 point_out.getVector3fMap () = point_in.getVector3fMap ();
393 intensity_out_.set (point_out, geometric_threshold - area);
395 if (label_idx_ != -1)
398 uint32_t label =
static_cast<uint32_t
> (point_index);
399 memcpy (
reinterpret_cast<char*
> (&point_out) + out_fields_[label_idx_].offset,
400 &label,
sizeof (uint32_t));
405 response->push_back (point_out);
413 response->height = 1;
414 response->width =
static_cast<uint32_t
> (response->size ());
419 for (
size_t i = 0; i < response->size (); ++i)
420 keypoints_indices_->indices.
push_back (i);
427 output.
points.reserve (response->points.size());
432 for (
int idx = 0; idx < static_cast<int> (response->points.size ()); ++idx)
434 const PointOutT& point_in = response->points [idx];
435 const NormalT& normal_in = normals_->points [idx];
437 const float intensity = intensity_out_ (response->points[idx]);
440 std::vector<int> nn_indices;
441 std::vector<float> nn_dists;
442 tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
443 bool is_minima =
true;
444 for (std::vector<int>::const_iterator nn_it = nn_indices.begin(); nn_it != nn_indices.end(); ++nn_it)
447 if (intensity > intensity_out_ (response->points[*nn_it]))
458 output.
points.push_back (response->points[idx]);
459 keypoints_indices_->indices.push_back (idx);
464 output.
width =
static_cast<uint32_t
> (output.
points.size());
469 #define PCL_INSTANTIATE_SUSAN(T,U,N) template class PCL_EXPORTS pcl::SUSANKeypoint<T,U,N>;
470 #endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_