Point Cloud Library (PCL)  1.14.1-dev
susan.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_SUSAN_IMPL_HPP_
39 #define PCL_SUSAN_IMPL_HPP_
40 
41 #include <pcl/common/io.h> // for getFieldIndex
42 #include <pcl/keypoints/susan.h>
43 #include <pcl/features/normal_3d.h>
44 #include <pcl/features/integral_image_normal.h>
45 
46 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
49 {
50  nonmax_ = nonmax;
51 }
52 
53 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
54 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
56 {
57  geometric_validation_ = validate;
58 }
59 
60 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
61 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
63 {
64  search_radius_ = radius;
65 }
66 
67 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
68 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
70 {
71  distance_threshold_ = distance_threshold;
72 }
73 
74 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
75 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
77 {
78  angular_threshold_ = angular_threshold;
79 }
80 
81 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
82 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
84 {
85  intensity_threshold_ = intensity_threshold;
86 }
87 
88 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
89 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
91 {
92  normals_ = normals;
93 }
94 
95 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
96 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
98 {
99  surface_ = cloud;
100  normals_.reset (new pcl::PointCloud<NormalT>);
101 }
102 
103 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
104 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
106 {
107  threads_ = nr_threads;
108 }
109 
110 
111 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
112 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
113 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::USAN (const PointInT& nucleus,
114 // const NormalT& nucleus_normal,
115 // const pcl::Indices& neighbors,
116 // const float& t,
117 // float& response,
118 // Eigen::Vector3f& centroid) const
119 // {
120 // float area = 0;
121 // response = 0;
122 // float x0 = nucleus.x;
123 // float y0 = nucleus.y;
124 // float z0 = nucleus.z;
125 // //xx xy xz yy yz zz
126 // std::vector<float> coefficients(6);
127 // memset (&coefficients[0], 0, sizeof (float) * 6);
128 // for (const auto& index : neighbors)
129 // {
130 // if (std::isfinite ((*normals_)[index].normal_x))
131 // {
132 // Eigen::Vector3f diff = (*normals_)[index].getNormal3fMap () - nucleus_normal.getNormal3fMap ();
133 // float c = diff.norm () / t;
134 // c = -1 * pow (c, 6.0);
135 // c = std::exp (c);
136 // Eigen::Vector3f xyz = (*surface_)[index].getVector3fMap ();
137 // centroid += c * xyz;
138 // area += c;
139 // coefficients[0] += c * (x0 - xyz.x) * (x0 - xyz.x);
140 // coefficients[1] += c * (x0 - xyz.x) * (y0 - xyz.y);
141 // coefficients[2] += c * (x0 - xyz.x) * (z0 - xyz.z);
142 // coefficients[3] += c * (y0 - xyz.y) * (y0 - xyz.y);
143 // coefficients[4] += c * (y0 - xyz.y) * (z0 - xyz.z);
144 // coefficients[5] += c * (z0 - xyz.z) * (z0 - xyz.z);
145 // }
146 // }
147 
148 // if (area > 0)
149 // {
150 // centroid /= area;
151 // if (area < geometric_threshold)
152 // response = geometric_threshold - area;
153 // // Look for edge direction
154 // // X direction
155 // if ((coefficients[3]/coefficients[0]) < 1 && (coefficients[5]/coefficients[0]) < 1)
156 // direction = Eigen::Vector3f (1, 0, 0);
157 // else
158 // {
159 // // Y direction
160 // if ((coefficients[0]/coefficients[3]) < 1 && (coefficients[5]/coefficients[3]) < 1)
161 // direction = Eigen::Vector3f (0, 1, 0);
162 // else
163 // {
164 // // Z direction
165 // if ((coefficients[0]/coefficients[5]) < 1 && (coefficients[3]/coefficients[5]) < 1)
166 // direction = Eigen::Vector3f (0, 1, 0);
167 // // Diagonal edge
168 // else
169 // {
170 // //XY direction
171 // if ((coefficients[2]/coeffcients[1]) < 1 && (coeffcients[4]/coeffcients[1]) < 1)
172 // {
173 // if (coeffcients[1] > 0)
174 // direction = Eigen::Vector3f (1,1,0);
175 // else
176 // direction = Eigen::Vector3f (-1,1,0);
177 // }
178 // else
179 // {
180 // //XZ direction
181 // if ((coefficients[1]/coeffcients[2]) > 1 && (coeffcients[4]/coeffcients[2]) < 1)
182 // {
183 // if (coeffcients[2] > 0)
184 // direction = Eigen::Vector3f (1,0,1);
185 // else
186 // direction = Eigen::Vector3f (-1,0,1);
187 // }
188 // //YZ direction
189 // else
190 // {
191 // if (coeffcients[4] > 0)
192 // direction = Eigen::Vector3f (0,1,1);
193 // else
194 // direction = Eigen::Vector3f (0,-1,1);
195 // }
196 // }
197 // }
198 // }
199 // }
200 
201 // // std::size_t max_index = std::distance (coefficients.begin (), max);
202 // // switch (max_index)
203 // // {
204 // // case 0 : direction = Eigen::Vector3f (1, 0, 0); break;
205 // // case 1 : direction = Eigen::Vector3f (1, 1, 0); break;
206 // // case 2 : direction = Eigen::Vector3f (1, 0, 1); break;
207 // // case 3 : direction = Eigen::Vector3f (0, 1, 0); break;
208 // // case 4 : direction = Eigen::Vector3f (0, 1, 1); break;
209 // // case 5 : direction = Eigen::Vector3f (0, 0, 1); break;
210 // // }
211 // }
212 // }
213 
214 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
215 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
217 {
219  {
220  PCL_ERROR ("[pcl::%s::initCompute] init failed!\n", name_.c_str ());
221  return (false);
222  }
223 
224  if (normals_->empty ())
225  {
226  PointCloudNPtr normals (new PointCloudN ());
227  normals->reserve (normals->size ());
228  if (!surface_->isOrganized ())
229  {
231  normal_estimation.setInputCloud (surface_);
232  normal_estimation.setRadiusSearch (search_radius_);
233  normal_estimation.compute (*normals);
234  }
235  else
236  {
239  normal_estimation.setInputCloud (surface_);
240  normal_estimation.setNormalSmoothingSize (5.0);
241  normal_estimation.compute (*normals);
242  }
243  normals_ = normals;
244  }
245  if (normals_->size () != surface_->size ())
246  {
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 ());
248  return (false);
249  }
250 
251  return (true);
252 }
253 
254 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
255 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
257  const Eigen::Vector3f& centroid,
258  const Eigen::Vector3f& nc,
259  const PointInT& point) const
260 {
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));
265 }
266 
267 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
268 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isValidQueryPoint3D (int point_index) const
269 // {
270 // return (isFinite (surface_->points [point_index]) &&
271 // isFinite (normals_->points [point_index]));
272 // }
273 
274 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
275 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isValidQueryPoint2D (int point_index) const
276 // {
277 // return (isFinite (surface_->points [point_index]));
278 // }
279 
280 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
281 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isWithinSusan2D (int nucleus, int neighbor) const
282 // {
283 // return (std::abs (intensity_ ((*surface_)[nucleus]) -
284 // intensity_ ((*surface_)[neighbor])) <= intensity_threshold_);
285 // }
286 
287 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
288 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isWithinSusan3D (int nucleus, int neighbor) const
289 // {
290 // Eigen::Vector3f nucleus_normal = normals_->point[nucleus].getVector3fMap ();
291 // return (1 - nucleus_normal.dot ((*normals_)[*index].getNormalVector3fMap ()) <= angular_threshold_);
292 // }
293 
294 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
295 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isWithinSusanH (int nucleus, int neighbor) const
296 // {
297 // Eigen::Vector3f nucleus_normal = normals_->point[nucleus].getVector3fMap ();
298 // return ((1 - nucleus_normal.dot ((*normals_)[*index].getNormalVector3fMap ()) <= angular_threshold_) ||
299 // (std::abs (intensity_ ((*surface_)[nucleus]) - intensity_ ((*surface_)[neighbor])) <= intensity_threshold_));
300 // }
301 
302 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
303 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
305 {
307  response->reserve (surface_->size ());
308 
309  // Check if the output has a "label" field
310  label_idx_ = pcl::getFieldIndex<PointOutT> ("label", out_fields_);
311 
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)
314  {
315  const PointInT& point_in = input_->points [point_index];
316  const NormalT& normal_in = normals_->points [point_index];
317  if (!isFinite (point_in) || !isFinite (normal_in))
318  continue;
319 
320  Eigen::Vector3f nucleus = point_in.getVector3fMap ();
321  Eigen::Vector3f nucleus_normal = normals_->points [point_index].getNormalVector3fMap ();
322  float nucleus_intensity = intensity_ (point_in);
323  pcl::Indices nn_indices;
324  std::vector<float> nn_dists;
325  tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_dists);
326  float area = 0;
327  Eigen::Vector3f centroid = Eigen::Vector3f::Zero ();
328  // Exclude nucleus from the usan
329  std::vector<int> usan; usan.reserve (nn_indices.size () - 1);
330  for (const auto& index : nn_indices)
331  {
332  if ((index != point_index) && std::isfinite ((*normals_)[index].normal_x))
333  {
334  // if the point fulfill condition
335  if ((std::abs (nucleus_intensity - intensity_ ((*input_)[index])) <= intensity_threshold_) ||
336  (1 - nucleus_normal.dot ((*normals_)[index].getNormalVector3fMap ()) <= angular_threshold_))
337  {
338  ++area;
339  centroid += (*input_)[index].getVector3fMap ();
340  usan.push_back (index);
341  }
342  }
343  }
344 
345  float geometric_threshold = 0.5f * (static_cast<float> (nn_indices.size () - 1));
346  if ((area > 0) && (area < geometric_threshold))
347  {
348  // if no geometric validation required add the point to the response
349  if (!geometric_validation_)
350  {
351  PointOutT point_out;
352  point_out.getVector3fMap () = point_in.getVector3fMap ();
353  //point_out.intensity = geometric_threshold - area;
354  intensity_out_.set (point_out, geometric_threshold - area);
355  // if a label field is found use it to save the index
356  if (label_idx_ != -1)
357  {
358  // save the index in the cloud
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));
362  }
363  response->push_back (point_out);
364  }
365  else
366  {
367  centroid /= area;
368  Eigen::Vector3f dist = nucleus - centroid;
369  // Check the distance <= distance_threshold_
370  if (dist.norm () >= distance_threshold_)
371  {
372  // point is valid from distance point of view
373  Eigen::Vector3f nc = centroid - nucleus;
374  // Check the contiguity
375  auto usan_it = usan.cbegin ();
376  for (; usan_it != usan.cend (); ++usan_it)
377  {
378  if (!isWithinNucleusCentroid (nucleus, centroid, nc, (*input_)[*usan_it]))
379  break;
380  }
381  // All points within usan lies on the segment [nucleus centroid]
382  if (usan_it == usan.end ())
383  {
384  PointOutT point_out;
385  point_out.getVector3fMap () = point_in.getVector3fMap ();
386  // point_out.intensity = geometric_threshold - area;
387  intensity_out_.set (point_out, geometric_threshold - area);
388  // if a label field is found use it to save the index
389  if (label_idx_ != -1)
390  {
391  // save the index in the cloud
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));
395  }
396  response->push_back (point_out);
397  }
398  }
399  }
400  }
401  }
402 
403  response->height = 1;
404  response->width = response->size ();
405 
406  if (!nonmax_)
407  {
408  output = *response;
409  for (std::size_t i = 0; i < response->size (); ++i)
410  keypoints_indices_->indices.push_back (i);
411  // we don not change the denseness
412  output.is_dense = input_->is_dense;
413  }
414  else
415  {
416  output.clear ();
417  output.reserve (response->size());
418 
419  for (pcl::index_t idx = 0; idx < static_cast<pcl::index_t> (response->size ()); ++idx)
420  {
421  const PointOutT& point_in = response->points [idx];
422  const NormalT& normal_in = normals_->points [idx];
423  //const float intensity = (*response)[idx].intensity;
424  const float intensity = intensity_out_ ((*response)[idx]);
425  if (!isFinite (point_in) || !isFinite (normal_in) || (intensity == 0))
426  continue;
427  pcl::Indices nn_indices;
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)
432  {
433 // if (intensity > (*response)[nn_index].intensity)
434  if (intensity > intensity_out_ ((*response)[nn_index]))
435  {
436  is_minima = false;
437  break;
438  }
439  }
440  if (is_minima)
441  {
442  output.push_back ((*response)[idx]);
443  keypoints_indices_->indices.push_back (idx);
444  }
445  }
446 
447  output.height = 1;
448  output.width = output.size();
449  output.is_dense = true;
450  }
451 }
452 
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_
455 
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
Definition: feature.h:198
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:194
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.
Definition: keypoint.h:49
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
Definition: normal_3d.h:244
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: normal_3d.h:328
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:663
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
std::size_t size() const
Definition: point_cloud.h:443
void reserve(std::size_t n)
Definition: point_cloud.h:445
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:395
void setIntensityThreshold(float intensity_threshold)
set the intensity_threshold value for detecting corners.
Definition: susan.hpp:83
void setGeometricValidation(bool validate)
Filetr false positive using geometric criteria.
Definition: susan.hpp:55
bool isWithinNucleusCentroid(const Eigen::Vector3f &nucleus, const Eigen::Vector3f &centroid, const Eigen::Vector3f &nc, const PointInT &point) const
return true if a point lies within the line between the nucleus and the centroid
Definition: susan.hpp:256
void setRadius(float radius)
set the radius for normal estimation and non maxima supression.
Definition: susan.hpp:62
void setNonMaxSupression(bool nonmax)
Apply non maxima suppression to the responses to keep strongest corners.
Definition: susan.hpp:48
typename PointCloudN::Ptr PointCloudNPtr
Definition: susan.h:68
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: susan.h:65
void detectKeypoints(PointCloudOut &output) override
Definition: susan.hpp:304
void setDistanceThreshold(float distance_threshold)
Definition: susan.hpp:69
void setSearchSurface(const PointCloudInConstPtr &cloud) override
Definition: susan.hpp:97
bool initCompute() override
Definition: susan.hpp:216
typename PointCloudN::ConstPtr PointCloudNConstPtr
Definition: susan.h:69
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: susan.h:63
void setNumberOfThreads(unsigned int nr_threads)
Initialize the scheduler and set the number of threads to use.
Definition: susan.hpp:105
void setAngularThreshold(float angular_threshold)
set the angular_threshold value for detecting corners.
Definition: susan.hpp:76
void setNormals(const PointCloudNConstPtr &normals)
set normals if precalculated normals are available.
Definition: susan.hpp:90
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...
Definition: point_tests.h:55
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing normal coordinates and the surface curvature estimate.