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/filters/passthrough.h> 44 #include <pcl/filters/extract_indices.h> 45 #include <pcl/features/normal_3d.h> 46 #include <pcl/features/integral_image_normal.h> 50 #include <xmmintrin.h> 54 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 57 if (normals_ && input_ && (cloud != input_))
63 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 70 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 73 threshold_= threshold;
77 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 80 search_radius_ = radius;
84 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 91 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 98 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 105 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 113 __m128 vec1 = _mm_setzero_ps();
115 __m128 vec2 = _mm_setzero_ps();
123 for (
const int &neighbor : neighbors)
125 if (std::isfinite ((*normals_)[neighbor].normal_x))
128 norm1 = _mm_load_ps (&((*normals_)[neighbor].normal_x));
131 norm2 = _mm_set1_ps ((*normals_)[neighbor].normal_x);
134 norm2 = _mm_mul_ps (norm1, norm2);
137 vec1 = _mm_add_ps (vec1, norm2);
140 norm2 = _mm_set1_ps ((*normals_)[neighbor].normal_y);
143 norm2 = _mm_mul_ps (norm1, norm2);
146 vec2 = _mm_add_ps (vec2, norm2);
148 zz += (*normals_)[neighbor].normal_z * (*normals_)[neighbor].normal_z;
154 norm2 = _mm_set1_ps (
float(count));
155 vec1 = _mm_div_ps (vec1, norm2);
156 vec2 = _mm_div_ps (vec2, norm2);
157 _mm_store_ps (coefficients, vec1);
158 _mm_store_ps (coefficients + 4, vec2);
159 coefficients [7] = zz / float(count);
162 memset (coefficients, 0,
sizeof (
float) * 8);
164 memset (coefficients, 0,
sizeof (
float) * 8);
165 for (std::vector<int>::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt)
167 if (std::isfinite ((*normals_)[*iIt].normal_x))
169 coefficients[0] += (*normals_)[*iIt].normal_x * (*normals_)[*iIt].normal_x;
170 coefficients[1] += (*normals_)[*iIt].normal_x * (*normals_)[*iIt].normal_y;
171 coefficients[2] += (*normals_)[*iIt].normal_x * (*normals_)[*iIt].normal_z;
173 coefficients[5] += (*normals_)[*iIt].normal_y * (*normals_)[*iIt].normal_y;
174 coefficients[6] += (*normals_)[*iIt].normal_y * (*normals_)[*iIt].normal_z;
175 coefficients[7] += (*normals_)[*iIt].normal_z * (*normals_)[*iIt].normal_z;
182 float norm = 1.0 / float (count);
183 coefficients[0] *= norm;
184 coefficients[1] *= norm;
185 coefficients[2] *= norm;
186 coefficients[5] *= norm;
187 coefficients[6] *= norm;
188 coefficients[7] *= norm;
194 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
bool 199 PCL_ERROR (
"[pcl::%s::initCompute] init failed!\n", name_.c_str ());
203 if (method_ < 1 || method_ > 5)
205 PCL_ERROR (
"[pcl::%s::initCompute] method (%d) must be in [1..5]!\n", name_.c_str (), method_);
212 normals->reserve (normals->size ());
213 if (!surface_->isOrganized ())
218 normal_estimation.
compute (*normals);
226 normal_estimation.
compute (*normals);
230 if (normals_->size () != surface_->size ())
232 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_);
240 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 245 response->
points.reserve (input_->size());
250 responseHarris(*response);
253 responseNoble(*response);
256 responseLowe(*response);
259 responseCurvature(*response);
262 responseTomasi(*response);
271 for (std::size_t i = 0; i < response->
size (); ++i)
272 keypoints_indices_->indices.push_back (i);
276 output.points.clear ();
277 output.points.reserve (response->
size());
279 #pragma omp parallel for \ 281 shared(output, response) \ 282 num_threads(threads_) 283 for (
int idx = 0; idx < static_cast<int> (response->
size ()); ++idx)
286 !std::isfinite ((*response)[idx].intensity) ||
287 (*response)[idx].intensity < threshold_)
290 std::vector<int> nn_indices;
291 std::vector<float> nn_dists;
292 tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
293 bool is_maxima =
true;
294 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
296 if ((*response)[idx].intensity < (*response)[*iIt].intensity)
305 output.points.push_back ((*response)[idx]);
306 keypoints_indices_->indices.push_back (idx);
311 refineCorners (output);
314 output.width = output.size();
315 output.is_dense =
true;
320 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 323 PCL_ALIGN (16)
float covar [8];
324 output.resize (input_->size ());
325 #pragma omp parallel for \ 328 firstprivate(covar) \ 329 num_threads(threads_) 330 for (
int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
332 const PointInT& pointIn = input_->points [pIdx];
333 output [pIdx].intensity = 0.0;
336 std::vector<int> nn_indices;
337 std::vector<float> nn_dists;
338 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
339 calculateNormalCovar (nn_indices, covar);
341 float trace = covar [0] + covar [5] + covar [7];
344 float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
345 - covar [2] * covar [2] * covar [5]
346 - covar [1] * covar [1] * covar [7]
347 - covar [6] * covar [6] * covar [0];
349 output [pIdx].intensity = 0.04f + det - 0.04f * trace * trace;
352 output [pIdx].x = pointIn.x;
353 output [pIdx].y = pointIn.y;
354 output [pIdx].z = pointIn.z;
356 output.height = input_->height;
357 output.width = input_->width;
361 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 364 PCL_ALIGN (16)
float covar [8];
365 output.resize (input_->size ());
366 #pragma omp parallel \ 369 firstprivate(covar) \ 370 num_threads(threads_) 371 for (
int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
373 const PointInT& pointIn = input_->points [pIdx];
374 output [pIdx].intensity = 0.0;
377 std::vector<int> nn_indices;
378 std::vector<float> nn_dists;
379 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
380 calculateNormalCovar (nn_indices, covar);
381 float trace = covar [0] + covar [5] + covar [7];
384 float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
385 - covar [2] * covar [2] * covar [5]
386 - covar [1] * covar [1] * covar [7]
387 - covar [6] * covar [6] * covar [0];
389 output [pIdx].intensity = det / trace;
392 output [pIdx].x = pointIn.x;
393 output [pIdx].y = pointIn.y;
394 output [pIdx].z = pointIn.z;
396 output.height = input_->height;
397 output.width = input_->width;
401 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 404 PCL_ALIGN (16)
float covar [8];
405 output.resize (input_->size ());
406 #pragma omp parallel for \ 409 firstprivate(covar) \ 410 num_threads(threads_) 411 for (
int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
413 const PointInT& pointIn = input_->points [pIdx];
414 output [pIdx].intensity = 0.0;
417 std::vector<int> nn_indices;
418 std::vector<float> nn_dists;
419 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
420 calculateNormalCovar (nn_indices, covar);
421 float trace = covar [0] + covar [5] + covar [7];
424 float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
425 - covar [2] * covar [2] * covar [5]
426 - covar [1] * covar [1] * covar [7]
427 - covar [6] * covar [6] * covar [0];
429 output [pIdx].intensity = det / (trace * trace);
432 output [pIdx].x = pointIn.x;
433 output [pIdx].y = pointIn.y;
434 output [pIdx].z = pointIn.z;
436 output.height = input_->height;
437 output.width = input_->width;
441 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 445 for (std::size_t idx = 0; idx < input_->size(); ++idx)
447 point.x = (*input_)[idx].x;
448 point.y = (*input_)[idx].y;
449 point.z = (*input_)[idx].z;
450 point.intensity = (*normals_)[idx].curvature;
451 output.points.push_back(point);
454 output.height = input_->height;
455 output.width = input_->width;
459 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 462 PCL_ALIGN (16)
float covar [8];
463 Eigen::Matrix3f covariance_matrix;
464 output.resize (input_->size ());
465 #pragma omp parallel for \ 468 firstprivate(covar, covariance_matrix) \ 469 num_threads(threads_) 470 for (
int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
472 const PointInT& pointIn = input_->points [pIdx];
473 output [pIdx].intensity = 0.0;
476 std::vector<int> nn_indices;
477 std::vector<float> nn_dists;
478 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
479 calculateNormalCovar (nn_indices, covar);
480 float trace = covar [0] + covar [5] + covar [7];
483 covariance_matrix.coeffRef (0) = covar [0];
484 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = covar [1];
485 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = covar [2];
486 covariance_matrix.coeffRef (4) = covar [5];
487 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = covar [6];
488 covariance_matrix.coeffRef (8) = covar [7];
490 EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
492 output [pIdx].intensity = eigen_values[0];
495 output [pIdx].x = pointIn.x;
496 output [pIdx].y = pointIn.y;
497 output [pIdx].z = pointIn.z;
499 output.height = input_->height;
500 output.width = input_->width;
504 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 509 Eigen::Matrix3f NNTInv;
510 Eigen::Vector3f NNTp;
512 const unsigned max_iterations = 10;
513 #pragma omp parallel for \ 516 firstprivate(nnT, NNT, NNTInv, NNTp, diff) \ 517 num_threads(threads_) 518 for (
int cIdx = 0; cIdx < static_cast<int> (corners.size ()); ++cIdx)
520 unsigned iterations = 0;
525 corner.x = corners[cIdx].x;
526 corner.y = corners[cIdx].y;
527 corner.z = corners[cIdx].z;
528 std::vector<int> nn_indices;
529 std::vector<float> nn_dists;
530 tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists);
531 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
533 if (!std::isfinite ((*normals_)[*iIt].normal_x))
536 nnT = (*normals_)[*iIt].getNormalVector3fMap () * (*normals_)[*iIt].getNormalVector3fMap ().transpose();
538 NNTp += nnT * (*surface_)[*iIt].getVector3fMap ();
541 corners[cIdx].getVector3fMap () = NNTInv * NNTp;
543 diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm ();
544 }
while (diff > 1e-6 && ++iterations < max_iterations);
548 #define PCL_INSTANTIATE_HarrisKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint3D<T,U,N>; 549 #endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_ void responseNoble(PointCloudOut &output) const
void setRadius(float radius)
Set the radius for normal estimation and non maxima supression.
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.
void responseHarris(PointCloudOut &output) const
gets the corner response for valid input points
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 responseCurvature(PointCloudOut &output) const
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
void refineCorners(PointCloudOut &corners) const
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Surface normal estimation on organized data using integral images.
void responseLowe(PointCloudOut &output) const
bool initCompute() override
void setRefine(bool do_refine)
Whether the detected key points should be refined or not.
void calculateNormalCovar(const std::vector< int > &neighbors, float *coefficients) const
calculates the upper triangular part of unnormalized covariance matrix over the normals given by the ...
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 setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
Matrix::Scalar invert3x3SymMatrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 3x3 symmetric matrix.
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 <setInputCloud (), setIndices ()> using th...
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Define methods for measuring time spent in code blocks.
typename PointCloudN::ConstPtr PointCloudNConstPtr
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
void responseTomasi(PointCloudOut &output) const
Define methods for centroid estimation and covariance matrix calculus.