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>
47 #include <pcl/common/time.h>
48 #include <pcl/common/centroid.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 (std::vector<int>::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt)
125 if (pcl_isfinite (normals_->points[*iIt].normal_x))
128 norm1 = _mm_load_ps (&(normals_->points[*iIt].normal_x));
131 norm2 = _mm_set1_ps (normals_->points[*iIt].normal_x);
134 norm2 = _mm_mul_ps (norm1, norm2);
137 vec1 = _mm_add_ps (vec1, norm2);
140 norm2 = _mm_set1_ps (normals_->points[*iIt].normal_y);
143 norm2 = _mm_mul_ps (norm1, norm2);
146 vec2 = _mm_add_ps (vec2, norm2);
148 zz += normals_->points[*iIt].normal_z * normals_->points[*iIt].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 (pcl_isfinite (normals_->points[*iIt].normal_x))
169 coefficients[0] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_x;
170 coefficients[1] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_y;
171 coefficients[2] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_z;
173 coefficients[5] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_y;
174 coefficients[6] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_z;
175 coefficients[7] += normals_->points[*iIt].normal_z * normals_->points[*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_->points.size());
250 responseHarris(*response);
253 responseNoble(*response);
256 responseLowe(*response);
259 responseCurvature(*response);
262 responseTomasi(*response);
271 for (
size_t i = 0; i < response->size (); ++i)
272 keypoints_indices_->indices.push_back (i);
277 output.
points.reserve (response->points.size());
280 #pragma omp parallel for shared (output) num_threads(threads_)
282 for (
int idx = 0; idx < static_cast<int> (response->points.size ()); ++idx)
284 if (!
isFinite (response->points[idx]) ||
285 !pcl_isfinite (response->points[idx].intensity) ||
286 response->points[idx].intensity < threshold_)
289 std::vector<int> nn_indices;
290 std::vector<float> nn_dists;
291 tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
292 bool is_maxima =
true;
293 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
295 if (response->points[idx].intensity < response->points[*iIt].intensity)
306 output.
points.push_back (response->points[idx]);
307 keypoints_indices_->indices.push_back (idx);
312 refineCorners (output);
315 output.
width =
static_cast<uint32_t
> (output.
points.size());
321 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
324 PCL_ALIGN (16)
float covar [8];
325 output.
resize (input_->size ());
327 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
329 for (
int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
331 const PointInT& pointIn = input_->points [pIdx];
332 output [pIdx].intensity = 0.0;
335 std::vector<int> nn_indices;
336 std::vector<float> nn_dists;
337 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
338 calculateNormalCovar (nn_indices, covar);
340 float trace = covar [0] + covar [5] + covar [7];
343 float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
344 - covar [2] * covar [2] * covar [5]
345 - covar [1] * covar [1] * covar [7]
346 - covar [6] * covar [6] * covar [0];
348 output [pIdx].intensity = 0.04f + det - 0.04f * trace * trace;
351 output [pIdx].x = pointIn.x;
352 output [pIdx].y = pointIn.y;
353 output [pIdx].z = pointIn.z;
355 output.
height = input_->height;
356 output.
width = input_->width;
360 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
363 PCL_ALIGN (16)
float covar [8];
364 output.
resize (input_->size ());
366 #pragma omp parallel for shared (output) private (covar) 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;
374 std::vector<int> 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];
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 ());
404 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
406 for (
int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
408 const PointInT& pointIn = input_->points [pIdx];
409 output [pIdx].intensity = 0.0;
412 std::vector<int> nn_indices;
413 std::vector<float> nn_dists;
414 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
415 calculateNormalCovar (nn_indices, covar);
416 float trace = covar [0] + covar [5] + covar [7];
419 float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
420 - covar [2] * covar [2] * covar [5]
421 - covar [1] * covar [1] * covar [7]
422 - covar [6] * covar [6] * covar [0];
424 output [pIdx].intensity = det / (trace * trace);
427 output [pIdx].x = pointIn.x;
428 output [pIdx].y = pointIn.y;
429 output [pIdx].z = pointIn.z;
431 output.
height = input_->height;
432 output.
width = input_->width;
436 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
440 for (
unsigned idx = 0; idx < input_->points.size(); ++idx)
442 point.x = input_->points[idx].x;
443 point.y = input_->points[idx].y;
444 point.z = input_->points[idx].z;
445 point.intensity = normals_->points[idx].curvature;
446 output.
points.push_back(point);
449 output.
height = input_->height;
450 output.
width = input_->width;
454 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
457 PCL_ALIGN (16)
float covar [8];
458 Eigen::Matrix3f covariance_matrix;
459 output.
resize (input_->size ());
461 #pragma omp parallel for shared (output) private (covar, covariance_matrix) num_threads(threads_)
463 for (
int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
465 const PointInT& pointIn = input_->points [pIdx];
466 output [pIdx].intensity = 0.0;
469 std::vector<int> nn_indices;
470 std::vector<float> nn_dists;
471 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
472 calculateNormalCovar (nn_indices, covar);
473 float trace = covar [0] + covar [5] + covar [7];
476 covariance_matrix.coeffRef (0) = covar [0];
477 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = covar [1];
478 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = covar [2];
479 covariance_matrix.coeffRef (4) = covar [5];
480 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = covar [6];
481 covariance_matrix.coeffRef (8) = covar [7];
485 output [pIdx].intensity = eigen_values[0];
488 output [pIdx].x = pointIn.x;
489 output [pIdx].y = pointIn.y;
490 output [pIdx].z = pointIn.z;
492 output.
height = input_->height;
493 output.
width = input_->width;
497 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
502 Eigen::Matrix3f NNTInv;
503 Eigen::Vector3f NNTp;
505 const unsigned max_iterations = 10;
507 #pragma omp parallel for shared (corners) private (nnT, NNT, NNTInv, NNTp, diff) num_threads(threads_)
509 for (
int cIdx = 0; cIdx < static_cast<int> (corners.
size ()); ++cIdx)
511 unsigned iterations = 0;
516 corner.x = corners[cIdx].x;
517 corner.y = corners[cIdx].y;
518 corner.z = corners[cIdx].z;
519 std::vector<int> nn_indices;
520 std::vector<float> nn_dists;
521 tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists);
522 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
524 if (!pcl_isfinite (normals_->points[*iIt].normal_x))
527 nnT = normals_->
points[*iIt].getNormalVector3fMap () * normals_->points[*iIt].getNormalVector3fMap ().transpose();
529 NNTp += nnT * surface_->points[*iIt].getVector3fMap ();
532 corners[cIdx].getVector3fMap () = NNTInv * NNTp;
534 diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm ();
535 }
while (diff > 1e-6 && ++iterations < max_iterations);
539 #define PCL_INSTANTIATE_HarrisKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint3D<T,U,N>;
540 #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 supression.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
virtual void setInputCloud(const PointCloudInConstPtr &cloud)
Provide a pointer to the input dataset.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void setNormals(const PointCloudNConstPtr &normals)
Set normals if precalculated normals are available.
struct pcl::_PointXYZHSV EIGEN_ALIGN16
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 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.
uint32_t height
The point cloud height (if organized as an image-structure).
Keypoint represents the base class for key points.
uint32_t width
The point cloud width (if organized as an image-structure).
PointCloudN::ConstPtr PointCloudNConstPtr
Surface normal estimation on organized data using integral images.
void detectKeypoints(PointCloudOut &output)
Abstract key point detection method.
void setRefine(bool do_refine)
Whether the detected key points should be refined or not.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
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 responseHarris(PointCloudOut &output) const
gets the corner response for valid input points
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
PointCloudIn::ConstPtr PointCloudInConstPtr
Matrix::Scalar invert3x3SymMatrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 3x3 symmetric matrix.
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).
void setMethod(ResponseMethod type)
Set the method of the response to be calculated.
void resize(size_t n)
Resize the cloud.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in using th...
PointCloudN::Ptr PointCloudNPtr
void responseCurvature(PointCloudOut &output) const
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
void responseLowe(PointCloudOut &output) const
void responseNoble(PointCloudOut &output) const