41 #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_
42 #define PCL_HARRIS_KEYPOINT_2D_IMPL_H_
48 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
55 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
58 threshold_= threshold;
62 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
69 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
76 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
79 window_width_= window_width;
83 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
86 window_height_= window_height;
90 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
93 skipped_pixels_= skipped_pixels;
97 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
100 min_distance_ = min_distance;
104 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
107 static const int width =
static_cast<int> (input_->width);
108 static const int height =
static_cast<int> (input_->height);
110 int x =
static_cast<int> (index % input_->width);
111 int y =
static_cast<int> (index / input_->width);
114 memset (coefficients, 0,
sizeof (
float) * 3);
116 int endx = std::min (width, x + half_window_width_);
117 int endy = std::min (height, y + half_window_height_);
118 for (
int xx = std::max (0, x - half_window_width_); xx < endx; ++xx)
119 for (
int yy = std::max (0, y - half_window_height_); yy < endy; ++yy)
121 const float& ix = derivatives_rows_ (xx,yy);
122 const float& iy = derivatives_cols_ (xx,yy);
123 coefficients[0]+= ix * ix;
124 coefficients[1]+= ix * iy;
125 coefficients[2]+= iy * iy;
130 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
bool
135 PCL_ERROR (
"[pcl::%s::initCompute] init failed.!\n", name_.c_str ());
139 if (!input_->isOrganized ())
141 PCL_ERROR (
"[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
145 if (indices_->size () != input_->size ())
147 PCL_ERROR (
"[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ());
151 if ((window_height_%2) == 0)
153 PCL_ERROR (
"[pcl::%s::initCompute] Window height must be odd!\n", name_.c_str ());
157 if ((window_width_%2) == 0)
159 PCL_ERROR (
"[pcl::%s::initCompute] Window width must be odd!\n", name_.c_str ());
163 if (window_height_ < 3 || window_width_ < 3)
165 PCL_ERROR (
"[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ());
169 half_window_width_ = window_width_ / 2;
170 half_window_height_ = window_height_ / 2;
176 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
179 derivatives_cols_.resize (input_->width, input_->height);
180 derivatives_rows_.resize (input_->width, input_->height);
183 int w =
static_cast<int> (input_->width) - 1;
184 int h =
static_cast<int> (input_->height) - 1;
187 derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5;
188 derivatives_rows_(0,0) = (intensity_ ((*input_) (1,0)) - intensity_ ((*input_) (0,0))) * 0.5;
190 for(
int i = 1; i < w; ++i)
192 derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5;
195 derivatives_rows_(w,0) = (intensity_ ((*input_) (w,0)) - intensity_ ((*input_) (w-1,0))) * 0.5;
196 derivatives_cols_(w,0) = (intensity_ ((*input_) (w,1)) - intensity_ ((*input_) (w,0))) * 0.5;
198 for(
int j = 1; j < h; ++j)
201 derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5;
202 for(
int i = 1; i < w; ++i)
205 derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5;
208 derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5;
211 derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5;
215 derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5;
216 derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5;
218 for(
int i = 1; i < w; ++i)
220 derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5;
222 derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5;
223 derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5;
228 responseHarris(*response_);
231 responseNoble(*response_);
234 responseLowe(*response_);
237 responseTomasi(*response_);
244 for (std::size_t i = 0; i < response_->size (); ++i)
245 keypoints_indices_->indices.push_back (i);
249 std::sort (indices_->begin (), indices_->end (), [
this] (
int p1,
int p2) {
return greaterIntensityAtIndices (p1, p2); });
250 const float threshold = threshold_ * response_->points[indices_->front ()].intensity;
252 output.reserve (response_->size());
253 std::vector<bool> occupency_map (response_->size (),
false);
254 int width (response_->width);
255 int height (response_->height);
256 const int occupency_map_size (occupency_map.size ());
258 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
259 #pragma omp parallel for \
261 shared(occupency_map, output) \
262 firstprivate(width, height) \
263 num_threads(threads_)
265 #pragma omp parallel for \
267 shared(occupency_map, occupency_map_size, output, threshold) \
268 firstprivate(width, height) \
269 num_threads(threads_)
271 for (
int i = 0; i < occupency_map_size; ++i)
273 int idx = indices_->at (i);
274 const PointOutT& point_out = response_->points [idx];
275 if (occupency_map[idx] || point_out.intensity < threshold || !
isFinite (point_out))
280 output.push_back (point_out);
281 keypoints_indices_->indices.push_back (idx);
284 int u_end = std::min (width, idx % width + min_distance_);
285 int v_end = std::min (height, idx / width + min_distance_);
286 for(
int u = std::max (0, idx % width - min_distance_); u < u_end; ++u)
287 for(
int v = std::max (0, idx / width - min_distance_); v < v_end; ++v)
288 occupency_map[v*input_->width+u] =
true;
295 output.width =
static_cast<std::uint32_t
> (output.size());
299 output.is_dense = input_->is_dense;
303 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
306 PCL_ALIGN (16)
float covar [3];
308 output.resize (input_->size ());
309 const int output_size (output.size ());
311 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
312 #pragma omp parallel for \
316 num_threads(threads_)
318 #pragma omp parallel for \
320 shared(output, output_size) \
322 num_threads(threads_)
324 for (
int index = 0; index < output_size; ++index)
326 PointOutT& out_point = output.points [index];
327 const PointInT &in_point = (*input_).points [index];
328 out_point.intensity = 0;
329 out_point.x = in_point.x;
330 out_point.y = in_point.y;
331 out_point.z = in_point.z;
334 computeSecondMomentMatrix (index, covar);
335 float trace = covar [0] + covar [2];
338 float det = covar[0] * covar[2] - covar[1] * covar[1];
339 out_point.intensity = 0.04f + det - 0.04f * trace * trace;
344 output.height = input_->height;
345 output.width = input_->width;
349 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
352 PCL_ALIGN (16)
float covar [3];
354 output.resize (input_->size ());
355 const int output_size (output.size ());
357 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
358 #pragma omp parallel for \
362 num_threads(threads_)
364 #pragma omp parallel for \
366 shared(output, output_size) \
368 num_threads(threads_)
370 for (
int index = 0; index < output_size; ++index)
372 PointOutT &out_point = output.points [index];
373 const PointInT &in_point = input_->points [index];
374 out_point.x = in_point.x;
375 out_point.y = in_point.y;
376 out_point.z = in_point.z;
377 out_point.intensity = 0;
380 computeSecondMomentMatrix (index, covar);
381 float trace = covar [0] + covar [2];
384 float det = covar[0] * covar[2] - covar[1] * covar[1];
385 out_point.intensity = det / trace;
390 output.height = input_->height;
391 output.width = input_->width;
395 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
398 PCL_ALIGN (16)
float covar [3];
400 output.resize (input_->size ());
401 const int output_size (output.size ());
403 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
404 #pragma omp parallel for \
408 num_threads(threads_)
410 #pragma omp parallel for \
412 shared(output, output_size) \
414 num_threads(threads_)
416 for (
int index = 0; index < output_size; ++index)
418 PointOutT &out_point = output.points [index];
419 const PointInT &in_point = input_->points [index];
420 out_point.x = in_point.x;
421 out_point.y = in_point.y;
422 out_point.z = in_point.z;
423 out_point.intensity = 0;
426 computeSecondMomentMatrix (index, covar);
427 float trace = covar [0] + covar [2];
430 float det = covar[0] * covar[2] - covar[1] * covar[1];
431 out_point.intensity = det / (trace * trace);
436 output.height = input_->height;
437 output.width = input_->width;
441 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
444 PCL_ALIGN (16)
float covar [3];
446 output.resize (input_->size ());
447 const int output_size (output.size ());
449 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
450 #pragma omp parallel for \
454 num_threads(threads_)
456 #pragma omp parallel for \
458 shared(output, output_size) \
460 num_threads(threads_)
462 for (
int index = 0; index < output_size; ++index)
464 PointOutT &out_point = output.points [index];
465 const PointInT &in_point = input_->points [index];
466 out_point.x = in_point.x;
467 out_point.y = in_point.y;
468 out_point.z = in_point.z;
469 out_point.intensity = 0;
472 computeSecondMomentMatrix (index, covar);
474 out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f);
478 output.height = input_->height;
479 output.width = input_->width;
484 #define PCL_INSTANTIATE_HarrisKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::HarrisKeypoint2D<T,U,I>;
486 #endif // #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
void setMethod(ResponseMethod type)
set the method of the response to be calculated.
void responseHarris(PointCloudOut &output) const
gets the corner response for valid input points
void setWindowHeight(int window_height)
Set window height.
void setNonMaxSupression(bool=false)
whether non maxima suppression should be applied or the response for each point should be returned ...
void setMinimalDistance(int min_distance)
Set minimal distance between candidate keypoints.
void setRefine(bool do_refine)
whether the detected key points should be refined or not.
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
Keypoint represents the base class for key points.
void responseNoble(PointCloudOut &output) const
bool initCompute() override
void setSkippedPixels(int skipped_pixels)
Set number of pixels to skip.
void detectKeypoints(PointCloudOut &output) override
void setWindowWidth(int window_width)
Set window width.
void setThreshold(float threshold)
set the threshold value for detecting corners.
void responseTomasi(PointCloudOut &output) const
void computeSecondMomentMatrix(std::size_t pos, float *coefficients) const
calculates the upper triangular part of unnormalized covariance matrix over intensities given by the ...
void responseLowe(PointCloudOut &output) const