Point Cloud Library (PCL) 1.12.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
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
111 // accumulator for yy, yz, zz
113
115
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
132
133 // accumulate
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
141
142 // accumulate
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 (float(count));
154 _mm_store_ps (coefficients, vec1);
155 _mm_store_ps (coefficients + 4, vec2);
156 coefficients [7] = zz / float(count);
157 }
158 else
159 memset (coefficients, 0, sizeof (float) * 8);
160#else
161 memset (coefficients, 0, sizeof (float) * 8);
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
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
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 {
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 {
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 {
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 {
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::Matrix3f NNTInv;
507 Eigen::Vector3f NNTp;
508 const unsigned max_iterations = 10;
509#pragma omp parallel for \
510 default(none) \
511 shared(corners) \
512 firstprivate(nnT, NNT, NNTInv, NNTp) \
513 num_threads(threads_)
514 for (int cIdx = 0; cIdx < static_cast<int> (corners.size ()); ++cIdx)
515 {
516 unsigned iterations = 0;
517 do {
518 NNT.setZero();
519 NNTp.setZero();
521 corner.x = corners[cIdx].x;
522 corner.y = corners[cIdx].y;
523 corner.z = corners[cIdx].z;
525 std::vector<float> nn_dists;
526 tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists);
527 for (const auto& index : nn_indices)
528 {
529 if (!std::isfinite ((*normals_)[index].normal_x))
530 continue;
531
532 nnT = (*normals_)[index].getNormalVector3fMap () * (*normals_)[index].getNormalVector3fMap ().transpose();
533 NNT += nnT;
534 NNTp += nnT * (*surface_)[index].getVector3fMap ();
535 }
536 if (invert3x3SymMatrix (NNT, NNTInv) != 0)
538
539 const auto diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm ();
540 if (diff <= 1e-6) {
541 break;
542 }
543 } while (++iterations < max_iterations);
544 }
545}
546
547#define PCL_INSTANTIATE_HarrisKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint3D<T,U,N>;
548#endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_
Define methods for centroid estimation and covariance matrix calculus.
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
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 supression.
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.
Keypoint represents the base class for key points.
Definition keypoint.h:49
shared_ptr< PointCloud< PointT > > Ptr
Matrix::Scalar invert3x3SymMatrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 3x3 symmetric matrix.
Definition eigen.hpp:424
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:296
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