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::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::hessianBlob (ImageType &output, ImageType &input, const float start_scale, const float scaling_factor, const int num_scales){
const size_t height = input.size();
const size_t width = input[0].size();
const int local_search_radius = 1;
float scale = start_scale;
std::vector<ImageType> cornerness;
cornerness.resize(num_scales);
for(int i = 0;i < num_scales;i++){
hessianBlob(cornerness[i], input, scale, false);
scale *= scaling_factor;
}
bool non_max_flag = false;
float scale_max, local_max;
for(size_t i = 0;i < height;i++){
for(size_t j = 0;j < width;j++){
scale_max = std::numeric_limits<float>::min();
/*default output in case of no blob at the current point is 0*/
output[i][j] = 0;
for(int k = 0;k < num_scales;k++){
/*check if the current point (k,i,j) is a maximum in the defined search radius*/
non_max_flag = false;
local_max = cornerness[k][i][j];
for(int n = -local_search_radius; n <= local_search_radius;n++){
if(n+k < 0 || n+k >= num_scales)
continue;
for(int l = -local_search_radius;l <= local_search_radius;l++){
if(l+i < 0 || l+i >= height)
continue;
for(int m = -local_search_radius; m <= local_search_radius;m++){
if(m+j < 0 || m+j >= width)
continue;
if(cornerness[n+k][l+i][m+j] > local_max){
non_max_flag = true;
break;
}
}
if(non_max_flag)
break;
}
if(non_max_flag)
break;
}
/*if the current point is a point of local maximum, check if it is a maximum point across scales*/
if(!non_max_flag){
if(cornerness[k][i][j] > scale_max){
scale_max = cornerness[k][i][j];
/*output indicates the scale at which the blob is found at the current location in the image*/
output[i][i] = start_scale*pow(scaling_factor, k);
}
}
}
}
}
}
void
pcl::pcl_2d::edge::cannyTraceEdge (int rowOffset, int colOffset, int row, int col, float theta, float tLow, float tHigh, ImageType &G, ImageType &thet){
int newRow = row + rowOffset;
int newCol = col + colOffset;
if (newRow > 0 && newRow < G.size () && newCol > 0 && newCol < G[0].size ())
{
if (G[newRow][newCol] < tLow || G[newRow][newCol] > tHigh || thet[newRow][newCol] != theta)
return;
G[newRow][newCol] = tHigh + 1;
cannyTraceEdge (rowOffset,colOffset, newRow, newCol, theta, tLow, tHigh, G, thet);
}
}
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::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);
}
}
