Point Cloud Library (PCL)  1.10.0
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 #ifndef PCL_2D_KEYPOINT_HPP_
43 #define PCL_2D_KEYPOINT_HPP_
44 
45 #include <limits>
46 #include <pcl/2d/convolution.h>
47 #include <pcl/2d/edge.h>
48 
49 //////////////////////////////////////////////////////////////////////////////
50 void
51 pcl::keypoint::harrisCorner(ImageType& output,
52  ImageType& input,
53  const float sigma_d,
54  const float sigma_i,
55  const float alpha,
56  const float thresh)
57 {
58 
59  /*creating the gaussian kernels*/
60  ImageType kernel_d;
61  ImageType kernel_i;
62  conv_2d.gaussianKernel(5, sigma_d, kernel_d);
63  conv_2d.gaussianKernel(5, sigma_i, kernel_i);
64 
65  /*scaling the image with differentiation scale*/
66  ImageType smoothed_image;
67  conv_2d.convolve(smoothed_image, kernel_d, input);
68 
69  /*image derivatives*/
70  ImageType I_x, I_y;
71  edge_detection.ComputeDerivativeXCentral(I_x, smoothed_image);
72  edge_detection.ComputeDerivativeYCentral(I_y, smoothed_image);
73 
74  /*second moment matrix*/
75  ImageType I_x2, I_y2, I_xI_y;
76  imageElementMultiply(I_x2, I_x, I_x);
77  imageElementMultiply(I_y2, I_y, I_y);
78  imageElementMultiply(I_xI_y, I_x, I_y);
79 
80  /*scaling second moment matrix with integration scale*/
81  ImageType M00, M10, M11;
82  conv_2d.convolve(M00, kernel_i, I_x2);
83  conv_2d.convolve(M10, kernel_i, I_xI_y);
84  conv_2d.convolve(M11, kernel_i, I_y2);
85 
86  /*harris function*/
87  const std::size_t height = input.size();
88  const std::size_t width = input[0].size();
89  output.resize(height);
90  for (std::size_t i = 0; i < height; i++) {
91  output[i].resize(width);
92  for (std::size_t j = 0; j < width; j++) {
93  output[i][j] = M00[i][j] * M11[i][j] - (M10[i][j] * M10[i][j]) -
94  alpha * ((M00[i][j] + M11[i][j]) * (M00[i][j] + M11[i][j]));
95  if (thresh != 0) {
96  if (output[i][j] < thresh)
97  output[i][j] = 0;
98  else
99  output[i][j] = 255;
100  }
101  }
102  }
103 
104  /*local maxima*/
105  for (std::size_t i = 1; i < height - 1; i++) {
106  for (std::size_t j = 1; j < width - 1; j++) {
107  if (output[i][j] > output[i - 1][j - 1] && output[i][j] > output[i - 1][j] &&
108  output[i][j] > output[i - 1][j + 1] && output[i][j] > output[i][j - 1] &&
109  output[i][j] > output[i][j + 1] && output[i][j] > output[i + 1][j - 1] &&
110  output[i][j] > output[i + 1][j] && output[i][j] > output[i + 1][j + 1])
111  ;
112  else
113  output[i][j] = 0;
114  }
115  }
116 }
117 
118 //////////////////////////////////////////////////////////////////////////////
119 void
120 pcl::keypoint::hessianBlob(ImageType& output,
121  ImageType& input,
122  const float sigma,
123  bool SCALED)
124 {
125  /*creating the gaussian kernels*/
126  ImageType kernel, cornerness;
127  conv_2d.gaussianKernel(5, sigma, kernel);
128 
129  /*scaling the image with differentiation scale*/
130  ImageType smoothed_image;
131  conv_2d.convolve(smoothed_image, kernel, input);
132 
133  /*image derivatives*/
134  ImageType I_x, I_y;
135  edge_detection.ComputeDerivativeXCentral(I_x, smoothed_image);
136  edge_detection.ComputeDerivativeYCentral(I_y, smoothed_image);
137 
138  /*second moment matrix*/
139  ImageType I_xx, I_yy, I_xy;
140  edge_detection.ComputeDerivativeXCentral(I_xx, I_x);
141  edge_detection.ComputeDerivativeYCentral(I_xy, I_x);
142  edge_detection.ComputeDerivativeYCentral(I_yy, I_y);
143  /*Determinant of Hessian*/
144  const std::size_t height = input.size();
145  const std::size_t width = input[0].size();
146  float min = std::numeric_limits<float>::max();
147  float max = std::numeric_limits<float>::min();
148  cornerness.resize(height);
149  for (std::size_t i = 0; i < height; i++) {
150  cornerness[i].resize(width);
151  for (std::size_t j = 0; j < width; j++) {
152  cornerness[i][j] =
153  sigma * sigma * (I_xx[i][j] + I_yy[i][j] - I_xy[i][j] * I_xy[i][j]);
154  if (SCALED) {
155  if (cornerness[i][j] < min)
156  min = cornerness[i][j];
157  if (cornerness[i][j] > max)
158  max = cornerness[i][j];
159  }
160  }
161 
162  /*local maxima*/
163  output.resize(height);
164  output[0].resize(width);
165  output[height - 1].resize(width);
166  for (std::size_t i = 1; i < height - 1; i++) {
167  output[i].resize(width);
168  for (std::size_t j = 1; j < width - 1; j++) {
169  if (SCALED)
170  output[i][j] = ((cornerness[i][j] - min) / (max - min));
171  else
172  output[i][j] = cornerness[i][j];
173  }
174  }
175  }
176 }
177 
178 //////////////////////////////////////////////////////////////////////////////
179 void
180 pcl::keypoint::hessianBlob(ImageType& output,
181  ImageType& input,
182  const float start_scale,
183  const float scaling_factor,
184  const int num_scales)
185 {
186  const std::size_t height = input.size();
187  const std::size_t width = input[0].size();
188  const int local_search_radius = 1;
189  float scale = start_scale;
190  std::vector<ImageType> cornerness;
191  cornerness.resize(num_scales);
192  for (int i = 0; i < num_scales; i++) {
193  hessianBlob(cornerness[i], input, scale, false);
194  scale *= scaling_factor;
195  }
196  bool non_max_flag = false;
197  float scale_max, local_max;
198  for (std::size_t i = 0; i < height; i++) {
199  for (std::size_t j = 0; j < width; j++) {
200  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  non_max_flag = false;
206  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][i] = start_scale * pow(scaling_factor, k);
235  }
236  }
237  }
238  }
239  }
240 }
241 
242 //////////////////////////////////////////////////////////////////////////////
243 void
244 pcl::keypoint::imageElementMultiply(ImageType& output,
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 
259 #endif // PCL_2D_KEYPOINT_HPP_