42 #ifndef PCL_2D_KEYPOINT_HPP_
43 #define PCL_2D_KEYPOINT_HPP_
45 #include <pcl/2d/edge.h>
46 #include <pcl/2d/convolution.h>
51 pcl::keypoint::harrisCorner (ImageType &output, ImageType &input,
const float sigma_d,
const float sigma_i,
const float alpha,
const float thresh){
56 conv_2d.gaussianKernel (5, sigma_d, kernel_d);
57 conv_2d.gaussianKernel (5, sigma_i, kernel_i);
60 ImageType smoothed_image;
61 conv_2d.convolve (smoothed_image, kernel_d, input);
65 edge_detection.ComputeDerivativeXCentral (I_x, smoothed_image);
66 edge_detection.ComputeDerivativeYCentral (I_y, smoothed_image);
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);
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);
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++)
86 output[i].resize (width);
87 for (
size_t j = 0; j < width; j++)
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]));
92 if (output[i][j] < thresh)
101 for (
size_t i = 1; i < height - 1; i++)
103 for (
size_t j = 1; j < width - 1; j++)
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])
117 pcl::keypoint::hessianBlob (ImageType &output, ImageType &input,
const float sigma,
bool SCALED){
119 ImageType kernel, cornerness;
120 conv_2d.gaussianKernel (5, sigma, kernel);
123 ImageType smoothed_image;
124 conv_2d.convolve (smoothed_image, kernel, input);
128 edge_detection.ComputeDerivativeXCentral (I_x, smoothed_image);
129 edge_detection.ComputeDerivativeYCentral (I_y, smoothed_image);
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);
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++)
144 cornerness[i].resize (width);
145 for (
size_t j = 0; j < width; j++)
147 cornerness[i][j] = sigma*sigma*(I_xx[i][j]+I_yy[i][j]-I_xy[i][j]*I_xy[i][j]);
149 if(cornerness[i][j] < min)
150 min = cornerness[i][j];
151 if(cornerness[i][j] > max)
152 max = cornerness[i][j];
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++)
162 output[i].resize (width);
163 for (
size_t j = 1; j < width - 1; j++)
166 output[i][j] = ((cornerness[i][j]-min)/(max-min));
168 output[i][j] = cornerness[i][j];
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;
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();
194 for(
int k = 0;k < num_scales;k++){
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)
201 for(
int l = -local_search_radius;l <= local_search_radius;l++){
202 if(l+i < 0 || l+i >= height)
204 for(
int m = -local_search_radius; m <= local_search_radius;m++){
205 if(m+j < 0 || m+j >= width)
207 if(cornerness[n+k][l+i][m+j] > local_max){
220 if(cornerness[k][i][j] > scale_max){
221 scale_max = cornerness[k][i][j];
223 output[i][i] = start_scale*pow(scaling_factor, k);
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++)
239 output[i].resize (width);
240 for (
size_t j = 0; j < width; j++)
242 output[i][j] = input1[i][j] * input2[i][j];
247 #endif // PCL_2D_KEYPOINT_HPP_