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 (
size_t i = 0; i < response_->size (); ++i)
241 keypoints_indices_->indices.
push_back (i);
245 std::sort (indices_->begin (), indices_->end (),
246 boost::bind (&HarrisKeypoint2D::greaterIntensityAtIndices,
this, _1, _2));
247 float threshold = threshold_ * response_->points[indices_->front ()].intensity;
249 output.
reserve (response_->size());
250 std::vector<bool> occupency_map (response_->size (),
false);
251 int width (response_->width);
252 int height (response_->height);
253 const int occupency_map_size (occupency_map.size ());
256 #pragma omp parallel for shared (output, occupency_map) firstprivate (width, height) num_threads(threads_)
258 for (
int i = 0; i < occupency_map_size; ++i)
260 int idx = indices_->at (i);
261 const PointOutT& point_out = response_->points [idx];
262 if (occupency_map[idx] || point_out.intensity < threshold || !
isFinite (point_out))
270 keypoints_indices_->indices.push_back (idx);
273 int u_end = std::min (width, idx % width + min_distance_);
274 int v_end = std::min (height, idx / width + min_distance_);
275 for(
int u = std::max (0, idx % width - min_distance_); u < u_end; ++u)
276 for(
int v = std::max (0, idx / width - min_distance_); v < v_end; ++v)
277 occupency_map[v*input_->width+u] =
true;
284 output.
width =
static_cast<uint32_t
> (output.
size());
292 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
295 PCL_ALIGN (16)
float covar [3];
297 output.
resize (input_->size ());
298 const int output_size (output.
size ());
301 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
303 for (
int index = 0; index < output_size; ++index)
305 PointOutT& out_point = output.
points [index];
306 const PointInT &in_point = (*input_).points [index];
307 out_point.intensity = 0;
308 out_point.x = in_point.x;
309 out_point.y = in_point.y;
310 out_point.z = in_point.z;
313 computeSecondMomentMatrix (index, covar);
314 float trace = covar [0] + covar [2];
317 float det = covar[0] * covar[2] - covar[1] * covar[1];
318 out_point.intensity = 0.04f + det - 0.04f * trace * trace;
323 output.
height = input_->height;
324 output.
width = input_->width;
328 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
331 PCL_ALIGN (16)
float covar [3];
333 output.
resize (input_->size ());
334 const int output_size (output.
size ());
337 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
339 for (
int index = 0; index < output_size; ++index)
341 PointOutT &out_point = output.
points [index];
342 const PointInT &in_point = input_->points [index];
343 out_point.x = in_point.x;
344 out_point.y = in_point.y;
345 out_point.z = in_point.z;
346 out_point.intensity = 0;
349 computeSecondMomentMatrix (index, covar);
350 float trace = covar [0] + covar [2];
353 float det = covar[0] * covar[2] - covar[1] * covar[1];
354 out_point.intensity = det / trace;
359 output.
height = input_->height;
360 output.
width = input_->width;
364 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
367 PCL_ALIGN (16)
float covar [3];
369 output.
resize (input_->size ());
370 const int output_size (output.
size ());
373 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
375 for (
int index = 0; index < output_size; ++index)
377 PointOutT &out_point = output.
points [index];
378 const PointInT &in_point = input_->points [index];
379 out_point.x = in_point.x;
380 out_point.y = in_point.y;
381 out_point.z = in_point.z;
382 out_point.intensity = 0;
385 computeSecondMomentMatrix (index, covar);
386 float trace = covar [0] + covar [2];
389 float det = covar[0] * covar[2] - covar[1] * covar[1];
390 out_point.intensity = det / (trace * trace);
395 output.
height = input_->height;
396 output.
width = input_->width;
400 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
403 PCL_ALIGN (16)
float covar [3];
405 output.
resize (input_->size ());
406 const int output_size (output.
size ());
409 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
411 for (
int index = 0; index < output_size; ++index)
413 PointOutT &out_point = output.
points [index];
414 const PointInT &in_point = input_->points [index];
415 out_point.x = in_point.x;
416 out_point.y = in_point.y;
417 out_point.z = in_point.z;
418 out_point.intensity = 0;
421 computeSecondMomentMatrix (index, covar);
423 out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f);
427 output.
height = input_->height;
428 output.
width = input_->width;
475 #define PCL_INSTANTIATE_HarrisKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::HarrisKeypoint2D<T,U,I>;
476 #endif // #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_