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 auto input_size =
static_cast<pcl::index_t> (input_->size ());
313 for (
pcl::index_t 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);
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 auto 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 auto 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 (
pcl::index_t idx = 0; idx < static_cast<pcl::index_t> (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]);
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_
void setNormals(const PointCloudNConstPtr &normals)
set normals if precalculated normals are available.
A point structure representing normal coordinates and the surface curvature estimate.
typename PointCloudIn::ConstPtr PointCloudInConstPtr
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...
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void setNumberOfThreads(unsigned int nr_threads)
Initialize the scheduler and set the number of threads to use.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
void setDistanceThreshold(float distance_threshold)
void detectKeypoints(PointCloudOut &output) override
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...
typename PointCloudN::ConstPtr PointCloudNConstPtr
std::uint32_t width
The point cloud width (if organized as an image-structure).
Keypoint represents the base class for key points.
void setSearchSurface(const PointCloudInConstPtr &cloud) override
Provide a pointer to the input dataset that we need to estimate features at every point for...
void setGeometricValidation(bool validate)
Filetr false positive using geometric criteria.
void setAngularThreshold(float angular_threshold)
set the angular_threshold value for detecting corners.
bool initCompute() override
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
std::uint32_t height
The point cloud height (if organized as an image-structure).
IndicesAllocator<> Indices
Type used for indices in PCL.
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
typename PointCloudN::Ptr PointCloudNPtr
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
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 reserve(std::size_t n)
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in using th...
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
void setIntensityThreshold(float intensity_threshold)
set the intensity_threshold value for detecting corners.