Point Cloud Library (PCL) 1.12.0
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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
47template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
49{
50 nonmax_ = nonmax;
51}
52
53//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
54template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
56{
57 geometric_validation_ = validate;
58}
59
60//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
61template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
63{
64 search_radius_ = radius;
65}
66
67//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
68template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
70{
71 distance_threshold_ = distance_threshold;
72}
73
74//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
75template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
77{
78 angular_threshold_ = angular_threshold;
79}
80
81//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
82template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
84{
85 intensity_threshold_ = intensity_threshold;
86}
87
88//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
89template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
91{
92 normals_ = normals;
93}
94
95//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
96template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
98{
99 surface_ = cloud;
100 normals_.reset (new pcl::PointCloud<NormalT>);
101}
102
103//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
104template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
215template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
255template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
303template <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 std::uint32_t 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 std::uint32_t 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:201
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:332
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:652
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.