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);
111 memset (coefficients, 0,
sizeof (
float) * 3);
113 int endx = std::min (width, x + half_window_width_);
114 int endy = std::min (height, y + half_window_height_);
115 for (
int xx = std::max (0, x - half_window_width_); xx < endx; ++xx)
116 for (
int yy = std::max (0, y - half_window_height_); yy < endy; ++yy)
118 const float& ix = derivatives_rows_ (xx,yy);
119 const float& iy = derivatives_cols_ (xx,yy);
120 coefficients[0]+= ix * ix;
121 coefficients[1]+= ix * iy;
122 coefficients[2]+= iy * iy;
127 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
bool
132 PCL_ERROR (
"[pcl::%s::initCompute] init failed.!\n", name_.c_str ());
136 if (!input_->isOrganized ())
138 PCL_ERROR (
"[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
142 if (indices_->size () != input_->size ())
144 PCL_ERROR (
"[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ());
148 if ((window_height_%2) == 0)
150 PCL_ERROR (
"[pcl::%s::initCompute] Window height must be odd!\n", name_.c_str ());
154 if ((window_width_%2) == 0)
156 PCL_ERROR (
"[pcl::%s::initCompute] Window width must be odd!\n", name_.c_str ());
160 if (window_height_ < 3 || window_width_ < 3)
162 PCL_ERROR (
"[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ());
166 half_window_width_ = window_width_ / 2;
167 half_window_height_ = window_height_ / 2;
173 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
176 derivatives_cols_.resize (input_->width, input_->height);
177 derivatives_rows_.resize (input_->width, input_->height);
180 int w =
static_cast<int> (input_->width) - 1;
181 int h =
static_cast<int> (input_->height) - 1;
184 derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5;
185 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;
203 for(
int j = 1; j < h; ++j)
206 derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5;
207 for(
int i = 1; i < w; ++i)
210 derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5;
213 derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5;
216 derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5;
220 derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5;
221 derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5;
227 for(
int i = 1; i < w; ++i)
229 derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5;
231 derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5;
232 derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5;
234 float highest_response_;
239 responseHarris(*response_, highest_response_);
242 responseNoble(*response_, highest_response_);
245 responseLowe(*response_, highest_response_);
248 responseTomasi(*response_, highest_response_);
256 threshold_*= highest_response_;
258 std::sort (indices_->begin (), indices_->end (),
259 boost::bind (&HarrisKeypoint2D::greaterIntensityAtIndices,
this, _1, _2));
262 output.
reserve (response_->size());
263 std::vector<bool> occupency_map (response_->size (),
false);
264 int width (response_->width);
265 int height (response_->height);
266 const int occupency_map_size (occupency_map.size ());
269 #pragma omp parallel for shared (output, occupency_map) private (width, height) num_threads(threads_)
271 for (
int idx = 0; idx < occupency_map_size; ++idx)
273 if (occupency_map[idx] || response_->points [indices_->at (idx)].intensity < threshold_ || !
isFinite (response_->points[idx]))
279 output.
push_back (response_->at (indices_->at (idx)));
281 int u_end = std::min (width, indices_->at (idx) % width + min_distance_);
282 int v_end = std::min (height, indices_->at (idx) / width + min_distance_);
283 for(
int u = std::max (0, indices_->at (idx) % width - min_distance_); u < u_end; ++u)
284 for(
int v = std::max (0, indices_->at (idx) / width - min_distance_); v < v_end; ++v)
285 occupency_map[v*input_->width+u] =
true;
292 output.
width =
static_cast<uint32_t
> (output.
size());
300 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
303 PCL_ALIGN (16)
float covar [3];
305 output.
resize (input_->size ());
306 highest_response = - std::numeric_limits<float>::max ();
307 const int output_size (output.
size ());
310 #pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_)
312 for (
int index = 0; index < output_size; ++index)
314 PointOutT& out_point = output.
points [index];
315 const PointInT &in_point = (*input_).points [index];
316 out_point.intensity = 0;
317 out_point.x = in_point.x;
318 out_point.y = in_point.y;
319 out_point.z = in_point.z;
322 computeSecondMomentMatrix (index, covar);
323 float trace = covar [0] + covar [2];
326 float det = covar[0] * covar[2] - covar[1] * covar[1];
327 out_point.intensity = 0.04f + det - 0.04f * trace * trace;
332 highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response;
337 output.
height = input_->height;
338 output.
width = input_->width;
342 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
345 PCL_ALIGN (16)
float covar [3];
347 output.
resize (input_->size ());
348 highest_response = - std::numeric_limits<float>::max ();
349 const int output_size (output.
size ());
352 #pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_)
354 for (
size_t index = 0; index < output_size; ++index)
356 PointOutT &out_point = output.
points [index];
357 const PointInT &in_point = input_->points [index];
358 out_point.x = in_point.x;
359 out_point.y = in_point.y;
360 out_point.z = in_point.z;
361 out_point.intensity = 0;
364 computeSecondMomentMatrix (index, covar);
365 float trace = covar [0] + covar [2];
368 float det = covar[0] * covar[2] - covar[1] * covar[1];
369 out_point.intensity = det / trace;
374 highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response;
379 output.
height = input_->height;
380 output.
width = input_->width;
384 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
387 PCL_ALIGN (16)
float covar [3];
389 output.
resize (input_->size ());
390 highest_response = -std::numeric_limits<float>::max ();
391 const int output_size (output.
size ());
394 #pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_)
396 for (
size_t index = 0; index < output_size; ++index)
398 PointOutT &out_point = output.
points [index];
399 const PointInT &in_point = input_->points [index];
400 out_point.x = in_point.x;
401 out_point.y = in_point.y;
402 out_point.z = in_point.z;
403 out_point.intensity = 0;
406 computeSecondMomentMatrix (index, covar);
407 float trace = covar [0] + covar [2];
410 float det = covar[0] * covar[2] - covar[1] * covar[1];
411 out_point.intensity = det / (trace * trace);
416 highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response;
421 output.
height = input_->height;
422 output.
width = input_->width;
426 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
429 PCL_ALIGN (16)
float covar [3];
431 output.
resize (input_->size ());
432 highest_response = -std::numeric_limits<float>::max ();
433 const int output_size (output.
size ());
436 #pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_)
438 for (
size_t index = 0; index < output_size; ++index)
440 PointOutT &out_point = output.
points [index];
441 const PointInT &in_point = input_->points [index];
442 out_point.x = in_point.x;
443 out_point.y = in_point.y;
444 out_point.z = in_point.z;
445 out_point.intensity = 0;
448 computeSecondMomentMatrix (index, covar);
450 out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f);
455 highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response;
459 output.
height = input_->height;
460 output.
width = input_->width;
507 #define PCL_INSTANTIATE_HarrisKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::HarrisKeypoint2D<T,U,I>;
508 #endif // #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_