Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
harris_3d.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_HARRIS_KEYPOINT_3D_IMPL_H_
39#define PCL_HARRIS_KEYPOINT_3D_IMPL_H_
40
41#include <pcl/keypoints/harris_3d.h>
42#include <pcl/common/io.h>
43#include <pcl/features/normal_3d.h>
44#include <pcl/features/integral_image_normal.h>
45#include <pcl/common/centroid.h>
46#ifdef __SSE__
47#include <xmmintrin.h>
48#endif
49
50//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51template <typename PointInT, typename PointOutT, typename NormalT> void
53{
54 if (normals_ && input_ && (cloud != input_))
55 normals_.reset ();
56 input_ = cloud;
57}
58
59//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
60template <typename PointInT, typename PointOutT, typename NormalT> void
65
66//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
67template <typename PointInT, typename PointOutT, typename NormalT> void
69{
70 threshold_= threshold;
71}
72
73//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
74template <typename PointInT, typename PointOutT, typename NormalT> void
76{
77 search_radius_ = radius;
78}
79
80//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
81template <typename PointInT, typename PointOutT, typename NormalT> void
83{
84 refine_ = do_refine;
85}
86
87//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
88template <typename PointInT, typename PointOutT, typename NormalT> void
93
94//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
95template <typename PointInT, typename PointOutT, typename NormalT> void
100
101//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
102template <typename PointInT, typename PointOutT, typename NormalT> void
104{
105 unsigned count = 0;
106 // indices 0 1 2 3 4 5 6 7
107 // coefficients: xx xy xz ?? yx yy yz ??
108#ifdef __SSE__
109 // accumulator for xx, xy, xz
110 __m128 vec1 = _mm_setzero_ps();
111 // accumulator for yy, yz, zz
112 __m128 vec2 = _mm_setzero_ps();
113
114 __m128 norm1;
115
116 __m128 norm2;
117
118 float zz = 0;
119
120 for (const auto &neighbor : neighbors)
121 {
122 if (std::isfinite ((*normals_)[neighbor].normal_x))
123 {
124 // nx, ny, nz, h
125 norm1 = _mm_load_ps (&((*normals_)[neighbor].normal_x));
126
127 // nx, nx, nx, nx
128 norm2 = _mm_set1_ps ((*normals_)[neighbor].normal_x);
129
130 // nx * nx, nx * ny, nx * nz, nx * h
131 norm2 = _mm_mul_ps (norm1, norm2);
132
133 // accumulate
134 vec1 = _mm_add_ps (vec1, norm2);
135
136 // ny, ny, ny, ny
137 norm2 = _mm_set1_ps ((*normals_)[neighbor].normal_y);
138
139 // ny * nx, ny * ny, ny * nz, ny * h
140 norm2 = _mm_mul_ps (norm1, norm2);
141
142 // accumulate
143 vec2 = _mm_add_ps (vec2, norm2);
144
145 zz += (*normals_)[neighbor].normal_z * (*normals_)[neighbor].normal_z;
146 ++count;
147 }
148 }
149 if (count > 0)
150 {
151 norm2 = _mm_set1_ps (static_cast<float>(count));
152 vec1 = _mm_div_ps (vec1, norm2);
153 vec2 = _mm_div_ps (vec2, norm2);
154 _mm_store_ps (coefficients, vec1);
155 _mm_store_ps (coefficients + 4, vec2);
156 coefficients [7] = zz / static_cast<float>(count);
157 }
158 else
159 std::fill_n(coefficients, 8, 0);
160#else
161 std::fill_n(coefficients, 8, 0);
162 for (const auto& index : neighbors)
163 {
164 if (std::isfinite ((*normals_)[index].normal_x))
165 {
166 coefficients[0] += (*normals_)[index].normal_x * (*normals_)[index].normal_x;
167 coefficients[1] += (*normals_)[index].normal_x * (*normals_)[index].normal_y;
168 coefficients[2] += (*normals_)[index].normal_x * (*normals_)[index].normal_z;
169
170 coefficients[5] += (*normals_)[index].normal_y * (*normals_)[index].normal_y;
171 coefficients[6] += (*normals_)[index].normal_y * (*normals_)[index].normal_z;
172 coefficients[7] += (*normals_)[index].normal_z * (*normals_)[index].normal_z;
173
174 ++count;
175 }
176 }
177 if (count > 0)
178 {
179 float norm = 1.0 / float (count);
180 coefficients[0] *= norm;
181 coefficients[1] *= norm;
182 coefficients[2] *= norm;
183 coefficients[5] *= norm;
184 coefficients[6] *= norm;
185 coefficients[7] *= norm;
186 }
187#endif
188}
189
190//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
191template <typename PointInT, typename PointOutT, typename NormalT> bool
193{
195 {
196 PCL_ERROR ("[pcl::%s::initCompute] init failed!\n", name_.c_str ());
197 return (false);
198 }
199
200 if (method_ < 1 || method_ > 5)
201 {
202 PCL_ERROR ("[pcl::%s::initCompute] method (%d) must be in [1..5]!\n", name_.c_str (), method_);
203 return (false);
204 }
205
206 if (!normals_)
207 {
208 PointCloudNPtr normals (new PointCloudN ());
209 normals->reserve (normals->size ());
210 if (!surface_->isOrganized ())
211 {
213 normal_estimation.setInputCloud (surface_);
214 normal_estimation.setRadiusSearch (search_radius_);
215 normal_estimation.compute (*normals);
216 }
217 else
218 {
221 normal_estimation.setInputCloud (surface_);
222 normal_estimation.setNormalSmoothingSize (5.0);
223 normal_estimation.compute (*normals);
224 }
225 normals_ = normals;
226 }
227 if (normals_->size () != surface_->size ())
228 {
229 PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str (), method_);
230 return (false);
231 }
232
233 return (true);
234}
235
236//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
237template <typename PointInT, typename PointOutT, typename NormalT> void
239{
241
242 response->points.reserve (input_->size());
243
244 switch (method_)
245 {
246 case HARRIS:
247 responseHarris(*response);
248 break;
249 case NOBLE:
250 responseNoble(*response);
251 break;
252 case LOWE:
253 responseLowe(*response);
254 break;
255 case CURVATURE:
256 responseCurvature(*response);
257 break;
258 case TOMASI:
259 responseTomasi(*response);
260 break;
261 }
262
263 if (!nonmax_)
264 {
265 output = *response;
266 // we do not change the denseness in this case
267 output.is_dense = input_->is_dense;
268 for (std::size_t i = 0; i < response->size (); ++i)
269 keypoints_indices_->indices.push_back (i);
270 }
271 else
272 {
273 output.clear ();
274 output.reserve (response->size());
275
276#pragma omp parallel for \
277 default(none) \
278 shared(output, response) \
279 num_threads(threads_)
280 for (int idx = 0; idx < static_cast<int> (response->size ()); ++idx)
281 {
282 if (!isFinite ((*response)[idx]) ||
283 !std::isfinite ((*response)[idx].intensity) ||
284 (*response)[idx].intensity < threshold_)
285 continue;
286
287 pcl::Indices nn_indices;
288 std::vector<float> nn_dists;
289 tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
290 bool is_maxima = true;
291 for (const auto& index : nn_indices)
292 {
293 if ((*response)[idx].intensity < (*response)[index].intensity)
294 {
295 is_maxima = false;
296 break;
297 }
298 }
299 if (is_maxima)
300#pragma omp critical
301 {
302 output.push_back ((*response)[idx]);
303 keypoints_indices_->indices.push_back (idx);
304 }
305 }
306
307 if (refine_)
308 refineCorners (output);
309
310 output.height = 1;
311 output.width = output.size();
312 output.is_dense = true;
313 }
314}
315
316//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
317template <typename PointInT, typename PointOutT, typename NormalT> void
319{
320 PCL_ALIGN (16) float covar [8];
321 output.resize (input_->size ());
322#pragma omp parallel for \
323 default(none) \
324 shared(output) \
325 firstprivate(covar) \
326 num_threads(threads_)
327 for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
328 {
329 const PointInT& pointIn = input_->points [pIdx];
330 output [pIdx].intensity = 0.0; //std::numeric_limits<float>::quiet_NaN ();
331 if (isFinite (pointIn))
332 {
333 pcl::Indices nn_indices;
334 std::vector<float> nn_dists;
335 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
336 calculateNormalCovar (nn_indices, covar);
337
338 float trace = covar [0] + covar [5] + covar [7];
339 if (trace != 0)
340 {
341 float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
342 - covar [2] * covar [2] * covar [5]
343 - covar [1] * covar [1] * covar [7]
344 - covar [6] * covar [6] * covar [0];
345
346 output [pIdx].intensity = 0.04f + det - 0.04f * trace * trace;
347 }
348 }
349 output [pIdx].x = pointIn.x;
350 output [pIdx].y = pointIn.y;
351 output [pIdx].z = pointIn.z;
352 }
353 output.height = input_->height;
354 output.width = input_->width;
355}
356
357//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
358template <typename PointInT, typename PointOutT, typename NormalT> void
360{
361 PCL_ALIGN (16) float covar [8];
362 output.resize (input_->size ());
363#pragma omp parallel \
364 for default(none) \
365 shared(output) \
366 firstprivate(covar) \
367 num_threads(threads_)
368 for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
369 {
370 const PointInT& pointIn = input_->points [pIdx];
371 output [pIdx].intensity = 0.0;
372 if (isFinite (pointIn))
373 {
374 pcl::Indices nn_indices;
375 std::vector<float> nn_dists;
376 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
377 calculateNormalCovar (nn_indices, covar);
378 float trace = covar [0] + covar [5] + covar [7];
379 if (trace != 0)
380 {
381 float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
382 - covar [2] * covar [2] * covar [5]
383 - covar [1] * covar [1] * covar [7]
384 - covar [6] * covar [6] * covar [0];
385
386 output [pIdx].intensity = det / trace;
387 }
388 }
389 output [pIdx].x = pointIn.x;
390 output [pIdx].y = pointIn.y;
391 output [pIdx].z = pointIn.z;
392 }
393 output.height = input_->height;
394 output.width = input_->width;
395}
396
397//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
398template <typename PointInT, typename PointOutT, typename NormalT> void
400{
401 PCL_ALIGN (16) float covar [8];
402 output.resize (input_->size ());
403#pragma omp parallel for \
404 default(none) \
405 shared(output) \
406 firstprivate(covar) \
407 num_threads(threads_)
408 for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
409 {
410 const PointInT& pointIn = input_->points [pIdx];
411 output [pIdx].intensity = 0.0;
412 if (isFinite (pointIn))
413 {
414 pcl::Indices nn_indices;
415 std::vector<float> nn_dists;
416 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
417 calculateNormalCovar (nn_indices, covar);
418 float trace = covar [0] + covar [5] + covar [7];
419 if (trace != 0)
420 {
421 float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
422 - covar [2] * covar [2] * covar [5]
423 - covar [1] * covar [1] * covar [7]
424 - covar [6] * covar [6] * covar [0];
425
426 output [pIdx].intensity = det / (trace * trace);
427 }
428 }
429 output [pIdx].x = pointIn.x;
430 output [pIdx].y = pointIn.y;
431 output [pIdx].z = pointIn.z;
432 }
433 output.height = input_->height;
434 output.width = input_->width;
435}
436
437//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
438template <typename PointInT, typename PointOutT, typename NormalT> void
440{
441 PointOutT point;
442 for (std::size_t idx = 0; idx < input_->size(); ++idx)
443 {
444 point.x = (*input_)[idx].x;
445 point.y = (*input_)[idx].y;
446 point.z = (*input_)[idx].z;
447 point.intensity = (*normals_)[idx].curvature;
448 output.push_back(point);
449 }
450 // does not change the order
451 output.height = input_->height;
452 output.width = input_->width;
453}
454
455//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
456template <typename PointInT, typename PointOutT, typename NormalT> void
458{
459 PCL_ALIGN (16) float covar [8];
460 Eigen::Matrix3f covariance_matrix;
461 output.resize (input_->size ());
462#pragma omp parallel for \
463 default(none) \
464 shared(output) \
465 firstprivate(covar, covariance_matrix) \
466 num_threads(threads_)
467 for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
468 {
469 const PointInT& pointIn = input_->points [pIdx];
470 output [pIdx].intensity = 0.0;
471 if (isFinite (pointIn))
472 {
473 pcl::Indices nn_indices;
474 std::vector<float> nn_dists;
475 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
476 calculateNormalCovar (nn_indices, covar);
477 float trace = covar [0] + covar [5] + covar [7];
478 if (trace != 0)
479 {
480 covariance_matrix.coeffRef (0) = covar [0];
481 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = covar [1];
482 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = covar [2];
483 covariance_matrix.coeffRef (4) = covar [5];
484 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = covar [6];
485 covariance_matrix.coeffRef (8) = covar [7];
486
487 EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
488 pcl::eigen33(covariance_matrix, eigen_values);
489 output [pIdx].intensity = eigen_values[0];
490 }
491 }
492 output [pIdx].x = pointIn.x;
493 output [pIdx].y = pointIn.y;
494 output [pIdx].z = pointIn.z;
495 }
496 output.height = input_->height;
497 output.width = input_->width;
498}
499
500//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
501template <typename PointInT, typename PointOutT, typename NormalT> void
503{
504 Eigen::Matrix3f nnT;
505 Eigen::Matrix3f NNT;
506 Eigen::Vector3f NNTp;
507 constexpr unsigned max_iterations = 10;
508#pragma omp parallel for \
509 shared(corners) \
510 firstprivate(nnT, NNT, NNTp) \
511 num_threads(threads_)
512 for (int cIdx = 0; cIdx < static_cast<int> (corners.size ()); ++cIdx)
513 {
514 unsigned iterations = 0;
515 do {
516 NNT.setZero();
517 NNTp.setZero();
518 PointInT corner;
519 corner.x = corners[cIdx].x;
520 corner.y = corners[cIdx].y;
521 corner.z = corners[cIdx].z;
522 pcl::Indices nn_indices;
523 std::vector<float> nn_dists;
524 tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists);
525 for (const auto& index : nn_indices)
526 {
527 if (!std::isfinite ((*normals_)[index].normal_x))
528 continue;
529
530 nnT = (*normals_)[index].getNormalVector3fMap () * (*normals_)[index].getNormalVector3fMap ().transpose();
531 NNT += nnT;
532 NNTp += nnT * (*surface_)[index].getVector3fMap ();
533 }
534 const Eigen::LDLT<Eigen::Matrix3f> ldlt(NNT);
535 if (ldlt.rcond() > 1e-4)
536 corners[cIdx].getVector3fMap () = ldlt.solve(NNTp);
537
538 const auto diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm ();
539 if (diff <= 1e-6) {
540 break;
541 }
542 } while (++iterations < max_iterations);
543 }
544}
545
546#define PCL_INSTANTIATE_HarrisKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint3D<T,U,N>;
547#endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_
Define methods for centroid estimation and covariance matrix calculus.
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
typename PointCloudN::Ptr PointCloudNPtr
Definition harris_3d.h:63
void responseTomasi(PointCloudOut &output) const
void detectKeypoints(PointCloudOut &output) override
typename PointCloudN::ConstPtr PointCloudNConstPtr
Definition harris_3d.h:64
void responseNoble(PointCloudOut &output) const
bool initCompute() override
void responseHarris(PointCloudOut &output) const
gets the corner response for valid input points
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition harris_3d.h:58
void setNormals(const PointCloudNConstPtr &normals)
Set normals if precalculated normals are available.
Definition harris_3d.hpp:96
void responseCurvature(PointCloudOut &output) const
void refineCorners(PointCloudOut &corners) const
void setRadius(float radius)
Set the radius for normal estimation and non maxima suppression.
Definition harris_3d.hpp:75
void setNonMaxSupression(bool=false)
Whether non maxima suppression should be applied or the response for each point should be returned.
Definition harris_3d.hpp:89
void setInputCloud(const PointCloudInConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition harris_3d.hpp:52
void setMethod(ResponseMethod type)
Set the method of the response to be calculated.
Definition harris_3d.hpp:61
void setThreshold(float threshold)
Set the threshold value for detecting corners.
Definition harris_3d.hpp:68
void responseLowe(PointCloudOut &output) const
void calculateNormalCovar(const pcl::Indices &neighbors, float *coefficients) const
calculates the upper triangular part of unnormalized covariance matrix over the normals given by the ...
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition harris_3d.h:60
void setRefine(bool do_refine)
Whether the detected key points should be refined or not.
Definition harris_3d.hpp:82
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
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
std::size_t size() const
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition eigen.hpp:295
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
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133