Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
harris_2d.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 * $Id$
37 *
38 */
39
40
41#ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_
42#define PCL_HARRIS_KEYPOINT_2D_IMPL_H_
43
44#include <pcl/common/point_tests.h>
45
46namespace pcl
47{
48
49template <typename PointInT, typename PointOutT, typename IntensityT> void
51{
52 method_ = method;
53}
54
55
56template <typename PointInT, typename PointOutT, typename IntensityT> void
58{
59 threshold_= threshold;
60}
61
62
63template <typename PointInT, typename PointOutT, typename IntensityT> void
65{
66 refine_ = do_refine;
67}
68
69
70template <typename PointInT, typename PointOutT, typename IntensityT> void
72{
73 nonmax_ = nonmax;
74}
75
76
77template <typename PointInT, typename PointOutT, typename IntensityT> void
79{
80 window_width_= window_width;
81}
82
83
84template <typename PointInT, typename PointOutT, typename IntensityT> void
86{
87 window_height_= window_height;
88}
89
90
91template <typename PointInT, typename PointOutT, typename IntensityT> void
93{
94 skipped_pixels_= skipped_pixels;
95}
96
97
98template <typename PointInT, typename PointOutT, typename IntensityT> void
100{
101 min_distance_ = min_distance;
102}
103
104
105template <typename PointInT, typename PointOutT, typename IntensityT> void
107{
108 static const int width = static_cast<int> (input_->width);
109 static const int height = static_cast<int> (input_->height);
110
111 int x = static_cast<int> (index % input_->width);
112 int y = static_cast<int> (index / input_->width);
113 // indices 0 1 2
114 // coefficients: ixix ixiy iyiy
115 memset (coefficients, 0, sizeof (float) * 3);
116
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)
121 {
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;
127 }
128}
129
130
131template <typename PointInT, typename PointOutT, typename IntensityT> bool
133{
135 {
136 PCL_ERROR ("[pcl::%s::initCompute] init failed.!\n", name_.c_str ());
137 return (false);
138 }
139
140 if (!input_->isOrganized ())
141 {
142 PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
143 return (false);
144 }
145
146 if (indices_->size () != input_->size ())
147 {
148 PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ());
149 return (false);
150 }
151
152 if ((window_height_%2) == 0)
153 {
154 PCL_ERROR ("[pcl::%s::initCompute] Window height must be odd!\n", name_.c_str ());
155 return (false);
156 }
157
158 if ((window_width_%2) == 0)
159 {
160 PCL_ERROR ("[pcl::%s::initCompute] Window width must be odd!\n", name_.c_str ());
161 return (false);
162 }
163
164 if (window_height_ < 3 || window_width_ < 3)
165 {
166 PCL_ERROR ("[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ());
167 return (false);
168 }
169
170 half_window_width_ = window_width_ / 2;
171 half_window_height_ = window_height_ / 2;
172
173 return (true);
174}
175
176
177template <typename PointInT, typename PointOutT, typename IntensityT> void
179{
180 derivatives_cols_.resize (input_->width, input_->height);
181 derivatives_rows_.resize (input_->width, input_->height);
182 //Compute cloud intensities first derivatives along columns and rows
183 //!!! nsallem 20120220 : we don't test here for density so if one term is nan the result is nan
184 int w = static_cast<int> (input_->width) - 1;
185 int h = static_cast<int> (input_->height) - 1;
186 // j = 0 --> j-1 out of range ; use 0
187 // i = 0 --> i-1 out of range ; use 0
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;
190
191 for(int i = 1; i < w; ++i)
192 {
193 derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5;
194 }
195
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;
198
199 for(int j = 1; j < h; ++j)
200 {
201 // i = 0 --> i-1 out of range ; use 0
202 derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5;
203 for(int i = 1; i < w; ++i)
204 {
205 // derivative with respect to rows
206 derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5;
207
208 // derivative with respect to cols
209 derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5;
210 }
211 // i = w --> w+1 out of range ; use w
212 derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5;
213 }
214
215 // j = h --> j+1 out of range use h
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;
218
219 for(int i = 1; i < w; ++i)
220 {
221 derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5;
222 }
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;
225
226 switch (method_)
227 {
228 case HARRIS:
229 responseHarris(*response_);
230 break;
231 case NOBLE:
232 responseNoble(*response_);
233 break;
234 case LOWE:
235 responseLowe(*response_);
236 break;
237 case TOMASI:
238 responseTomasi(*response_);
239 break;
240 }
241
242 if (!nonmax_)
243 {
244 output = *response_;
245 for (std::size_t i = 0; i < response_->size (); ++i)
246 keypoints_indices_->indices.push_back (i);
247 }
248 else
249 {
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;
252 output.clear ();
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 ());
258
259#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
260#pragma omp parallel for \
261 default(none) \
262 shared(occupency_map, output) \
263 firstprivate(width, height) \
264 num_threads(threads_)
265#else
266#pragma omp parallel for \
267 default(none) \
268 shared(occupency_map, occupency_map_size, output, threshold) \
269 firstprivate(width, height) \
270 num_threads(threads_)
271#endif
272 for (int i = 0; i < occupency_map_size; ++i)
273 {
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))
277 continue;
278
279#pragma omp critical
280 {
281 output.push_back (point_out);
282 keypoints_indices_->indices.push_back (idx);
283 }
284
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;
290 }
291
292 // if (refine_)
293 // refineCorners (output);
294
295 output.height = 1;
296 output.width = output.size();
297 }
298
299 // we don not change the denseness
300 output.is_dense = input_->is_dense;
301}
302
303
304template <typename PointInT, typename PointOutT, typename IntensityT> void
306{
307 PCL_ALIGN (16) float covar [3];
308 output.clear ();
309 output.resize (input_->size ());
310 const int output_size (output.size ());
311
312#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
313#pragma omp parallel for \
314 default(none) \
315 shared(output) \
316 firstprivate(covar) \
317 num_threads(threads_)
318#else
319#pragma omp parallel for \
320 default(none) \
321 shared(output, output_size) \
322 firstprivate(covar) \
323 num_threads(threads_)
324#endif
325 for (int index = 0; index < output_size; ++index)
326 {
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;
333 if (isXYZFinite (in_point))
334 {
335 computeSecondMomentMatrix (index, covar);
336 float trace = covar [0] + covar [2];
337 if (trace != 0.f)
338 {
339 float det = covar[0] * covar[2] - covar[1] * covar[1];
340 out_point.intensity = 0.04f + det - 0.04f * trace * trace;
341 }
342 }
343 }
344
345 output.height = input_->height;
346 output.width = input_->width;
347}
348
349
350template <typename PointInT, typename PointOutT, typename IntensityT> void
352{
353 PCL_ALIGN (16) float covar [3];
354 output.clear ();
355 output.resize (input_->size ());
356 const int output_size (output.size ());
357
358#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
359#pragma omp parallel for \
360 default(none) \
361 shared(output) \
362 firstprivate(covar) \
363 num_threads(threads_)
364#else
365#pragma omp parallel for \
366 default(none) \
367 shared(output, output_size) \
368 firstprivate(covar) \
369 num_threads(threads_)
370#endif
371 for (int index = 0; index < output_size; ++index)
372 {
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;
379 if (isXYZFinite (in_point))
380 {
381 computeSecondMomentMatrix (index, covar);
382 float trace = covar [0] + covar [2];
383 if (trace != 0)
384 {
385 float det = covar[0] * covar[2] - covar[1] * covar[1];
386 out_point.intensity = det / trace;
387 }
388 }
389 }
390
391 output.height = input_->height;
392 output.width = input_->width;
393}
394
395
396template <typename PointInT, typename PointOutT, typename IntensityT> void
398{
399 PCL_ALIGN (16) float covar [3];
400 output.clear ();
401 output.resize (input_->size ());
402 const int output_size (output.size ());
403
404#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
405#pragma omp parallel for \
406 default(none) \
407 shared(output) \
408 firstprivate(covar) \
409 num_threads(threads_)
410#else
411#pragma omp parallel for \
412 default(none) \
413 shared(output, output_size) \
414 firstprivate(covar) \
415 num_threads(threads_)
416#endif
417 for (int index = 0; index < output_size; ++index)
418 {
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;
425 if (isXYZFinite (in_point))
426 {
427 computeSecondMomentMatrix (index, covar);
428 float trace = covar [0] + covar [2];
429 if (trace != 0)
430 {
431 float det = covar[0] * covar[2] - covar[1] * covar[1];
432 out_point.intensity = det / (trace * trace);
433 }
434 }
435 }
436
437 output.height = input_->height;
438 output.width = input_->width;
439}
440
441
442template <typename PointInT, typename PointOutT, typename IntensityT> void
444{
445 PCL_ALIGN (16) float covar [3];
446 output.clear ();
447 output.resize (input_->size ());
448 const int output_size (output.size ());
449
450#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
451#pragma omp parallel for \
452 default(none) \
453 shared(output) \
454 firstprivate(covar) \
455 num_threads(threads_)
456#else
457#pragma omp parallel for \
458 default(none) \
459 shared(output, output_size) \
460 firstprivate(covar) \
461 num_threads(threads_)
462#endif
463 for (int index = 0; index < output_size; ++index)
464 {
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;
471 if (isXYZFinite (in_point))
472 {
473 computeSecondMomentMatrix (index, covar);
474 // min egenvalue
475 out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f);
476 }
477 }
478
479 output.height = input_->height;
480 output.width = input_->width;
481}
482
483} // namespace pcl
484
485#define PCL_INSTANTIATE_HarrisKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::HarrisKeypoint2D<T,U,I>;
486
487#endif // #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_
488
void setNonMaxSupression(bool=false)
whether non maxima suppression should be applied or the response for each point should be returned
Definition harris_2d.hpp:71
void setSkippedPixels(int skipped_pixels)
Set number of pixels to skip.
Definition harris_2d.hpp:92
void responseTomasi(PointCloudOut &output) const
void responseLowe(PointCloudOut &output) const
void setRefine(bool do_refine)
whether the detected key points should be refined or not.
Definition harris_2d.hpp:64
void detectKeypoints(PointCloudOut &output) override
void responseNoble(PointCloudOut &output) const
void setMinimalDistance(int min_distance)
Set minimal distance between candidate keypoints.
Definition harris_2d.hpp:99
void setMethod(ResponseMethod type)
set the method of the response to be calculated.
Definition harris_2d.hpp:50
void computeSecondMomentMatrix(std::size_t pos, float *coefficients) const
calculates the upper triangular part of unnormalized covariance matrix over intensities given by the ...
void setWindowHeight(int window_height)
Set window height.
Definition harris_2d.hpp:85
void responseHarris(PointCloudOut &output) const
gets the corner response for valid input points
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition harris_2d.h:60
bool initCompute() override
void setThreshold(float threshold)
set the threshold value for detecting corners.
Definition harris_2d.hpp:57
void setWindowWidth(int window_width)
Set window width.
Definition harris_2d.hpp:78
Keypoint represents the base class for key points.
Definition keypoint.h:49
constexpr bool isXYZFinite(const PointT &) noexcept