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 setRadius(float radius)
Set the radius for normal estimation and non maxima supression.
void setThreshold(float threshold)
Set the threshold value for detecting corners.
void responseTomasi(PointCloudOut &output) const
void resize(size_t n)
Resize the cloud.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void responseNoble(PointCloudOut &output) const
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...
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
void refineCorners(PointCloudOut &corners) const
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
void setMethod(ResponseMethod type)
Set the method of the response to be calculated.
void setNonMaxSupression(bool=false)
Whether non maxima suppression should be applied or the response for each point should be returned...
Keypoint represents the base class for key points.
void detectKeypoints(PointCloudOut &output)
Abstract key point detection method.
Surface normal estimation on organized data using integral images.
void setRefine(bool do_refine)
Whether the detected key points should be refined or not.
uint32_t width
The point cloud width (if organized as an image-structure).
PointCloudN::Ptr PointCloudNPtr
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
void setNormals(const PointCloudNConstPtr &normals)
Set normals if precalculated normals are available.
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
uint32_t height
The point cloud height (if organized as an image-structure).
void responseHarris(PointCloudOut &output) const
gets the corner response for valid input points
Matrix::Scalar invert3x3SymMatrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 3x3 symmetric matrix.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
PointCloudN::ConstPtr PointCloudNConstPtr
struct pcl::_PointXYZHSV EIGEN_ALIGN16
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
void responseCurvature(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 responseLowe(PointCloudOut &output) const