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_->points[neighbor].normal_x))
128 norm1 = _mm_load_ps (&(normals_->points[neighbor].normal_x));
131 norm2 = _mm_set1_ps (normals_->points[neighbor].normal_x);
134 norm2 = _mm_mul_ps (norm1, norm2);
137 vec1 = _mm_add_ps (vec1, norm2);
140 norm2 = _mm_set1_ps (normals_->points[neighbor].normal_y);
143 norm2 = _mm_mul_ps (norm1, norm2);
146 vec2 = _mm_add_ps (vec2, norm2);
148 zz += normals_->points[neighbor].normal_z * normals_->points[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_->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 (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->
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)
285 !std::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<std::uint32_t> (output.points.size());
316 output.is_dense =
true;
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 (std::size_t 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];
483 EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
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 (!std::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_