void
pcl::pcl_2d::edge::robertsXY (ImageType &Gx, ImageType &Gy, ImageType &input)
{
ImageType kernelX;
kernelX.resize (2); kernelX[0].resize (2); kernelX[1].resize (2);
kernelX[0][0] = 1; kernelX[0][1] = 0;
kernelX[1][0] = 0; kernelX[1][1] = -1;
conv_2d->convolve (Gx, kernelX, input);
ImageType kernelY;
kernelY.resize (2); kernelY[0].resize (2); kernelY[1].resize (2);
kernelY[0][0] = 0; kernelY[0][1] = 1;
kernelY[1][0] = -1; kernelY[1][1] = 0;
conv_2d->convolve (Gy, kernelY, input);
}
void
pcl::pcl_2d::convolution_2d::conv (ImageType &output, ImageType &kernel, ImageType &input){
int rows = input.size ();
int cols = input[0].size ();
int k_rows = kernel.size ();
int k_cols = kernel[0].size ();
/*default boundary option : zero padding*/
output.resize (input.size ());
for (int i = 0; i < rows; i++)
{
output[i].resize (cols);
for (int j = 0; j < cols; j++)
{
output[i][j] = 0;
for (int k = 0; k < k_rows; k++)
{
for (int l = 0; l < k_cols; l++)
{
if ((i + k - k_rows / 2) < 0 || (i + k - k_rows / 2) >= rows || (j + l - k_cols / 2) < 0 || (j + l - k_cols / 2) >= cols)
{
continue;
}
else
{
output[i][j] += kernel[k][l] * input[i + k - k_rows / 2][j + l - k_cols / 2];
}
}
}
}
}
}
void
pcl::pcl_2d::edge::ComputeDerivativeXCentral (ImageType &output, ImageType &input){
ImageType kernel;
kernel.resize (3);
kernel[0].resize (1); kernel[1].resize (1); kernel[2].resize (1);
kernel[0][0] = -1; kernel[1][0] = 0; kernel[2][0] = 1;
conv_2d->convolve (output, kernel, input);
}
void
pcl::pcl_2d::edge::robertsMagnitudeDirection (ImageType &G, ImageType &thet, ImageType &input){
ImageType Gx;
ImageType Gy;
robertsXY (Gx, Gy, input);
G.resize (input.size ());
thet.resize (input.size ());
for (int i = 0; i < input.size (); i++)
{
G[i].resize (input[i].size ());
thet[i].resize (input[i].size ());
for (int j = 0; j < input[i].size (); j++)
{
G[i][j] = sqrt (Gx[i][j] * Gx[i][j] + Gy[i][j] * Gy[i][j]);
thet[i][j] = atan2 (Gy[i][j], Gx[i][j]);
}
}
}
void
pcl::keypoint::hessianBlob (ImageType &output, ImageType &input, const float sigma, bool SCALED){
/*creating the gaussian kernels*/
ImageType kernel, cornerness;
conv_2d.gaussianKernel (5, sigma, kernel);
/*scaling the image with differentiation scale*/
ImageType smoothed_image;
conv_2d.convolve (smoothed_image, kernel, input);
/*image derivatives*/
ImageType I_x, I_y;
edge_detection.ComputeDerivativeXCentral (I_x, smoothed_image);
edge_detection.ComputeDerivativeYCentral (I_y, smoothed_image);
/*second moment matrix*/
ImageType I_xx, I_yy, I_xy;
edge_detection.ComputeDerivativeXCentral (I_xx, I_x);
edge_detection.ComputeDerivativeYCentral (I_xy, I_x);
edge_detection.ComputeDerivativeYCentral (I_yy, I_y);
/*Determinant of Hessian*/
const size_t height = input.size ();
const size_t width = input[0].size ();
float min = std::numeric_limits<float>::max();
float max = std::numeric_limits<float>::min();
cornerness.resize (height);
for (size_t i = 0; i < height; i++)
{
cornerness[i].resize (width);
for (size_t j = 0; j < width; j++)
{
cornerness[i][j] = sigma*sigma*(I_xx[i][j]+I_yy[i][j]-I_xy[i][j]*I_xy[i][j]);
if(SCALED){
if(cornerness[i][j] < min)
min = cornerness[i][j];
if(cornerness[i][j] > max)
max = cornerness[i][j];
}
}
/*local maxima*/
output.resize (height);
output[0].resize (width);
output[height-1].resize (width);
for (size_t i = 1; i < height - 1; i++)
{
output[i].resize (width);
for (size_t j = 1; j < width - 1; j++)
{
if(SCALED)
output[i][j] = ((cornerness[i][j]-min)/(max-min));
else
output[i][j] = cornerness[i][j];
}
}
}
}
void
pcl::keypoint::imageElementMultiply (ImageType &output, ImageType &input1, ImageType &input2){
const size_t height = input1.size ();
const size_t width = input1[0].size ();
output.resize (height);
for (size_t i = 0; i < height; i++)
{
output[i].resize (width);
for (size_t j = 0; j < width; j++)
{
output[i][j] = input1[i][j] * input2[i][j];
}
}
}
void
pcl::pcl_2d::convolution_2d::gaussianKernel (const int kernel_size, const float sigma, ImageType &kernel){
float sum = 0;
kernel.resize (kernel_size);
for (int i = 0; i < kernel_size; i++)
{
kernel[i].resize (kernel_size);
for (int j = 0; j < kernel_size; j++)
{
kernel[i][j] = exp (-(((i - kernel_size / 2) * (i - kernel_size / 2) + (j - kernel_size / 2) * (j - kernel_size / 2)) / (2 * sigma * sigma)));
sum += kernel[i][j];
}
}
/*normalizing the kernel*/
for (int i = 0; i < kernel_size; i++)
{
for (int j = 0; j < kernel_size; j++)
{
kernel[i][j] /= sum;
}
}
}
void
pcl::pcl_2d::edge::LoGKernel (ImageType &kernel, const int kernel_size, const float sigma)
{
float sum = 0;
float temp = 0;
kernel.resize (kernel_size);
for (int i = 0; i < kernel_size; i++)
{
kernel[i].resize (kernel_size);
for (int j = 0; j < kernel_size; j++)
{
temp = (((i - kernel_size / 2) * (i - kernel_size / 2) + (j - kernel_size / 2) * (j - kernel_size / 2)) / (2 * sigma * sigma));
kernel[i][j] = (1 - temp) * exp (-temp);
sum += kernel[i][j];
}
}
for (int i = 0; i < kernel_size; i++)
{
for (int j = 0; j < kernel_size; j++)
{
kernel[i][j] /= sum;
}
}
}
void
pcl::pcl_2d::edge::canny (ImageType &output, ImageType &input)
{
float tHigh = 50;
float tLow = 20;
const int height = input.size();
const int width = input[0].size();
/*noise reduction using gaussian blurring*/
ImageType gaussian_kernel;
conv_2d->gaussianKernel (5, 1.4, gaussian_kernel);
conv_2d->convolve (output, gaussian_kernel, input);
/*edge detection usign Sobel*/
ImageType G;
ImageType thet;
sobelMagnitudeDirection (G, thet, input);
/*edge discretization*/
float angle;
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
angle = (thet[i][j] / 3.14f) * 180;
if (((angle < 22.5) && (angle > -22.5)) || (angle > 157.5) || (angle < -157.5))
thet[i][j] = 0;
else
if (((angle > 22.5) && (angle < 67.5)) || ((angle < -112.5) && (angle > -157.5)))
thet[i][j] = 45;
else
if (((angle > 67.5) && (angle < 112.5)) || ((angle < -67.5) && (angle > -112.5)))
thet[i][j] = 90;
else
if (((angle > 112.5) && (angle < 157.5)) || ((angle < -22.5) && (angle > -67.5)))
thet[i][j] = 135;
}
}
float max;
/*tHigh and non-maximal supression*/
for (int i = 1; i < height - 1; i++)
{
for (int j = 1; j < width - 1; j++)
{
if (G[i][j] < tHigh)
continue;
max = G[i][j];
switch ((int)thet[i][j])
{
case 0:
if(G[i][j] < G[i][j-1] || G[i][j] < G[i][j+1])
G[i][j] = 0;
break;
case 45:
if(G[i][j] < G[i-1][j+1] || G[i][j] < G[i+1][j-1])
G[i][j] = 0;
break;
case 90:
if(G[i][j] < G[i-1][j] || G[i][j] < G[i+1][j])
G[i][j] = 0;
break;
case 135:
if(G[i][j] < G[i-1][j-1] || G[i][j] < G[i+1][j+1])
G[i][j] = 0;
break;
}
}
}
/*edge tracing*/
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
if (G[i][j] < tHigh)
continue;
switch ((int)thet[i][j])
{
case 0:
cannyTraceEdge (1, 0, i, j, 0, tLow, tHigh, G, thet);
cannyTraceEdge (-1, 0, i, j, 0, tLow, tHigh, G, thet);
break;
case 45:
cannyTraceEdge (1, 1, i, j, 45, tLow, tHigh, G, thet);
cannyTraceEdge (-1, -1, i, j, 45, tLow, tHigh, G, thet);
break;
case 90:
cannyTraceEdge (0, -1, i, j, 90, tLow, tHigh, G, thet);
cannyTraceEdge (0, 1, i, j, 90, tLow, tHigh, G, thet);
break;
case 135:
cannyTraceEdge (-1, 1, i, j, 135, tLow, tHigh, G, thet);
cannyTraceEdge (1, -1, i, j, 135, tLow, tHigh, G, thet);
break;
}
}
}
/*final thresholding*/
output.resize (height);
for (int i = 0; i < height; i++)
{
output[i].resize (width);
for (int j = 0; j < width; j++)
{
if (G[i][j] < tHigh)
output[i][j] = 0;
else
output[i][j] = 255;
}
}
}
void
pcl::keypoint::harrisCorner (ImageType &output, ImageType &input, const float sigma_d, const float sigma_i, const float alpha, const float thresh){
/*creating the gaussian kernels*/
ImageType kernel_d;
ImageType kernel_i;
conv_2d.gaussianKernel (5, sigma_d, kernel_d);
conv_2d.gaussianKernel (5, sigma_i, kernel_i);
/*scaling the image with differentiation scale*/
ImageType smoothed_image;
conv_2d.convolve (smoothed_image, kernel_d, input);
/*image derivatives*/
ImageType I_x, I_y;
edge_detection.ComputeDerivativeXCentral (I_x, smoothed_image);
edge_detection.ComputeDerivativeYCentral (I_y, smoothed_image);
/*second moment matrix*/
ImageType I_x2, I_y2, I_xI_y;
imageElementMultiply (I_x2, I_x, I_x);
imageElementMultiply (I_y2, I_y, I_y);
imageElementMultiply (I_xI_y, I_x, I_y);
/*scaling second moment matrix with integration scale*/
ImageType M00, M10, M11;
conv_2d.convolve (M00, kernel_i, I_x2);
conv_2d.convolve (M10, kernel_i, I_xI_y);
conv_2d.convolve (M11, kernel_i, I_y2);
/*harris function*/
const size_t height = input.size ();
const size_t width = input[0].size ();
output.resize (height);
for (size_t i = 0; i < height; i++)
{
output[i].resize (width);
for (size_t j = 0; j < width; j++)
{
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]));
if (thresh != 0)
{
if (output[i][j] < thresh)
output[i][j] = 0;
else
output[i][j] = 255;
}
}
}
/*local maxima*/
for (size_t i = 1; i < height - 1; i++)
{
for (size_t j = 1; j < width - 1; j++)
{
if (output[i][j] > output[i - 1][j - 1] && output[i][j] > output[i - 1][j] && output[i][j] > output[i - 1][j + 1] &&
output[i][j] > output[i][j - 1] && output[i][j] > output[i][j + 1] &&
output[i][j] > output[i + 1][j - 1] && output[i][j] > output[i + 1][j] && output[i][j] > output[i + 1][j + 1])
;
else
output[i][j] = 0;
}
}
}
void
pcl::pcl_2d::convolution_2d::conv (ImageType &output, ImageType &kernel, ImageType &input, BOUNDARY_OPTIONS_ENUM boundary_option){
int rows = input.size ();
int cols = input[0].size ();
int k_rows = kernel.size ();
int k_cols = kernel[0].size ();
int input_row = 0, input_col = 0;
output.resize (input.size ());
if (boundary_option == BOUNDARY_OPTION_CLAMP)
{
for (int i = 0; i < rows; i++)
{
output[i].resize (cols);
for (int j = 0; j < cols; j++)
{
output[i][j] = 0;
for (int k = 0; k < k_rows; k++)
{
for (int l = 0; l < k_cols; l++)
{
if ((i + k - k_rows / 2) < 0)
input_row = 0;
else
if ((i + k - k_rows / 2) >= rows)
{
input_row = rows - 1;
}
else
input_row = i + k - k_rows / 2;
if ((j + l - k_cols / 2) < 0)
input_col = 0;
else
if ((j + l - k_cols / 2) >= cols)
input_col = cols - 1;
else
input_col = j + l - k_cols / 2;
output[i][j] += kernel[k][l] * input[input_row][input_col];
}
}
}
}
}
else
if (boundary_option == BOUNDARY_OPTION_MIRROR)
{
for (int i = 0; i < rows; i++)
{
output[i].resize (cols);
for (int j = 0; j < cols; j++)
{
output[i][j] = 0;
for (int k = 0; k < k_rows; k++)
{
for (int l = 0; l < k_cols; l++)
{
if ((i + k - (k_rows / 2)) < 0)
input_row = -(i + k - (k_rows / 2));
else
if ((i + k - (k_rows / 2)) >= rows)
{
input_row = 2 * rows - 1 - (i + k - (k_rows / 2));
}
else
input_row = i + k - (k_rows / 2);
if ((j + l - (k_cols / 2)) < 0)
input_col = -(j + l - (k_cols / 2));
else
if ((j + l - (k_cols / 2)) >= cols)
input_col = 2 * cols - 1 - (j + l - (k_cols / 2));
else
input_col = j + l - (k_cols / 2);
output[i][j] += kernel[k][l] * input[input_row][input_col];
}
}
}
}
}
else
if (boundary_option == BOUNDARY_OPTION_ZERO_PADDING)
{
conv (output, kernel, input);
}
}
