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_