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);
422 output.is_dense = input_->is_dense;
426 output.points.clear ();
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());
465 output.is_dense =
true;
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_
void setNormals(const PointCloudNConstPtr &normals)
set normals if precalculated normals are available.
A point structure representing normal coordinates and the surface curvature estimate.
void setRadius(float radius)
set the radius for normal estimation and non maxima supression.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
virtual void setSearchSurface(const PointCloudInConstPtr &cloud)
void setNumberOfThreads(unsigned int nr_threads)
Initialize the scheduler and set the number of threads to use.
PointCloudIn::ConstPtr PointCloudInConstPtr
void setDistanceThreshold(float distance_threshold)
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
Keypoint represents the base class for key points.
void setGeometricValidation(bool validate)
Filetr false positive using geometric criteria.
PointCloudN::Ptr PointCloudNPtr
void setAngularThreshold(float angular_threshold)
set the angular_threshold value for detecting corners.
Surface normal estimation on organized data using integral images.
bool isWithinNucleusCentroid(const Eigen::Vector3f &nucleus, const Eigen::Vector3f ¢roid, const Eigen::Vector3f &nc, const PointInT &point) const
return true if a point lies within the line between the nucleus and the centroid
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void detectKeypoints(PointCloudOut &output)
Abstract key point detection method.
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
void setNonMaxSupression(bool nonmax)
Apply non maxima suppression to the responses to keep strongest corners.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in using th...
PointCloudN::ConstPtr PointCloudNConstPtr
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
void setIntensityThreshold(float intensity_threshold)
set the intensity_threshold value for detecting corners.