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());
279 #pragma omp parallel for \
281 shared(output, response) \
282 num_threads(threads_)
283 for (
int idx = 0; idx < static_cast<int> (response->
points.size ()); ++idx)
286 !std::isfinite (response->
points[idx].intensity) ||
287 response->
points[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->
points[idx].intensity < response->
points[*iIt].intensity)
305 output.points.push_back (response->
points[idx]);
306 keypoints_indices_->indices.push_back (idx);
311 refineCorners (output);
314 output.width =
static_cast<std::uint32_t> (output.points.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 \
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 \
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 \
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_->points.size(); ++idx)
447 point.x = input_->points[idx].x;
448 point.y = input_->points[idx].y;
449 point.z = input_->points[idx].z;
450 point.intensity = normals_->points[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 private(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 private(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_->points[*iIt].normal_x))
536 nnT = normals_->points[*iIt].getNormalVector3fMap () * normals_->points[*iIt].getNormalVector3fMap ().transpose();
538 NNTp += nnT * surface_->points[*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_