%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% EE362 Final Project Winter 2006 
% Title: Distortion.m
% Written By: Jenny Hu
% Date: 3/19/06
%
% This code calculates the third order distortion coefficient
% from k1 = (ru - rd)/rd^3.  The inputed image should be resized
% for faster computation.  The input should be of a square grid.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Converts the color image into a black and white image.  The values are
% converted to double for computation purposes.

COLOR=imread('Input Image.jpg');
BW=rgb2gray(COLOR);
dimensions = size(BW)
ratio = dimensions(2)/dimensions(1);
BW=imresize(BW, [750 round(750*ratio)]);
figure,imshow(BW);

BW2=double(BW);
dimensions=size(BW);
number_of_rows = dimensions(1);
number_of_col = dimensions(2);

% Uses the edge function to determine the location of the line
% boundaries.
 image_edges=edge(BW2,'sobel');
 figure, imshow(image_edges)

% All the pixels are scanned to find the points that are local maxima.
% These are possible cross points.
index_possible_cross_points = 1;
possible_cross_points=[];
for row=6:number_of_rows-6;
    for col=6:number_of_col-6;
        A=max(max(image_edges(row-1:row+1,col+5:col+5)));
        B=max(max(image_edges(row-1:row+1,col-5:col-5)));
        C=max(max(image_edges(row+5:row+5,col-1:col+1)));
        D=max(max(image_edges(row-5:row-5,col-1:col+1)));
        if (A&B&C&D&(image_edges(row,col)==0))
            possible_cross_points(index_possible_cross_points,1)=row;
            possible_cross_points(index_possible_cross_points,2)=col;
            index_possible_cross_points = index_possible_cross_points +1;
        else
            index_possible_cross_points = index_possible_cross_points + 0;
        end
    end
end
figure, plot(possible_cross_points(:,2), possible_cross_points(:,1),'.') 

% Finds the locations where the location of cross points jumps to a new row
% group, or where there is a large difference in row values.
difference_loc=[];
difference_loc(1)=0; index_difference = 2;
for i=1:index_possible_cross_points - 2;
    difference_row=possible_cross_points(i+1,1)-possible_cross_points(i,1);
    if(difference_row > 1)
        difference_row_loc(index_difference) = i;
        index_difference = index_difference + 1;
    elseif i == index_possible_cross_points - 2;    
        difference_row_loc(index_difference) = i;
    else
        index_difference = index_difference + 0;
    end
end

% Separates the row groups into a structure.  An average is taken to obtain
% a single cross point value.
cross_points = [];
index_cross = 1;
distorted = struct('cross_points',{})
separate = struct('data',{})
for i = 1:index_difference-1;
    start_group = difference_row_loc(i)+1;
    end_group = difference_row_loc(i+1);
    separate(i).data=possible_cross_points(start_group:end_group,:);
    separate(i).data=sortrows(separate(i).data,2);
    length = size(separate(i).data);
    difference_col_loc = []; difference_col_loc(1) = 0;
    index_col = 2; 
    for m = 1:length(1)-1
        difference_col = separate(i).data(m+1,2) - separate(i).data(m,2);
        if ((difference_col >= 5) || (m == length(1)-1))
            difference_col_loc(index_col) = m;
            index_col = index_col + 1;
        else
            index_col = index_col + 0;
        end
    end
    for n = 1:index_col-2
       before = difference_col_loc(n)+1;
       after = difference_col_loc(n+1);
       distorted(i).cross_points(n,1) = mean(separate(i).data(before:after,1));
       distorted(i).cross_points(n,2) = mean(separate(i).data(before:after,2));
       cross_points(index_cross,1) = mean(separate(i).data(before:after,1));
       cross_points(index_cross,2) = mean(separate(i).data(before:after,2));
       index_cross = index_cross + 1;
    end    
end
figure, plot(cross_points(:,1),cross_points(:,2),'.')
grid on; 

% Finds the center of the cross points by first finding the center row
% group in the structure.  Then, within that row group, the column values
% are sorted to find the center point.
length = size(distorted);
number_of_row_groups = length(2);
center=round(number_of_row_groups/2); % Y direction
i=0
length = size(distorted(center).cross_points);
temp = sortrows(distorted(center).cross_points,2);
center_col = round(length(1)/2);
center_cross_point = temp(center_col,:);  % (Y,X)

% Finds the column spacing from the average spacing between the two nearest
% column neighbors of the center point.
spacing_col_1 = abs(temp(center_col,2) - temp(center_col + 1,2));
spacing_col_2 = abs(temp(center_col,2) - temp(center_col - 1,2));
spacing_col = (spacing_col_1 + spacing_col_2)/2

% Finds the row spacing from the average spacing between the nearest row
% neighbors of the center point.
temp = sortrows(cross_points,2);
center_row = find(temp(:,2) == center_cross_point(2));
center_row = center_row(1);
spacing_row_1 = abs(temp(center_row,1) - temp(center_row + 1,1));
spacing_row_2 = abs(temp(center_row,1) - temp(center_row - 1,1));
spacing_row = (spacing_row_1 + spacing_row_2)/2

% Finds the location of expected undistorted cross points.
number_of_cross_points = size(cross_points);
number_of_cross_points = number_of_cross_points(1);
undistorted=[]; ratio = [];
center_point = ones(number_of_cross_points,2);
center_point(:,1) = ones(number_of_cross_points,1)*center_cross_point(1);
center_point(:,2) = ones(number_of_cross_points,1)*center_cross_point(2);
spacing = ones(number_of_cross_points,2);
spacing(:,1) = ones(number_of_cross_points,1)*spacing_row;
spacing(:,2) = ones(number_of_cross_points,1)*spacing_col;
ratio = round(( cross_points - center_point)./ spacing );
undistorted = center_point + ratio.*spacing;
Ru = (undistorted - center_point*1.0000001).^2;
Ru = sqrt(Ru(:,1) + Ru(:,2));
Rd = (cross_points - center_point*1.0000001).^2;
Rd = sqrt(Rd(:,1) + Rd(:,2));
normalization_factor = max(Rd);
Rd_norm = Rd/normalization_factor;
Ru_norm = Ru/normalization_factor;

figure, plot(undistorted(:,2), undistorted(:,1),'r.')
hold on, plot(cross_points(:,2), cross_points(:,1),'b.')
hold on, plot(center_cross_point(2), center_cross_point(1),'+')
xlim([0 number_of_col*1.02])
ylim([0 number_of_rows*1.2])
legend('Undistorted','Distorted',0)
xlabel('Pixel Number')
ylabel('Pixel Number')

third_order_k = mean((Ru_norm - Rd_norm)./(Rd_norm.^3))
str = {['Third Order k : ',num2str(third_order_k)]};
text(0.05*number_of_col, 1.1*number_of_rows, str, 'BackgroundColor', [0.85 0.85 0.85]);
title('SD500 Picture 003.jpg Distortion')