38 #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_
39 #define PCL_HARRIS_KEYPOINT_3D_IMPL_H_
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>
47 #include <xmmintrin.h>
51 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
54 if (normals_ && input_ && (cloud != input_))
60 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
67 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
70 threshold_= threshold;
74 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
77 search_radius_ = radius;
81 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
88 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
95 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
102 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
110 __m128 vec1 = _mm_setzero_ps();
112 __m128 vec2 = _mm_setzero_ps();
120 for (
const auto &neighbor : neighbors)
122 if (std::isfinite ((*normals_)[neighbor].normal_x))
125 norm1 = _mm_load_ps (&((*normals_)[neighbor].normal_x));
128 norm2 = _mm_set1_ps ((*normals_)[neighbor].normal_x);
131 norm2 = _mm_mul_ps (norm1, norm2);
134 vec1 = _mm_add_ps (vec1, norm2);
137 norm2 = _mm_set1_ps ((*normals_)[neighbor].normal_y);
140 norm2 = _mm_mul_ps (norm1, norm2);
143 vec2 = _mm_add_ps (vec2, norm2);
145 zz += (*normals_)[neighbor].normal_z * (*normals_)[neighbor].normal_z;
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);
159 std::fill_n(coefficients, 8, 0);
161 std::fill_n(coefficients, 8, 0);
162 for (
const auto& index : neighbors)
164 if (std::isfinite ((*normals_)[index].normal_x))
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;
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;
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;
191 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
bool
196 PCL_ERROR (
"[pcl::%s::initCompute] init failed!\n", name_.c_str ());
200 if (method_ < 1 || method_ > 5)
202 PCL_ERROR (
"[pcl::%s::initCompute] method (%d) must be in [1..5]!\n", name_.c_str (), method_);
209 normals->reserve (normals->size ());
210 if (!surface_->isOrganized ())
215 normal_estimation.
compute (*normals);
223 normal_estimation.
compute (*normals);
227 if (normals_->size () != surface_->size ())
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_);
237 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
242 response->
points.reserve (input_->size());
247 responseHarris(*response);
250 responseNoble(*response);
253 responseLowe(*response);
256 responseCurvature(*response);
259 responseTomasi(*response);
268 for (std::size_t i = 0; i < response->
size (); ++i)
269 keypoints_indices_->indices.push_back (i);
274 output.reserve (response->
size());
276 #pragma omp parallel for \
278 shared(output, response) \
279 num_threads(threads_)
280 for (
int idx = 0; idx < static_cast<int> (response->
size ()); ++idx)
283 !std::isfinite ((*response)[idx].intensity) ||
284 (*response)[idx].intensity < threshold_)
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)
293 if ((*response)[idx].intensity < (*response)[index].intensity)
302 output.push_back ((*response)[idx]);
303 keypoints_indices_->indices.push_back (idx);
308 refineCorners (output);
311 output.width = output.size();
312 output.is_dense =
true;
317 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
320 PCL_ALIGN (16)
float covar [8];
321 output.resize (input_->size ());
322 #pragma omp parallel for \
325 firstprivate(covar) \
326 num_threads(threads_)
327 for (
int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
329 const PointInT& pointIn = input_->points [pIdx];
330 output [pIdx].intensity = 0.0;
334 std::vector<float> nn_dists;
335 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
336 calculateNormalCovar (nn_indices, covar);
338 float trace = covar [0] + covar [5] + covar [7];
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];
346 output [pIdx].intensity = 0.04f + det - 0.04f * trace * trace;
349 output [pIdx].x = pointIn.x;
350 output [pIdx].y = pointIn.y;
351 output [pIdx].z = pointIn.z;
353 output.height = input_->height;
354 output.width = input_->width;
358 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
361 PCL_ALIGN (16)
float covar [8];
362 output.resize (input_->size ());
363 #pragma omp parallel \
366 firstprivate(covar) \
367 num_threads(threads_)
368 for (
int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
370 const PointInT& pointIn = input_->points [pIdx];
371 output [pIdx].intensity = 0.0;
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];
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];
386 output [pIdx].intensity = det / trace;
389 output [pIdx].x = pointIn.x;
390 output [pIdx].y = pointIn.y;
391 output [pIdx].z = pointIn.z;
393 output.height = input_->height;
394 output.width = input_->width;
398 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
401 PCL_ALIGN (16)
float covar [8];
402 output.resize (input_->size ());
403 #pragma omp parallel for \
406 firstprivate(covar) \
407 num_threads(threads_)
408 for (
int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
410 const PointInT& pointIn = input_->points [pIdx];
411 output [pIdx].intensity = 0.0;
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];
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];
426 output [pIdx].intensity = det / (trace * trace);
429 output [pIdx].x = pointIn.x;
430 output [pIdx].y = pointIn.y;
431 output [pIdx].z = pointIn.z;
433 output.height = input_->height;
434 output.width = input_->width;
438 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
442 for (std::size_t idx = 0; idx < input_->size(); ++idx)
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);
451 output.height = input_->height;
452 output.width = input_->width;
456 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
459 PCL_ALIGN (16)
float covar [8];
460 Eigen::Matrix3f covariance_matrix;
461 output.resize (input_->size ());
462 #pragma omp parallel for \
465 firstprivate(covar, covariance_matrix) \
466 num_threads(threads_)
467 for (
int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
469 const PointInT& pointIn = input_->points [pIdx];
470 output [pIdx].intensity = 0.0;
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];
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];
487 EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
489 output [pIdx].intensity = eigen_values[0];
492 output [pIdx].x = pointIn.x;
493 output [pIdx].y = pointIn.y;
494 output [pIdx].z = pointIn.z;
496 output.height = input_->height;
497 output.width = input_->width;
501 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
506 Eigen::Vector3f NNTp;
507 constexpr
unsigned max_iterations = 10;
508 #pragma omp parallel for \
510 firstprivate(nnT, NNT, NNTp) \
511 num_threads(threads_)
512 for (
int cIdx = 0; cIdx < static_cast<int> (corners.size ()); ++cIdx)
514 unsigned iterations = 0;
519 corner.x = corners[cIdx].x;
520 corner.y = corners[cIdx].y;
521 corner.z = corners[cIdx].z;
523 std::vector<float> nn_dists;
524 tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists);
525 for (
const auto& index : nn_indices)
527 if (!std::isfinite ((*normals_)[index].normal_x))
530 nnT = (*normals_)[index].getNormalVector3fMap () * (*normals_)[index].getNormalVector3fMap ().transpose();
532 NNTp += nnT * (*surface_)[index].getVector3fMap ();
534 const Eigen::LDLT<Eigen::Matrix3f> ldlt(NNT);
535 if (ldlt.rcond() > 1e-4)
536 corners[cIdx].getVector3fMap () = ldlt.solve(NNTp);
538 const auto diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm ();
542 }
while (++iterations < max_iterations);
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_
void refineCorners(PointCloudOut &corners) const
void setRadius(float radius)
Set the radius for normal estimation and non maxima suppression.
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...
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void setNormals(const PointCloudNConstPtr &normals)
Set normals if precalculated normals are available.
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
void setThreshold(float threshold)
Set the threshold value for detecting corners.
void calculateNormalCovar(const pcl::Indices &neighbors, float *coefficients) const
calculates the upper triangular part of unnormalized covariance matrix over the normals given by the ...
void setInputCloud(const PointCloudInConstPtr &cloud) override
Provide a pointer to the input dataset.
Keypoint represents the base class for key points.
typename PointCloudN::Ptr PointCloudNPtr
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Surface normal estimation on organized data using integral images.
bool initCompute() override
void setRefine(bool do_refine)
Whether the detected key points should be refined or not.
IndicesAllocator<> Indices
Type used for indices in PCL.
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
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...
void responseTomasi(PointCloudOut &output) const
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
void responseHarris(PointCloudOut &output) const
gets the corner response for valid input points
void detectKeypoints(PointCloudOut &output) override
void setNonMaxSupression(bool=false)
Whether non maxima suppression should be applied or the response for each point should be returned...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
void setMethod(ResponseMethod type)
Set the method of the response to be calculated.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in using th...
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
void responseCurvature(PointCloudOut &output) const
typename PointCloudN::ConstPtr PointCloudNConstPtr
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
void responseLowe(PointCloudOut &output) const
void responseNoble(PointCloudOut &output) const