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