Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
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
73
74//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
75template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
80
81//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
82template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
87
88//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
89template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
94
95//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
96template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
102
103//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
104template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
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 ());
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);
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 {
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 {
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;
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
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
std::size_t size() const
Size of the range the iterator is going through.
Surface normal estimation on organized data using integral images.
Keypoint represents the base class for key points.
Definition keypoint.h:49
shared_ptr< PointCloud< PointT > > Ptr
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
A point structure representing normal coordinates and the surface curvature estimate.