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); } }