Point Cloud Library (PCL)  1.7.0
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 #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_
41 #define PCL_HARRIS_KEYPOINT_2D_IMPL_H_
42 
43 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
44 template <typename PointInT, typename PointOutT, typename IntensityT> void
46 {
47  method_ = method;
48 }
49 
50 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51 template <typename PointInT, typename PointOutT, typename IntensityT> void
53 {
54  threshold_= threshold;
55 }
56 
57 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
58 template <typename PointInT, typename PointOutT, typename IntensityT> void
60 {
61  refine_ = do_refine;
62 }
63 
64 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
65 template <typename PointInT, typename PointOutT, typename IntensityT> void
67 {
68  nonmax_ = nonmax;
69 }
70 
71 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
72 template <typename PointInT, typename PointOutT, typename IntensityT> void
74 {
75  window_width_= window_width;
76 }
77 
78 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
79 template <typename PointInT, typename PointOutT, typename IntensityT> void
81 {
82  window_height_= window_height;
83 }
84 
85 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
86 template <typename PointInT, typename PointOutT, typename IntensityT> void
88 {
89  skipped_pixels_= skipped_pixels;
90 }
91 
92 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
93 template <typename PointInT, typename PointOutT, typename IntensityT> void
95 {
96  min_distance_ = min_distance;
97 }
98 
99 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
100 template <typename PointInT, typename PointOutT, typename IntensityT> void
102 {
103  static const int width = static_cast<int> (input_->width);
104  static const int height = static_cast<int> (input_->height);
105 
106  int x = static_cast<int> (index % input_->width);
107  int y = static_cast<int> (index / input_->width);
108  unsigned count = 0;
109  // indices 0 1 2
110  // coefficients: ixix ixiy iyiy
111  memset (coefficients, 0, sizeof (float) * 3);
112 
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)
117  {
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;
123  }
124 }
125 
126 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
127 template <typename PointInT, typename PointOutT, typename IntensityT> bool
129 {
131  {
132  PCL_ERROR ("[pcl::%s::initCompute] init failed.!\n", name_.c_str ());
133  return (false);
134  }
135 
136  if (!input_->isOrganized ())
137  {
138  PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
139  return (false);
140  }
141 
142  if (indices_->size () != input_->size ())
143  {
144  PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ());
145  return (false);
146  }
147 
148  if ((window_height_%2) == 0)
149  {
150  PCL_ERROR ("[pcl::%s::initCompute] Window height must be odd!\n", name_.c_str ());
151  return (false);
152  }
153 
154  if ((window_width_%2) == 0)
155  {
156  PCL_ERROR ("[pcl::%s::initCompute] Window width must be odd!\n", name_.c_str ());
157  return (false);
158  }
159 
160  if (window_height_ < 3 || window_width_ < 3)
161  {
162  PCL_ERROR ("[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ());
163  return (false);
164  }
165 
166  half_window_width_ = window_width_ / 2;
167  half_window_height_ = window_height_ / 2;
168 
169  return (true);
170 }
171 
172 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
173 template <typename PointInT, typename PointOutT, typename IntensityT> void
175 {
176  derivatives_cols_.resize (input_->width, input_->height);
177  derivatives_rows_.resize (input_->width, input_->height);
178  //Compute cloud intensities first derivatives along columns and rows
179  //!!! nsallem 20120220 : we don't test here for density so if one term in nan the result is nan
180  int w = static_cast<int> (input_->width) - 1;
181  int h = static_cast<int> (input_->height) - 1;
182  // j = 0 --> j-1 out of range ; use 0
183  // i = 0 --> i-1 out of range ; use 0
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;
186 
187 // #ifdef _OPENMP
188 // //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_)
189 // #pragma omp parallel for num_threads (threads_)
190 // #endif
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 // #ifdef _OPENMP
200 // //#pragma omp parallel for shared (derivatives_cols_, derivatives_rows_, input_) num_threads (threads_)
201 // #pragma omp parallel for num_threads (threads_)
202 // #endif
203  for(int j = 1; j < h; ++j)
204  {
205  // i = 0 --> i-1 out of range ; use 0
206  derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5;
207  for(int i = 1; i < w; ++i)
208  {
209  // derivative with respect to rows
210  derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5;
211 
212  // derivative with respect to cols
213  derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5;
214  }
215  // i = w --> w+1 out of range ; use w
216  derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5;
217  }
218 
219  // j = h --> j+1 out of range use h
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;
222 
223 // #ifdef _OPENMP
224 // //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_)
225 // #pragma omp parallel for num_threads (threads_)
226 // #endif
227  for(int i = 1; i < w; ++i)
228  {
229  derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5;
230  }
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;
233 
234  float highest_response_;
235 
236  switch (method_)
237  {
238  case HARRIS:
239  responseHarris(*response_, highest_response_);
240  break;
241  case NOBLE:
242  responseNoble(*response_, highest_response_);
243  break;
244  case LOWE:
245  responseLowe(*response_, highest_response_);
246  break;
247  case TOMASI:
248  responseTomasi(*response_, highest_response_);
249  break;
250  }
251 
252  if (!nonmax_)
253  output = *response_;
254  else
255  {
256  threshold_*= highest_response_;
257 
258  std::sort (indices_->begin (), indices_->end (),
259  boost::bind (&HarrisKeypoint2D::greaterIntensityAtIndices, this, _1, _2));
260 
261  output.clear ();
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 ());
267 
268 #ifdef _OPENMP
269 #pragma omp parallel for shared (output, occupency_map) private (width, height) num_threads(threads_)
270 #endif
271  for (int idx = 0; idx < occupency_map_size; ++idx)
272  {
273  if (occupency_map[idx] || response_->points [indices_->at (idx)].intensity < threshold_ || !isFinite (response_->points[idx]))
274  continue;
275 
276 #ifdef _OPENMP
277 #pragma omp critical
278 #endif
279  output.push_back (response_->at (indices_->at (idx)));
280 
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;
286  }
287 
288  // if (refine_)
289  // refineCorners (output);
290 
291  output.height = 1;
292  output.width = static_cast<uint32_t> (output.size());
293  }
294 
295  // we don not change the denseness
296  output.is_dense = input_->is_dense;
297 }
298 
299 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
300 template <typename PointInT, typename PointOutT, typename IntensityT> void
302 {
303  PCL_ALIGN (16) float covar [3];
304  output.clear ();
305  output.resize (input_->size ());
306  highest_response = - std::numeric_limits<float>::max ();
307  const int output_size (output.size ());
308 
309 #ifdef _OPENMP
310 #pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_)
311 #endif
312  for (int index = 0; index < output_size; ++index)
313  {
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;
320  if (isFinite (in_point))
321  {
322  computeSecondMomentMatrix (index, covar);
323  float trace = covar [0] + covar [2];
324  if (trace != 0.f)
325  {
326  float det = covar[0] * covar[2] - covar[1] * covar[1];
327  out_point.intensity = 0.04f + det - 0.04f * trace * trace;
328 
329 #ifdef _OPENMP
330 #pragma omp critical
331 #endif
332  highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response;
333  }
334  }
335  }
336 
337  output.height = input_->height;
338  output.width = input_->width;
339 }
340 
341 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
342 template <typename PointInT, typename PointOutT, typename IntensityT> void
344 {
345  PCL_ALIGN (16) float covar [3];
346  output.clear ();
347  output.resize (input_->size ());
348  highest_response = - std::numeric_limits<float>::max ();
349  const int output_size (output.size ());
350 
351 #ifdef _OPENMP
352 #pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_)
353 #endif
354  for (size_t index = 0; index < output_size; ++index)
355  {
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;
362  if (isFinite (in_point))
363  {
364  computeSecondMomentMatrix (index, covar);
365  float trace = covar [0] + covar [2];
366  if (trace != 0)
367  {
368  float det = covar[0] * covar[2] - covar[1] * covar[1];
369  out_point.intensity = det / trace;
370 
371 #ifdef _OPENMP
372 #pragma omp critical
373 #endif
374  highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response;
375  }
376  }
377  }
378 
379  output.height = input_->height;
380  output.width = input_->width;
381 }
382 
383 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
384 template <typename PointInT, typename PointOutT, typename IntensityT> void
386 {
387  PCL_ALIGN (16) float covar [3];
388  output.clear ();
389  output.resize (input_->size ());
390  highest_response = -std::numeric_limits<float>::max ();
391  const int output_size (output.size ());
392 
393 #ifdef _OPENMP
394 #pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_)
395 #endif
396  for (size_t index = 0; index < output_size; ++index)
397  {
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;
404  if (isFinite (in_point))
405  {
406  computeSecondMomentMatrix (index, covar);
407  float trace = covar [0] + covar [2];
408  if (trace != 0)
409  {
410  float det = covar[0] * covar[2] - covar[1] * covar[1];
411  out_point.intensity = det / (trace * trace);
412 
413 #ifdef _OPENMP
414 #pragma omp critical
415 #endif
416  highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response;
417  }
418  }
419  }
420 
421  output.height = input_->height;
422  output.width = input_->width;
423 }
424 
425 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
426 template <typename PointInT, typename PointOutT, typename IntensityT> void
428 {
429  PCL_ALIGN (16) float covar [3];
430  output.clear ();
431  output.resize (input_->size ());
432  highest_response = -std::numeric_limits<float>::max ();
433  const int output_size (output.size ());
434 
435 #ifdef _OPENMP
436 #pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_)
437 #endif
438  for (size_t index = 0; index < output_size; ++index)
439  {
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;
446  if (isFinite (in_point))
447  {
448  computeSecondMomentMatrix (index, covar);
449  // min egenvalue
450  out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f);
451 
452 #ifdef _OPENMP
453 #pragma omp critical
454 #endif
455  highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response;
456  }
457  }
458 
459  output.height = input_->height;
460  output.width = input_->width;
461 }
462 
463 // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
464 // template <typename PointInT, typename PointOutT, typename IntensityT> void
465 // pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::refineCorners (PointCloudOut &corners) const
466 // {
467 // std::vector<int> nn_indices;
468 // std::vector<float> nn_dists;
469 
470 // Eigen::Matrix2f nnT;
471 // Eigen::Matrix2f NNT;
472 // Eigen::Matrix2f NNTInv;
473 // Eigen::Vector2f NNTp;
474 // float diff;
475 // const unsigned max_iterations = 10;
476 // #ifdef _OPENMP
477 // #pragma omp parallel for shared (corners) private (nnT, NNT, NNTInv, NNTp, diff, nn_indices, nn_dists) num_threads(threads_)
478 // #endif
479 // for (int cIdx = 0; cIdx < static_cast<int> (corners.size ()); ++cIdx)
480 // {
481 // unsigned iterations = 0;
482 // do {
483 // NNT.setZero();
484 // NNTp.setZero();
485 // PointInT corner;
486 // corner.x = corners[cIdx].x;
487 // corner.y = corners[cIdx].y;
488 // corner.z = corners[cIdx].z;
489 // tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists);
490 // for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
491 // {
492 // if (!pcl_isfinite (normals_->points[*iIt].normal_x))
493 // continue;
494 
495 // nnT = normals_->points[*iIt].getNormalVector3fMap () * normals_->points[*iIt].getNormalVector3fMap ().transpose();
496 // NNT += nnT;
497 // NNTp += nnT * surface_->points[*iIt].getVector3fMap ();
498 // }
499 // if (invert3x3SymMatrix (NNT, NNTInv) != 0)
500 // corners[cIdx].getVector3fMap () = NNTInv * NNTp;
501 
502 // diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm ();
503 // } while (diff > 1e-6 && ++iterations < max_iterations);
504 // }
505 // }
506 
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_