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_)[*index].normal_x))
334 if ((std::abs (nucleus_intensity - intensity_ ((*input_)[*index])) <= intensity_threshold_) ||
335 (1 - nucleus_normal.dot ((*normals_)[*index].getNormalVector3fMap ()) <= angular_threshold_))
338 centroid += (*input_)[*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_)[*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->
size());
418 for (
int idx = 0; idx < static_cast<int> (response->
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)[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)[*nn_it]))
441 output.points.push_back ((*response)[idx]);
442 keypoints_indices_->indices.push_back (idx);
447 output.width = output.size();
448 output.is_dense =
true;
452 #define PCL_INSTANTIATE_SUSAN(T,U,N) template class PCL_EXPORTS pcl::SUSANKeypoint<T,U,N>; 453 #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.
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.
std::uint32_t height
The point cloud height (if organized as an image-structure).
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 <setInputCloud (), setIndices ()> using th...
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
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 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.