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
61 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
64 threshold_= threshold;
68 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
71 search_radius_ = radius;
75 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
82 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
89 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
96 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
104 __m128 vec1 = _mm_setzero_ps();
106 __m128 vec2 = _mm_setzero_ps();
114 for (std::vector<int>::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt)
116 if (pcl_isfinite (normals_->points[*iIt].normal_x))
119 norm1 = _mm_load_ps (&(normals_->points[*iIt].normal_x));
122 norm2 = _mm_set1_ps (normals_->points[*iIt].normal_x);
125 norm2 = _mm_mul_ps (norm1, norm2);
128 vec1 = _mm_add_ps (vec1, norm2);
131 norm2 = _mm_set1_ps (normals_->points[*iIt].normal_y);
134 norm2 = _mm_mul_ps (norm1, norm2);
137 vec2 = _mm_add_ps (vec2, norm2);
139 zz += normals_->points[*iIt].normal_z * normals_->points[*iIt].normal_z;
145 norm2 = _mm_set1_ps (
float(count));
146 vec1 = _mm_div_ps (vec1, norm2);
147 vec2 = _mm_div_ps (vec2, norm2);
148 _mm_store_ps (coefficients, vec1);
149 _mm_store_ps (coefficients + 4, vec2);
150 coefficients [7] = zz / float(count);
153 memset (coefficients, 0,
sizeof (
float) * 8);
155 memset (coefficients, 0,
sizeof (
float) * 8);
156 for (std::vector<int>::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt)
158 if (pcl_isfinite (normals_->points[*iIt].normal_x))
160 coefficients[0] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_x;
161 coefficients[1] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_y;
162 coefficients[2] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_z;
164 coefficients[5] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_y;
165 coefficients[6] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_z;
166 coefficients[7] += normals_->points[*iIt].normal_z * normals_->points[*iIt].normal_z;
173 float norm = 1.0 / float (count);
174 coefficients[0] *= norm;
175 coefficients[1] *= norm;
176 coefficients[2] *= norm;
177 coefficients[5] *= norm;
178 coefficients[6] *= norm;
179 coefficients[7] *= norm;
185 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
bool
190 PCL_ERROR (
"[pcl::%s::initCompute] init failed!\n", name_.c_str ());
194 if (method_ < 1 || method_ > 5)
196 PCL_ERROR (
"[pcl::%s::initCompute] method (%d) must be in [1..5]!\n", name_.c_str (), method_);
203 normals->reserve (normals->size ());
204 if (!surface_->isOrganized ())
209 normal_estimation.
compute (*normals);
217 normal_estimation.
compute (*normals);
221 if (normals_->size () != surface_->size ())
223 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 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
235 response->points.reserve (input_->points.size());
240 responseHarris(*response);
243 responseNoble(*response);
246 responseLowe(*response);
249 responseCurvature(*response);
252 responseTomasi(*response);
265 output.
points.reserve (response->points.size());
268 #pragma omp parallel for shared (output) num_threads(threads_)
270 for (
int idx = 0; idx < static_cast<int> (response->points.size ()); ++idx)
272 if (!
isFinite (response->points[idx]) ||
273 !pcl_isfinite (response->points[idx].intensity) ||
274 response->points[idx].intensity < threshold_)
277 std::vector<int> nn_indices;
278 std::vector<float> nn_dists;
279 tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
280 bool is_maxima =
true;
281 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
283 if (response->points[idx].intensity < response->points[*iIt].intensity)
293 output.
points.push_back (response->points[idx]);
297 refineCorners (output);
300 output.
width =
static_cast<uint32_t
> (output.
points.size());
306 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
309 PCL_ALIGN (16)
float covar [8];
310 output.
resize (input_->size ());
312 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
314 for (
int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
316 const PointInT& pointIn = input_->points [pIdx];
317 output [pIdx].intensity = 0.0;
320 std::vector<int> nn_indices;
321 std::vector<float> nn_dists;
322 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
323 calculateNormalCovar (nn_indices, covar);
325 float trace = covar [0] + covar [5] + covar [7];
328 float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
329 - covar [2] * covar [2] * covar [5]
330 - covar [1] * covar [1] * covar [7]
331 - covar [6] * covar [6] * covar [0];
333 output [pIdx].intensity = 0.04f + det - 0.04f * trace * trace;
336 output [pIdx].x = pointIn.x;
337 output [pIdx].y = pointIn.y;
338 output [pIdx].z = pointIn.z;
340 output.
height = input_->height;
341 output.
width = input_->width;
345 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
348 PCL_ALIGN (16)
float covar [8];
349 output.
resize (input_->size ());
351 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
353 for (
int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
355 const PointInT& pointIn = input_->points [pIdx];
356 output [pIdx].intensity = 0.0;
359 std::vector<int> nn_indices;
360 std::vector<float> nn_dists;
361 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
362 calculateNormalCovar (nn_indices, covar);
363 float trace = covar [0] + covar [5] + covar [7];
366 float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
367 - covar [2] * covar [2] * covar [5]
368 - covar [1] * covar [1] * covar [7]
369 - covar [6] * covar [6] * covar [0];
371 output [pIdx].intensity = det / trace;
374 output [pIdx].x = pointIn.x;
375 output [pIdx].y = pointIn.y;
376 output [pIdx].z = pointIn.z;
378 output.
height = input_->height;
379 output.
width = input_->width;
383 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
386 PCL_ALIGN (16)
float covar [8];
387 output.
resize (input_->size ());
389 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
391 for (
int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
393 const PointInT& pointIn = input_->points [pIdx];
394 output [pIdx].intensity = 0.0;
397 std::vector<int> nn_indices;
398 std::vector<float> nn_dists;
399 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
400 calculateNormalCovar (nn_indices, covar);
401 float trace = covar [0] + covar [5] + covar [7];
404 float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
405 - covar [2] * covar [2] * covar [5]
406 - covar [1] * covar [1] * covar [7]
407 - covar [6] * covar [6] * covar [0];
409 output [pIdx].intensity = det / (trace * trace);
412 output [pIdx].x = pointIn.x;
413 output [pIdx].y = pointIn.y;
414 output [pIdx].z = pointIn.z;
416 output.
height = input_->height;
417 output.
width = input_->width;
421 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
425 for (
unsigned idx = 0; idx < input_->points.size(); ++idx)
427 point.x = input_->points[idx].x;
428 point.y = input_->points[idx].y;
429 point.z = input_->points[idx].z;
430 point.intensity = normals_->points[idx].curvature;
431 output.
points.push_back(point);
434 output.
height = input_->height;
435 output.
width = input_->width;
439 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
442 PCL_ALIGN (16)
float covar [8];
443 Eigen::Matrix3f covariance_matrix;
444 output.
resize (input_->size ());
446 #pragma omp parallel for shared (output) private (covar, covariance_matrix) num_threads(threads_)
448 for (
int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
450 const PointInT& pointIn = input_->points [pIdx];
451 output [pIdx].intensity = 0.0;
454 std::vector<int> nn_indices;
455 std::vector<float> nn_dists;
456 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
457 calculateNormalCovar (nn_indices, covar);
458 float trace = covar [0] + covar [5] + covar [7];
461 covariance_matrix.coeffRef (0) = covar [0];
462 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = covar [1];
463 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = covar [2];
464 covariance_matrix.coeffRef (4) = covar [5];
465 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = covar [6];
466 covariance_matrix.coeffRef (8) = covar [7];
470 output [pIdx].intensity = eigen_values[0];
473 output [pIdx].x = pointIn.x;
474 output [pIdx].y = pointIn.y;
475 output [pIdx].z = pointIn.z;
477 output.
height = input_->height;
478 output.
width = input_->width;
482 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
487 Eigen::Matrix3f NNTInv;
488 Eigen::Vector3f NNTp;
490 const unsigned max_iterations = 10;
492 #pragma omp parallel for shared (corners) private (nnT, NNT, NNTInv, NNTp, diff) num_threads(threads_)
494 for (
int cIdx = 0; cIdx < static_cast<int> (corners.
size ()); ++cIdx)
496 unsigned iterations = 0;
501 corner.x = corners[cIdx].x;
502 corner.y = corners[cIdx].y;
503 corner.z = corners[cIdx].z;
504 std::vector<int> nn_indices;
505 std::vector<float> nn_dists;
506 tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists);
507 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
509 if (!pcl_isfinite (normals_->points[*iIt].normal_x))
512 nnT = normals_->
points[*iIt].getNormalVector3fMap () * normals_->points[*iIt].getNormalVector3fMap ().transpose();
514 NNTp += nnT * surface_->points[*iIt].getVector3fMap ();
517 corners[cIdx].getVector3fMap () = NNTInv * NNTp;
519 diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm ();
520 }
while (diff > 1e-6 && ++iterations < max_iterations);
524 #define PCL_INSTANTIATE_HarrisKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint3D<T,U,N>;
525 #endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
PointCloudN::Ptr PointCloudNPtr
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void responseHarris(PointCloudOut &output) const
gets the corner response for valid input points
uint32_t height
The point cloud height (if organized as an image-structure).
void responseCurvature(PointCloudOut &output) const
void responseLowe(PointCloudOut &output) const
PointCloudN::ConstPtr PointCloudNConstPtr
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
struct pcl::_PointXYZHSV EIGEN_ALIGN16
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
void setRadius(float radius)
Set the radius for normal estimation and non maxima supression.
void responseTomasi(PointCloudOut &output) const
void responseNoble(PointCloudOut &output) const
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 compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
void refineCorners(PointCloudOut &corners) const
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
Surface normal estimation on organized data using integral images.
void setThreshold(float threshold)
Set the threshold value for detecting corners.
void resize(size_t n)
Resize the cloud.
void setMethod(ResponseMethod type)
Set the method of the response to be calculated.
Keypoint represents the base class for key points.
void detectKeypoints(PointCloudOut &output)
Abstract key point detection method.
uint32_t width
The point cloud width (if organized as an image-structure).
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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...
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 setRefine(bool do_refine)
Whether the detected key points should be refined or not.
void setNormals(const PointCloudNConstPtr &normals)
Set normals if precalculated normals are available.