40 #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_
41 #define PCL_HARRIS_KEYPOINT_2D_IMPL_H_
44 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
51 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
54 threshold_= threshold;
58 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
65 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
72 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
75 window_width_= window_width;
79 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
82 window_height_= window_height;
86 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
89 skipped_pixels_= skipped_pixels;
93 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
96 min_distance_ = min_distance;
100 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
103 static const int width =
static_cast<int> (input_->width);
104 static const int height =
static_cast<int> (input_->height);
106 int x =
static_cast<int> (index % input_->width);
107 int y =
static_cast<int> (index / input_->width);
110 memset (coefficients, 0,
sizeof (
float) * 3);
112 int endx = std::min (width, x + half_window_width_);
113 int endy = std::min (height, y + half_window_height_);
114 for (
int xx = std::max (0, x - half_window_width_); xx < endx; ++xx)
115 for (
int yy = std::max (0, y - half_window_height_); yy < endy; ++yy)
117 const float& ix = derivatives_rows_ (xx,yy);
118 const float& iy = derivatives_cols_ (xx,yy);
119 coefficients[0]+= ix * ix;
120 coefficients[1]+= ix * iy;
121 coefficients[2]+= iy * iy;
126 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
bool
131 PCL_ERROR (
"[pcl::%s::initCompute] init failed.!\n", name_.c_str ());
135 if (!input_->isOrganized ())
137 PCL_ERROR (
"[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
141 if (indices_->size () != input_->size ())
143 PCL_ERROR (
"[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ());
147 if ((window_height_%2) == 0)
149 PCL_ERROR (
"[pcl::%s::initCompute] Window height must be odd!\n", name_.c_str ());
153 if ((window_width_%2) == 0)
155 PCL_ERROR (
"[pcl::%s::initCompute] Window width must be odd!\n", name_.c_str ());
159 if (window_height_ < 3 || window_width_ < 3)
161 PCL_ERROR (
"[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ());
165 half_window_width_ = window_width_ / 2;
166 half_window_height_ = window_height_ / 2;
172 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
175 derivatives_cols_.resize (input_->width, input_->height);
176 derivatives_rows_.resize (input_->width, input_->height);
179 int w =
static_cast<int> (input_->width) - 1;
180 int h =
static_cast<int> (input_->height) - 1;
183 derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5;
184 derivatives_rows_(0,0) = (intensity_ ((*input_) (1,0)) - intensity_ ((*input_) (0,0))) * 0.5;
186 for(
int i = 1; i < w; ++i)
188 derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5;
191 derivatives_rows_(w,0) = (intensity_ ((*input_) (w,0)) - intensity_ ((*input_) (w-1,0))) * 0.5;
192 derivatives_cols_(w,0) = (intensity_ ((*input_) (w,1)) - intensity_ ((*input_) (w,0))) * 0.5;
194 for(
int j = 1; j < h; ++j)
197 derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5;
198 for(
int i = 1; i < w; ++i)
201 derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5;
204 derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5;
207 derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5;
211 derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5;
212 derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5;
214 for(
int i = 1; i < w; ++i)
216 derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5;
218 derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5;
219 derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5;
224 responseHarris(*response_);
227 responseNoble(*response_);
230 responseLowe(*response_);
233 responseTomasi(*response_);
240 for (std::size_t i = 0; i < response_->size (); ++i)
241 keypoints_indices_->indices.push_back (i);
245 std::sort (indices_->begin (), indices_->end (), [
this] (
int p1,
int p2) { return greaterIntensityAtIndices (p1, p2); });
246 float threshold = threshold_ * response_->points[indices_->front ()].intensity;
248 output.reserve (response_->size());
249 std::vector<bool> occupency_map (response_->size (),
false);
250 int width (response_->width);
251 int height (response_->height);
252 const int occupency_map_size (occupency_map.size ());
255 #pragma omp parallel for shared (output, occupency_map) firstprivate (width, height) num_threads(threads_)
257 for (
int i = 0; i < occupency_map_size; ++i)
259 int idx = indices_->at (i);
260 const PointOutT& point_out = response_->points [idx];
261 if (occupency_map[idx] || point_out.intensity < threshold || !
isFinite (point_out))
268 output.push_back (point_out);
269 keypoints_indices_->indices.push_back (idx);
272 int u_end = std::min (width, idx % width + min_distance_);
273 int v_end = std::min (height, idx / width + min_distance_);
274 for(
int u = std::max (0, idx % width - min_distance_); u < u_end; ++u)
275 for(
int v = std::max (0, idx / width - min_distance_); v < v_end; ++v)
276 occupency_map[v*input_->width+u] =
true;
287 output.is_dense = input_->is_dense;
291 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
294 PCL_ALIGN (16)
float covar [3];
296 output.resize (input_->size ());
297 const int output_size (output.size ());
300 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
302 for (
int index = 0; index < output_size; ++index)
304 PointOutT& out_point = output.points [index];
305 const PointInT &in_point = (*input_).points [index];
306 out_point.intensity = 0;
307 out_point.x = in_point.x;
308 out_point.y = in_point.y;
309 out_point.z = in_point.z;
312 computeSecondMomentMatrix (index, covar);
313 float trace = covar [0] + covar [2];
316 float det = covar[0] * covar[2] - covar[1] * covar[1];
317 out_point.intensity = 0.04f + det - 0.04f * trace * trace;
322 output.height = input_->height;
323 output.width = input_->width;
327 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
330 PCL_ALIGN (16)
float covar [3];
332 output.resize (input_->size ());
333 const int output_size (output.size ());
336 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
338 for (
int index = 0; index < output_size; ++index)
340 PointOutT &out_point = output.points [index];
341 const PointInT &in_point = input_->points [index];
342 out_point.x = in_point.x;
343 out_point.y = in_point.y;
344 out_point.z = in_point.z;
345 out_point.intensity = 0;
348 computeSecondMomentMatrix (index, covar);
349 float trace = covar [0] + covar [2];
352 float det = covar[0] * covar[2] - covar[1] * covar[1];
353 out_point.intensity = det / trace;
358 output.height = input_->height;
359 output.width = input_->width;
363 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
366 PCL_ALIGN (16)
float covar [3];
368 output.resize (input_->size ());
369 const int output_size (output.size ());
372 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
374 for (
int index = 0; index < output_size; ++index)
376 PointOutT &out_point = output.points [index];
377 const PointInT &in_point = input_->points [index];
378 out_point.x = in_point.x;
379 out_point.y = in_point.y;
380 out_point.z = in_point.z;
381 out_point.intensity = 0;
384 computeSecondMomentMatrix (index, covar);
385 float trace = covar [0] + covar [2];
388 float det = covar[0] * covar[2] - covar[1] * covar[1];
389 out_point.intensity = det / (trace * trace);
394 output.height = input_->height;
395 output.width = input_->width;
399 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
402 PCL_ALIGN (16)
float covar [3];
404 output.resize (input_->size ());
405 const int output_size (output.size ());
408 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
410 for (
int index = 0; index < output_size; ++index)
412 PointOutT &out_point = output.points [index];
413 const PointInT &in_point = input_->points [index];
414 out_point.x = in_point.x;
415 out_point.y = in_point.y;
416 out_point.z = in_point.z;
417 out_point.intensity = 0;
420 computeSecondMomentMatrix (index, covar);
422 out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f);
426 output.height = input_->height;
427 output.width = input_->width;
474 #define PCL_INSTANTIATE_HarrisKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::HarrisKeypoint2D<T,U,I>;
475 #endif // #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_