108 static const int width =
static_cast<int> (input_->width);
109 static const int height =
static_cast<int> (input_->height);
111 int x =
static_cast<int> (index % input_->width);
112 int y =
static_cast<int> (index / input_->width);
115 std::fill_n(coefficients, 3, 0);
117 int endx = std::min (width, x + half_window_width_);
118 int endy = std::min (height, y + half_window_height_);
119 for (
int xx = std::max (0, x - half_window_width_); xx < endx; ++xx)
120 for (
int yy = std::max (0, y - half_window_height_); yy < endy; ++yy)
122 const float& ix = derivatives_rows_ (xx,yy);
123 const float& iy = derivatives_cols_ (xx,yy);
124 coefficients[0]+= ix * ix;
125 coefficients[1]+= ix * iy;
126 coefficients[2]+= iy * iy;
180 derivatives_cols_.resize (input_->width, input_->height);
181 derivatives_rows_.resize (input_->width, input_->height);
184 int w =
static_cast<int> (input_->width) - 1;
185 int h =
static_cast<int> (input_->height) - 1;
188 derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5;
189 derivatives_rows_(0,0) = (intensity_ ((*input_) (1,0)) - intensity_ ((*input_) (0,0))) * 0.5;
191 for(
int i = 1; i < w; ++i)
193 derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5;
196 derivatives_rows_(w,0) = (intensity_ ((*input_) (w,0)) - intensity_ ((*input_) (w-1,0))) * 0.5;
197 derivatives_cols_(w,0) = (intensity_ ((*input_) (w,1)) - intensity_ ((*input_) (w,0))) * 0.5;
199 for(
int j = 1; j < h; ++j)
202 derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5;
203 for(
int i = 1; i < w; ++i)
206 derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5;
209 derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5;
212 derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5;
216 derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5;
217 derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5;
219 for(
int i = 1; i < w; ++i)
221 derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5;
223 derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5;
224 derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5;
229 responseHarris(*response_);
232 responseNoble(*response_);
235 responseLowe(*response_);
238 responseTomasi(*response_);
245 for (std::size_t i = 0; i < response_->size (); ++i)
246 keypoints_indices_->indices.push_back (i);
250 std::sort (indices_->begin (), indices_->end (), [
this] (
int p1,
int p2) { return greaterIntensityAtIndices (p1, p2); });
251 const float threshold = threshold_ * (*response_)[indices_->front ()].intensity;
253 output.reserve (response_->size());
254 std::vector<bool> occupency_map (response_->size (),
false);
255 int width (response_->width);
256 int height (response_->height);
257 const int occupency_map_size (occupency_map.size ());
259#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
260#pragma omp parallel for \
262 shared(occupency_map, output) \
263 firstprivate(width, height) \
264 num_threads(threads_)
266#pragma omp parallel for \
268 shared(occupency_map, occupency_map_size, output, threshold) \
269 firstprivate(width, height) \
270 num_threads(threads_)
272 for (
int i = 0; i < occupency_map_size; ++i)
274 int idx = indices_->at (i);
275 const PointOutT& point_out = response_->points [idx];
276 if (occupency_map[idx] || point_out.intensity < threshold || !
isXYZFinite (point_out))
281 output.push_back (point_out);
282 keypoints_indices_->indices.push_back (idx);
285 int u_end = std::min (width, idx % width + min_distance_);
286 int v_end = std::min (height, idx / width + min_distance_);
287 for(
int u = std::max (0, idx % width - min_distance_); u < u_end; ++u)
288 for(
int v = std::max (0, idx / width - min_distance_); v < v_end; ++v)
289 occupency_map[v*input_->width+u] =
true;
296 output.width = output.size();
300 output.is_dense = input_->is_dense;
307 PCL_ALIGN (16)
float covar [3];
309 output.resize (input_->size ());
310 const int output_size (output.size ());
312#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
313#pragma omp parallel for \
316 firstprivate(covar) \
317 num_threads(threads_)
319#pragma omp parallel for \
321 shared(output, output_size) \
322 firstprivate(covar) \
323 num_threads(threads_)
325 for (
int index = 0; index < output_size; ++index)
327 PointOutT& out_point = output.points [index];
328 const PointInT &in_point = (*input_).points [index];
329 out_point.intensity = 0;
330 out_point.x = in_point.x;
331 out_point.y = in_point.y;
332 out_point.z = in_point.z;
335 computeSecondMomentMatrix (index, covar);
336 float trace = covar [0] + covar [2];
339 float det = covar[0] * covar[2] - covar[1] * covar[1];
340 out_point.intensity = 0.04f + det - 0.04f * trace * trace;
345 output.height = input_->height;
346 output.width = input_->width;
353 PCL_ALIGN (16)
float covar [3];
355 output.resize (input_->size ());
356 const int output_size (output.size ());
358#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
359#pragma omp parallel for \
362 firstprivate(covar) \
363 num_threads(threads_)
365#pragma omp parallel for \
367 shared(output, output_size) \
368 firstprivate(covar) \
369 num_threads(threads_)
371 for (
int index = 0; index < output_size; ++index)
373 PointOutT &out_point = output.points [index];
374 const PointInT &in_point = input_->points [index];
375 out_point.x = in_point.x;
376 out_point.y = in_point.y;
377 out_point.z = in_point.z;
378 out_point.intensity = 0;
381 computeSecondMomentMatrix (index, covar);
382 float trace = covar [0] + covar [2];
385 float det = covar[0] * covar[2] - covar[1] * covar[1];
386 out_point.intensity = det / trace;
391 output.height = input_->height;
392 output.width = input_->width;
399 PCL_ALIGN (16)
float covar [3];
401 output.resize (input_->size ());
402 const int output_size (output.size ());
404#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
405#pragma omp parallel for \
408 firstprivate(covar) \
409 num_threads(threads_)
411#pragma omp parallel for \
413 shared(output, output_size) \
414 firstprivate(covar) \
415 num_threads(threads_)
417 for (
int index = 0; index < output_size; ++index)
419 PointOutT &out_point = output.points [index];
420 const PointInT &in_point = input_->points [index];
421 out_point.x = in_point.x;
422 out_point.y = in_point.y;
423 out_point.z = in_point.z;
424 out_point.intensity = 0;
427 computeSecondMomentMatrix (index, covar);
428 float trace = covar [0] + covar [2];
431 float det = covar[0] * covar[2] - covar[1] * covar[1];
432 out_point.intensity = det / (trace * trace);
437 output.height = input_->height;
438 output.width = input_->width;
445 PCL_ALIGN (16)
float covar [3];
447 output.resize (input_->size ());
448 const int output_size (output.size ());
450#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
451#pragma omp parallel for \
454 firstprivate(covar) \
455 num_threads(threads_)
457#pragma omp parallel for \
459 shared(output, output_size) \
460 firstprivate(covar) \
461 num_threads(threads_)
463 for (
int index = 0; index < output_size; ++index)
465 PointOutT &out_point = output.points [index];
466 const PointInT &in_point = input_->points [index];
467 out_point.x = in_point.x;
468 out_point.y = in_point.y;
469 out_point.z = in_point.z;
470 out_point.intensity = 0;
473 computeSecondMomentMatrix (index, covar);
475 out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f);
479 output.height = input_->height;
480 output.width = input_->width;