Point Cloud Library (PCL)  1.9.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/keypoints/susan.h>
42 #include <pcl/features/normal_3d.h>
43 #include <pcl/features/integral_image_normal.h>
44 
45 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
46 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
48 {
49  nonmax_ = nonmax;
50 }
51 
52 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
53 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
55 {
56  geometric_validation_ = validate;
57 }
58 
59 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
60 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
62 {
63  search_radius_ = radius;
64 }
65 
66 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
67 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
69 {
70  distance_threshold_ = distance_threshold;
71 }
72 
73 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
74 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
76 {
77  angular_threshold_ = angular_threshold;
78 }
79 
80 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
81 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
83 {
84  intensity_threshold_ = intensity_threshold;
85 }
86 
87 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
88 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
90 {
91  normals_ = normals;
92 }
93 
94 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
95 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
97 {
98  surface_ = cloud;
99  normals_.reset (new pcl::PointCloud<NormalT>);
100 }
101 
102 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
103 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
105 {
106  threads_ = nr_threads;
107 }
108 
109 
110 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
111 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
112 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::USAN (const PointInT& nucleus,
113 // const NormalT& nucleus_normal,
114 // const std::vector<int>& neighbors,
115 // const float& t,
116 // float& response,
117 // Eigen::Vector3f& centroid) const
118 // {
119 // float area = 0;
120 // response = 0;
121 // float x0 = nucleus.x;
122 // float y0 = nucleus.y;
123 // float z0 = nucleus.z;
124 // //xx xy xz yy yz zz
125 // std::vector<float> coefficients(6);
126 // memset (&coefficients[0], 0, sizeof (float) * 6);
127 // for (std::vector<int>::const_iterator index = neighbors.begin(); index != neighbors.end(); ++index)
128 // {
129 // if (std::isfinite (normals_->points[*index].normal_x))
130 // {
131 // Eigen::Vector3f diff = normals_->points[*index].getNormal3fMap () - nucleus_normal.getNormal3fMap ();
132 // float c = diff.norm () / t;
133 // c = -1 * pow (c, 6.0);
134 // c = exp (c);
135 // Eigen::Vector3f xyz = surface_->points[*index].getVector3fMap ();
136 // centroid += c * xyz;
137 // area += c;
138 // coefficients[0] += c * (x0 - xyz.x) * (x0 - xyz.x);
139 // coefficients[1] += c * (x0 - xyz.x) * (y0 - xyz.y);
140 // coefficients[2] += c * (x0 - xyz.x) * (z0 - xyz.z);
141 // coefficients[3] += c * (y0 - xyz.y) * (y0 - xyz.y);
142 // coefficients[4] += c * (y0 - xyz.y) * (z0 - xyz.z);
143 // coefficients[5] += c * (z0 - xyz.z) * (z0 - xyz.z);
144 // }
145 // }
146 
147 // if (area > 0)
148 // {
149 // centroid /= area;
150 // if (area < geometric_threshold)
151 // response = geometric_threshold - area;
152 // // Look for edge direction
153 // // X direction
154 // if ((coefficients[3]/coefficients[0]) < 1 && (coefficients[5]/coefficients[0]) < 1)
155 // direction = Eigen::Vector3f (1, 0, 0);
156 // else
157 // {
158 // // Y direction
159 // if ((coefficients[0]/coefficients[3]) < 1 && (coefficients[5]/coefficients[3]) < 1)
160 // direction = Eigen::Vector3f (0, 1, 0);
161 // else
162 // {
163 // // Z direction
164 // if ((coefficients[0]/coefficients[5]) < 1 && (coefficients[3]/coefficients[5]) < 1)
165 // direction = Eigen::Vector3f (0, 1, 0);
166 // // Diagonal edge
167 // else
168 // {
169 // //XY direction
170 // if ((coefficients[2]/coeffcients[1]) < 1 && (coeffcients[4]/coeffcients[1]) < 1)
171 // {
172 // if (coeffcients[1] > 0)
173 // direction = Eigen::Vector3f (1,1,0);
174 // else
175 // direction = Eigen::Vector3f (-1,1,0);
176 // }
177 // else
178 // {
179 // //XZ direction
180 // if ((coefficients[1]/coeffcients[2]) > 1 && (coeffcients[4]/coeffcients[2]) < 1)
181 // {
182 // if (coeffcients[2] > 0)
183 // direction = Eigen::Vector3f (1,0,1);
184 // else
185 // direction = Eigen::Vector3f (-1,0,1);
186 // }
187 // //YZ direction
188 // else
189 // {
190 // if (coeffcients[4] > 0)
191 // direction = Eigen::Vector3f (0,1,1);
192 // else
193 // direction = Eigen::Vector3f (0,-1,1);
194 // }
195 // }
196 // }
197 // }
198 // }
199 
200 // // std::size_t max_index = std::distance (coefficients.begin (), max);
201 // // switch (max_index)
202 // // {
203 // // case 0 : direction = Eigen::Vector3f (1, 0, 0); break;
204 // // case 1 : direction = Eigen::Vector3f (1, 1, 0); break;
205 // // case 2 : direction = Eigen::Vector3f (1, 0, 1); break;
206 // // case 3 : direction = Eigen::Vector3f (0, 1, 0); break;
207 // // case 4 : direction = Eigen::Vector3f (0, 1, 1); break;
208 // // case 5 : direction = Eigen::Vector3f (0, 0, 1); break;
209 // // }
210 // }
211 // }
212 
213 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
214 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
216 {
218  {
219  PCL_ERROR ("[pcl::%s::initCompute] init failed!\n", name_.c_str ());
220  return (false);
221  }
222 
223  if (normals_->empty ())
224  {
225  PointCloudNPtr normals (new PointCloudN ());
226  normals->reserve (normals->size ());
227  if (!surface_->isOrganized ())
228  {
230  normal_estimation.setInputCloud (surface_);
231  normal_estimation.setRadiusSearch (search_radius_);
232  normal_estimation.compute (*normals);
233  }
234  else
235  {
238  normal_estimation.setInputCloud (surface_);
239  normal_estimation.setNormalSmoothingSize (5.0);
240  normal_estimation.compute (*normals);
241  }
242  normals_ = normals;
243  }
244  if (normals_->size () != surface_->size ())
245  {
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 ());
247  return (false);
248  }
249 
250  return (true);
251 }
252 
253 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
254 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
256  const Eigen::Vector3f& centroid,
257  const Eigen::Vector3f& nc,
258  const PointInT& point) const
259 {
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));
264 }
265 
266 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
267 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isValidQueryPoint3D (int point_index) const
268 // {
269 // return (isFinite (surface_->points [point_index]) &&
270 // isFinite (normals_->points [point_index]));
271 // }
272 
273 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
274 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isValidQueryPoint2D (int point_index) const
275 // {
276 // return (isFinite (surface_->points [point_index]));
277 // }
278 
279 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
280 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isWithinSusan2D (int nucleus, int neighbor) const
281 // {
282 // return (fabs (intensity_ (surface_->points[nucleus]) -
283 // intensity_ (surface_->points[neighbor])) <= intensity_threshold_);
284 // }
285 
286 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
287 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isWithinSusan3D (int nucleus, int neighbor) const
288 // {
289 // Eigen::Vector3f nucleus_normal = normals_->point[nucleus].getVector3fMap ();
290 // return (1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_);
291 // }
292 
293 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
294 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isWithinSusanH (int nucleus, int neighbor) const
295 // {
296 // Eigen::Vector3f nucleus_normal = normals_->point[nucleus].getVector3fMap ();
297 // return ((1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_) ||
298 // (fabs (intensity_ (surface_->points[nucleus]) - intensity_ (surface_->points[neighbor])) <= intensity_threshold_));
299 // }
300 
301 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
302 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
304 {
306  response->reserve (surface_->size ());
307 
308  // Check if the output has a "label" field
309  label_idx_ = pcl::getFieldIndex<PointOutT> (output, "label", out_fields_);
310 
311  const int input_size = static_cast<int> (input_->size ());
312 //#ifdef _OPENMP
313 //#pragma omp parallel for shared (response) num_threads(threads_)
314 //#endif
315  for (int point_index = 0; point_index < input_size; ++point_index)
316  {
317  const PointInT& point_in = input_->points [point_index];
318  const NormalT& normal_in = normals_->points [point_index];
319  if (!isFinite (point_in) || !isFinite (normal_in))
320  continue;
321  else
322  {
323  Eigen::Vector3f nucleus = point_in.getVector3fMap ();
324  Eigen::Vector3f nucleus_normal = normals_->points [point_index].getNormalVector3fMap ();
325  float nucleus_intensity = intensity_ (point_in);
326  std::vector<int> nn_indices;
327  std::vector<float> nn_dists;
328  tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_dists);
329  float area = 0;
330  Eigen::Vector3f centroid = Eigen::Vector3f::Zero ();
331  // Exclude nucleus from the usan
332  std::vector<int> usan; usan.reserve (nn_indices.size () - 1);
333  for (std::vector<int>::const_iterator index = nn_indices.begin(); index != nn_indices.end(); ++index)
334  {
335  if ((*index != point_index) && std::isfinite (normals_->points[*index].normal_x))
336  {
337  // if the point fulfill condition
338  if ((fabs (nucleus_intensity - intensity_ (input_->points[*index])) <= intensity_threshold_) ||
339  (1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_))
340  {
341  ++area;
342  centroid += input_->points[*index].getVector3fMap ();
343  usan.push_back (*index);
344  }
345  }
346  }
347 
348  float geometric_threshold = 0.5f * (static_cast<float> (nn_indices.size () - 1));
349  if ((area > 0) && (area < geometric_threshold))
350  {
351  // if no geometric validation required add the point to the response
352  if (!geometric_validation_)
353  {
354  PointOutT point_out;
355  point_out.getVector3fMap () = point_in.getVector3fMap ();
356  //point_out.intensity = geometric_threshold - area;
357  intensity_out_.set (point_out, geometric_threshold - area);
358  // if a label field is found use it to save the index
359  if (label_idx_ != -1)
360  {
361  // save the index in the cloud
362  uint32_t label = static_cast<uint32_t> (point_index);
363  memcpy (reinterpret_cast<char*> (&point_out) + out_fields_[label_idx_].offset,
364  &label, sizeof (uint32_t));
365  }
366 //#ifdef _OPENMP
367 //#pragma omp critical
368 //#endif
369  response->push_back (point_out);
370  }
371  else
372  {
373  centroid /= area;
374  Eigen::Vector3f dist = nucleus - centroid;
375  // Check the distance <= distance_threshold_
376  if (dist.norm () >= distance_threshold_)
377  {
378  // point is valid from distance point of view
379  Eigen::Vector3f nc = centroid - nucleus;
380  // Check the contiguity
381  auto usan_it = usan.cbegin ();
382  for (; usan_it != usan.cend (); ++usan_it)
383  {
384  if (!isWithinNucleusCentroid (nucleus, centroid, nc, input_->points[*usan_it]))
385  break;
386  }
387  // All points within usan lies on the segment [nucleus centroid]
388  if (usan_it == usan.end ())
389  {
390  PointOutT point_out;
391  point_out.getVector3fMap () = point_in.getVector3fMap ();
392  // point_out.intensity = geometric_threshold - area;
393  intensity_out_.set (point_out, geometric_threshold - area);
394  // if a label field is found use it to save the index
395  if (label_idx_ != -1)
396  {
397  // save the index in the cloud
398  uint32_t label = static_cast<uint32_t> (point_index);
399  memcpy (reinterpret_cast<char*> (&point_out) + out_fields_[label_idx_].offset,
400  &label, sizeof (uint32_t));
401  }
402 //#ifdef _OPENMP
403 //#pragma omp critical
404 //#endif
405  response->push_back (point_out);
406  }
407  }
408  }
409  }
410  }
411  }
412 
413  response->height = 1;
414  response->width = static_cast<uint32_t> (response->size ());
415 
416  if (!nonmax_)
417  {
418  output = *response;
419  for (size_t i = 0; i < response->size (); ++i)
420  keypoints_indices_->indices.push_back (i);
421  // we don not change the denseness
422  output.is_dense = input_->is_dense;
423  }
424  else
425  {
426  output.points.clear ();
427  output.points.reserve (response->points.size());
428 
429 //#ifdef _OPENMP
430 //#pragma omp parallel for shared (output) num_threads(threads_)
431 //#endif
432  for (int idx = 0; idx < static_cast<int> (response->points.size ()); ++idx)
433  {
434  const PointOutT& point_in = response->points [idx];
435  const NormalT& normal_in = normals_->points [idx];
436  //const float intensity = response->points[idx].intensity;
437  const float intensity = intensity_out_ (response->points[idx]);
438  if (!isFinite (point_in) || !isFinite (normal_in) || (intensity == 0))
439  continue;
440  std::vector<int> nn_indices;
441  std::vector<float> nn_dists;
442  tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
443  bool is_minima = true;
444  for (std::vector<int>::const_iterator nn_it = nn_indices.begin(); nn_it != nn_indices.end(); ++nn_it)
445  {
446 // if (intensity > response->points[*nn_it].intensity)
447  if (intensity > intensity_out_ (response->points[*nn_it]))
448  {
449  is_minima = false;
450  break;
451  }
452  }
453  if (is_minima)
454 //#ifdef _OPENMP
455 //#pragma omp critical
456 //#endif
457  {
458  output.points.push_back (response->points[idx]);
459  keypoints_indices_->indices.push_back (idx);
460  }
461  }
462 
463  output.height = 1;
464  output.width = static_cast<uint32_t> (output.points.size());
465  output.is_dense = true;
466  }
467 }
468 
469 #define PCL_INSTANTIATE_SUSAN(T,U,N) template class PCL_EXPORTS pcl::SUSANKeypoint<T,U,N>;
470 #endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_
471 
void setNormals(const PointCloudNConstPtr &normals)
set normals if precalculated normals are available.
Definition: susan.hpp:89
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.
Definition: susan.hpp:61
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:53
size_t size() const
Definition: point_cloud.h:447
void reserve(size_t n)
Definition: point_cloud.h:448
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:409
void setNumberOfThreads(unsigned int nr_threads)
Initialize the scheduler and set the number of threads to use.
Definition: susan.hpp:104
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:479
PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: keypoint.h:68
void setDistanceThreshold(float distance_threshold)
Definition: susan.hpp:68
void detectKeypoints(PointCloudOut &output) override
Abstract key point detection method.
Definition: susan.hpp:303
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:199
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
Definition: normal_3d.h:241
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:414
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:427
Keypoint represents the base class for key points.
Definition: keypoint.h:48
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:412
void setSearchSurface(const PointCloudInConstPtr &cloud) override
Definition: susan.hpp:96
void setGeometricValidation(bool validate)
Filetr false positive using geometric criteria.
Definition: susan.hpp:54
PointCloudN::Ptr PointCloudNPtr
Definition: susan.h:68
void setAngularThreshold(float angular_threshold)
set the angular_threshold value for detecting corners.
Definition: susan.hpp:75
bool initCompute() override
Definition: susan.hpp:215
Surface normal estimation on organized data using integral images.
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:255
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 setNonMaxSupression(bool nonmax)
Apply non maxima suppression to the responses to keep strongest corners.
Definition: susan.hpp:47
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:189
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: normal_3d.h:330
PointCloudN::ConstPtr PointCloudNConstPtr
Definition: susan.h:69
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
void setIntensityThreshold(float intensity_threshold)
set the intensity_threshold value for detecting corners.
Definition: susan.hpp:82