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 ());
253 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
bool
255 const Eigen::Vector3f& centroid,
256 const Eigen::Vector3f& nc,
257 const PointInT& point)
const
259 Eigen::Vector3f pc = centroid - point.getVector3fMap ();
260 Eigen::Vector3f pn = nucleus - point.getVector3fMap ();
261 Eigen::Vector3f pc_cross_nc = pc.cross (nc);
262 return ((pc_cross_nc.norm () <= tolerance_) && (pc.dot (nc) >= 0) && (pn.dot (nc) <= 0));
301 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT,
typename IntensityT>
void
305 response->reserve (surface_->size ());
308 label_idx_ = pcl::getFieldIndex<PointOutT> (output,
"label", out_fields_);
310 const int input_size =
static_cast<int> (input_->size ());
314 for (
int point_index = 0; point_index < input_size; ++point_index)
316 const PointInT& point_in = input_->points [point_index];
317 const NormalT& normal_in = normals_->points [point_index];
322 Eigen::Vector3f nucleus = point_in.getVector3fMap ();
323 Eigen::Vector3f nucleus_normal = normals_->points [point_index].getNormalVector3fMap ();
324 float nucleus_intensity = intensity_ (point_in);
325 std::vector<int> nn_indices;
326 std::vector<float> nn_dists;
327 tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_dists);
329 Eigen::Vector3f centroid = Eigen::Vector3f::Zero ();
331 std::vector<int> usan; usan.reserve (nn_indices.size () - 1);
332 for (std::vector<int>::const_iterator index = nn_indices.begin(); index != nn_indices.end(); ++index)
334 if ((*index != point_index) && pcl_isfinite (normals_->points[*index].normal_x))
337 if ((fabs (nucleus_intensity - intensity_ (input_->points[*index])) <= intensity_threshold_) ||
338 (1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_))
341 centroid += input_->points[*index].getVector3fMap ();
342 usan.push_back (*index);
347 float geometric_threshold = 0.5f * (
static_cast<float> (nn_indices.size () - 1));
348 if ((area > 0) && (area < geometric_threshold))
351 if (!geometric_validation_)
354 point_out.getVector3fMap () = point_in.getVector3fMap ();
356 intensity_out_.set (point_out, geometric_threshold - area);
358 if (label_idx_ != -1)
361 uint32_t label =
static_cast<uint32_t
> (point_index);
362 memcpy (reinterpret_cast<char*> (&point_out) + out_fields_[label_idx_].offset,
363 &label,
sizeof (uint32_t));
368 response->push_back (point_out);
373 Eigen::Vector3f dist = nucleus - centroid;
375 if (dist.norm () >= distance_threshold_)
378 Eigen::Vector3f nc = centroid - nucleus;
380 std::vector<int>::const_iterator usan_it = usan.begin ();
381 for (; usan_it != usan.end (); ++usan_it)
383 if (!isWithinNucleusCentroid (nucleus, centroid, nc, input_->points[*usan_it]))
387 if (usan_it == usan.end ())
390 point_out.getVector3fMap () = point_in.getVector3fMap ();
392 intensity_out_.set (point_out, geometric_threshold - area);
394 if (label_idx_ != -1)
397 uint32_t label =
static_cast<uint32_t
> (point_index);
398 memcpy (reinterpret_cast<char*> (&point_out) + out_fields_[label_idx_].offset,
399 &label,
sizeof (uint32_t));
404 response->push_back (point_out);
412 response->height = 1;
413 response->width =
static_cast<uint32_t
> (response->size ());
419 output.points.clear ();
420 output.points.reserve (response->points.size());
425 for (
int idx = 0; idx < static_cast<int> (response->points.size ()); ++idx)
427 const PointOutT& point_in = response->points [idx];
428 const NormalT& normal_in = normals_->points [idx];
430 const float intensity = intensity_out_ (response->points[idx]);
433 std::vector<int> nn_indices;
434 std::vector<float> nn_dists;
435 tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
436 bool is_minima =
true;
437 for (std::vector<int>::const_iterator nn_it = nn_indices.begin(); nn_it != nn_indices.end(); ++nn_it)
440 if (intensity > intensity_out_ (response->points[*nn_it]))
450 output.points.push_back (response->points[idx]);
454 output.width =
static_cast<uint32_t
> (output.points.size());
457 output.is_dense = input_->is_dense;
460 #define PCL_INSTANTIATE_SUSAN(T,U,N) template class PCL_EXPORTS pcl::SUSANKeypoint<T,U,N>;
461 #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.
virtual void setSearchSurface(const PointCloudInConstPtr &cloud)
Provide a pointer to the input dataset that we need to estimate features at every point for...
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 <setInputCloud (), setIndices ()> 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.