Point Cloud Library (PCL)  1.14.1-dev
keypoint.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, 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  * keypoint.hpp
37  *
38  * Created on: May 28, 2012
39  * Author: somani
40  */
41 
42 #pragma once
43 
44 #include <pcl/2d/convolution.h>
45 #include <pcl/2d/edge.h>
46 #include <pcl/2d/keypoint.h> // for pcl::Keypoint
47 
48 #include <limits>
49 
50 namespace pcl {
51 
52 template <typename ImageType>
53 void
55  ImageType& input,
56  const float sigma_d,
57  const float sigma_i,
58  const float alpha,
59  const float thresh)
60 {
61  /*creating the gaussian kernels*/
62  ImageType kernel_d;
63  ImageType kernel_i;
64  conv_2d.gaussianKernel(5, sigma_d, kernel_d);
65  conv_2d.gaussianKernel(5, sigma_i, kernel_i);
66 
67  /*scaling the image with differentiation scale*/
68  ImageType smoothed_image;
69  conv_2d.convolve(smoothed_image, kernel_d, input);
70 
71  /*image derivatives*/
72  ImageType I_x, I_y;
73  edge_detection.ComputeDerivativeXCentral(I_x, smoothed_image);
74  edge_detection.ComputeDerivativeYCentral(I_y, smoothed_image);
75 
76  /*second moment matrix*/
77  ImageType I_x2, I_y2, I_xI_y;
78  imageElementMultiply(I_x2, I_x, I_x);
79  imageElementMultiply(I_y2, I_y, I_y);
80  imageElementMultiply(I_xI_y, I_x, I_y);
81 
82  /*scaling second moment matrix with integration scale*/
83  ImageType M00, M10, M11;
84  conv_2d.convolve(M00, kernel_i, I_x2);
85  conv_2d.convolve(M10, kernel_i, I_xI_y);
86  conv_2d.convolve(M11, kernel_i, I_y2);
87 
88  /*harris function*/
89  const std::size_t height = input.size();
90  const std::size_t width = input[0].size();
91  output.resize(height);
92  for (std::size_t i = 0; i < height; i++) {
93  output[i].resize(width);
94  for (std::size_t j = 0; j < width; j++) {
95  output[i][j] = M00[i][j] * M11[i][j] - (M10[i][j] * M10[i][j]) -
96  alpha * ((M00[i][j] + M11[i][j]) * (M00[i][j] + M11[i][j]));
97  if (thresh != 0) {
98  if (output[i][j] < thresh)
99  output[i][j] = 0;
100  else
101  output[i][j] = 255;
102  }
103  }
104  }
105 
106  /*local maxima*/
107  for (std::size_t i = 1; i < height - 1; i++) {
108  for (std::size_t j = 1; j < width - 1; j++) {
109  if (output[i][j] > output[i - 1][j - 1] && output[i][j] > output[i - 1][j] &&
110  output[i][j] > output[i - 1][j + 1] && output[i][j] > output[i][j - 1] &&
111  output[i][j] > output[i][j + 1] && output[i][j] > output[i + 1][j - 1] &&
112  output[i][j] > output[i + 1][j] && output[i][j] > output[i + 1][j + 1])
113  ;
114  else
115  output[i][j] = 0;
116  }
117  }
118 }
119 
120 template <typename ImageType>
121 void
123  ImageType& input,
124  const float sigma,
125  bool SCALED)
126 {
127  /*creating the gaussian kernels*/
128  ImageType kernel, cornerness;
129  conv_2d.gaussianKernel(5, sigma, kernel);
130 
131  /*scaling the image with differentiation scale*/
132  ImageType smoothed_image;
133  conv_2d.convolve(smoothed_image, kernel, input);
134 
135  /*image derivatives*/
136  ImageType I_x, I_y;
137  edge_detection.ComputeDerivativeXCentral(I_x, smoothed_image);
138  edge_detection.ComputeDerivativeYCentral(I_y, smoothed_image);
139 
140  /*second moment matrix*/
141  ImageType I_xx, I_yy, I_xy;
142  edge_detection.ComputeDerivativeXCentral(I_xx, I_x);
143  edge_detection.ComputeDerivativeYCentral(I_xy, I_x);
144  edge_detection.ComputeDerivativeYCentral(I_yy, I_y);
145  /*Determinant of Hessian*/
146  const std::size_t height = input.size();
147  const std::size_t width = input[0].size();
148  float min = std::numeric_limits<float>::max();
149  float max = std::numeric_limits<float>::min();
150  cornerness.resize(height);
151  for (std::size_t i = 0; i < height; i++) {
152  cornerness[i].resize(width);
153  for (std::size_t j = 0; j < width; j++) {
154  cornerness[i][j] =
155  sigma * sigma * (I_xx[i][j] + I_yy[i][j] - I_xy[i][j] * I_xy[i][j]);
156  if (SCALED) {
157  if (cornerness[i][j] < min)
158  min = cornerness[i][j];
159  if (cornerness[i][j] > max)
160  max = cornerness[i][j];
161  }
162  }
163 
164  /*local maxima*/
165  output.resize(height);
166  output[0].resize(width);
167  output[height - 1].resize(width);
168  for (std::size_t i = 1; i < height - 1; i++) {
169  output[i].resize(width);
170  for (std::size_t j = 1; j < width - 1; j++) {
171  if (SCALED)
172  output[i][j] = ((cornerness[i][j] - min) / (max - min));
173  else
174  output[i][j] = cornerness[i][j];
175  }
176  }
177  }
178 }
179 
180 template <typename ImageType>
181 void
183  ImageType& input,
184  const float start_scale,
185  const float scaling_factor,
186  const int num_scales)
187 {
188  const std::size_t height = input.size();
189  const std::size_t width = input[0].size();
190  const int local_search_radius = 1;
191  float scale = start_scale;
192  std::vector<ImageType> cornerness;
193  cornerness.resize(num_scales);
194  for (int i = 0; i < num_scales; i++) {
195  hessianBlob(cornerness[i], input, scale, false);
196  scale *= scaling_factor;
197  }
198  for (std::size_t i = 0; i < height; i++) {
199  for (std::size_t j = 0; j < width; j++) {
200  float scale_max = std::numeric_limits<float>::min();
201  /*default output in case of no blob at the current point is 0*/
202  output[i][j] = 0;
203  for (int k = 0; k < num_scales; k++) {
204  /*check if the current point (k,i,j) is a maximum in the defined search radius*/
205  bool non_max_flag = false;
206  const float local_max = cornerness[k][i][j];
207  for (int n = -local_search_radius; n <= local_search_radius; n++) {
208  if (n + k < 0 || n + k >= num_scales)
209  continue;
210  for (int l = -local_search_radius; l <= local_search_radius; l++) {
211  if (l + i < 0 || l + i >= height)
212  continue;
213  for (int m = -local_search_radius; m <= local_search_radius; m++) {
214  if (m + j < 0 || m + j >= width)
215  continue;
216  if (cornerness[n + k][l + i][m + j] > local_max) {
217  non_max_flag = true;
218  break;
219  }
220  }
221  if (non_max_flag)
222  break;
223  }
224  if (non_max_flag)
225  break;
226  }
227  /*if the current point is a point of local maximum, check if it is a maximum
228  * point across scales*/
229  if (!non_max_flag) {
230  if (cornerness[k][i][j] > scale_max) {
231  scale_max = cornerness[k][i][j];
232  /*output indicates the scale at which the blob is found at the current
233  * location in the image*/
234  output[i][j] = start_scale * pow(scaling_factor, k);
235  }
236  }
237  }
238  }
239  }
240 }
241 
242 template <typename ImageType>
243 void
245  ImageType& input1,
246  ImageType& input2)
247 {
248  const std::size_t height = input1.size();
249  const std::size_t width = input1[0].size();
250  output.resize(height);
251  for (std::size_t i = 0; i < height; i++) {
252  output[i].resize(width);
253  for (std::size_t j = 0; j < width; j++) {
254  output[i][j] = input1[i][j] * input2[i][j];
255  }
256  }
257 }
258 } // namespace pcl
void imageElementMultiply(ImageType &output, ImageType &input1, ImageType &input2)
Definition: keypoint.hpp:244
void hessianBlob(ImageType &output, ImageType &input, const float sigma, bool SCALE)
Definition: keypoint.hpp:122
void harrisCorner(ImageType &output, ImageType &input, const float sigma_d, const float sigma_i, const float alpha, const float thresh)
Definition: keypoint.hpp:54
void gaussianKernel(pcl::PointCloud< PointT > &kernel)
Definition: kernel.hpp:96