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_->points[*index].normal_x))
334 if ((std::abs (nucleus_intensity - intensity_ (input_->points[*index])) <= intensity_threshold_) ||
335 (1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_))
338 centroid += input_->points[*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_->points[*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->
points.size());
418 for (
int idx = 0; idx < static_cast<int> (response->
points.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->
points[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->
points[*nn_it]))
441 output.points.push_back (response->
points[idx]);
442 keypoints_indices_->indices.push_back (idx);
447 output.width =
static_cast<std::uint32_t> (output.points.size());
448 output.is_dense =
true;
452 #define PCL_INSTANTIATE_SUSAN(T,U,N) template class PCL_EXPORTS pcl::SUSANKeypoint<T,U,N>;
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Surface normal estimation on organized data using integral images.
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
Keypoint represents the base class for key points.
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
void reserve(std::size_t n)
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void setIntensityThreshold(float intensity_threshold)
set the intensity_threshold value for detecting corners.
void setGeometricValidation(bool validate)
Filetr false positive using geometric criteria.
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
void setRadius(float radius)
set the radius for normal estimation and non maxima supression.
void setNonMaxSupression(bool nonmax)
Apply non maxima suppression to the responses to keep strongest corners.
typename PointCloudN::Ptr PointCloudNPtr
typename PointCloudIn::ConstPtr PointCloudInConstPtr
void detectKeypoints(PointCloudOut &output) override
void setDistanceThreshold(float distance_threshold)
void setSearchSurface(const PointCloudInConstPtr &cloud) override
bool initCompute() override
typename PointCloudN::ConstPtr PointCloudNConstPtr
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
void setNumberOfThreads(unsigned int nr_threads)
Initialize the scheduler and set the number of threads to use.
void setAngularThreshold(float angular_threshold)
set the angular_threshold value for detecting corners.
void setNormals(const PointCloudNConstPtr &normals)
set normals if precalculated normals are available.
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...
A point structure representing normal coordinates and the surface curvature estimate.