clear all 
close all
% STEP 1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Reading the images for base and input
rawPath = 'C:\Documents and Settings\Chris Paulson\Desktop\End of the year program';

startFrame = 104001;
endFrame = 104002;
numCamera = 0;
imHeight = 2672;
imWidth = 4016;
numPix = imHeight*imWidth;
cmap = gray(256); % 8-bit colormap


for frame = startFrame:endFrame
   
    
   % generate filenames
   rawFilename = strcat(repmat('0', 1, 5), int2str(numCamera), '-', ...
      repmat('0', 1, 6-length(num2str(frame))), int2str(frame), '.raw'); 

 for   camera = numCamera
      
      % read camera frame
      fid = fopen(fullfile(rawPath, rawFilename));
      if( fid ~= -1 )
         raw = fread(fid, [imWidth imHeight], 'uint8=>uint8')';
         fclose(fid);
         if( prod(size(raw)) ~= numPix )
            raw = zeros(imHeight, imWidth, 'uint8');
         end
      else
         raw = zeros(imHeight, imWidth, 'uint8');
      end
      

           rot90Direction = -1;

      raw = fliplr(rot90(raw, rot90Direction));
      

 
 end
     if frame==startFrame
         x=raw;  % base image
     else
         xx=raw; %input image
     end
end

% %  figure, imshow(x), title('base image'), axis image;
% %  figure, imshow(xx), title('input image'), axis image;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%STEP 2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
sigma = 1;
ws = 101;
hs = floor(ws/2);
xind = 1;
yind = 1;
GF = zeros(5);

for y = -hs:hs
    for z = -hs:hs
        GF(yind,xind) = 1/(2*pi*sigma^2)*exp(-((z)^2+(y)^2)/(2*sigma^2));
        xind = xind + 1;
    end
    xind = 1;
    yind = yind + 1;
end

x = filter2(GF,x);
xx = filter2(GF,xx);
%Applying Wavelet transformation for base and input images
 [c1,s1]=wavedec2(x,4,'db1');
 a4=appcoef2(c1,s1,'db1',4); %Extract approximate coefficients
% %  figure, imshow(mat2gray(a4)), title('base approximate coefficients')
% %  axis image;
 %This image will be 4 times smaller than original image.  The purpose of
 %this is to do all the calculations in the smaller image so the algorithm
 %will have less computations to perform.


 [c2,s2]=wavedec2(xx,4,'db1');
 aa4=appcoef2(c2,s2,'db1',4); %Extract approximate coefficients
% %  figure, imshow(mat2gray(aa4)), title('input approximate coefficients') 
% %  axis image;
 %This image will be 4 times smaller than original image.
 
 % Room for improvement for future work!!
 %Apply varies wavelet filters such as biorthogonal and different levels
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
%STEP 3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Calculates the gradient of the base image in both the x and y direction
[Gx Gy]=gradient(a4);
Gx=abs(Gx);
Gy=abs(Gy);
% % figure, imshow(mat2gray(Gx)), title('Base Gradient in x direction');
% % figure, imshow(mat2gray(Gy)),title('Base Gradient in y direction');

%Calculates the gradient of the input image in both the x and y direction
[Gx2 Gy2]=gradient(double(aa4));
Gx2=abs(Gx2);
Gy2=abs(Gy2);

% % figure, imshow(mat2gray(Gx2)), title('Input Gradient in x direction');
% % figure, imshow(mat2gray(Gy2)), title('Input Gradient in y direction');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%STEP 4
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Finding the maximum gradient in both the x and y direction in the base
%image.  The purpose of this is to find possible good feature points.  The
%reason why the algorithm looks for the maximum value is because that
%should be an edge point of an object.
% % iptsetpref ( 'ImshowAxesVisible',  'on')
% % figure, imshow(mat2gray(a4)), title('BASE MAX GRADIENT X DIRECTION')
% % hold on;
for i = 1:s1(1,1)
    [CC,II]=max(Gx(i,:));
    Rcp1(i,2)=i;
    Rcp1(i,3)=II;
    
    Rcp1(i,1)=round(CC);
end    
% % plot(Rcp1(:,3),Rcp1(:,2),'x','LineWidth',2,'Color','red');         
% % hold off; 

% % figure, imshow(mat2gray(aa4)), title('INPUT MAX GRADIENT X DIRECTION')
% % hold on;
for i = 1:s2(1,1)
    [CCC,III]=max(Gx2(i,:));
    Rcp2(i,2)=i;
    Rcp2(i,3)=III;
    
    Rcp2(i,1)=round(CCC);
end   
% % plot(Rcp2(:,3),Rcp2(:,2),'x','LineWidth',2,'Color','green');
% % hold off;

%Finding the maximum gradient in both the x and y direction in the input
%image.
% % iptsetpref ( 'ImshowAxesVisible',  'on')
% % figure, imshow(mat2gray(a4)), title('BASE MAX GRADIENT Y DIRECTION')
% % hold on;
for i = 1:s1(1,1)
    [CC,II]=max(Gy(i,:));
    Rcp1y(i,2)=i;
    Rcp1y(i,3)=II;
    
    Rcp1y(i,1)=round(CC);
end  
% % plot(Rcp1y(:,3),Rcp1y(:,2),'x','LineWidth',2,'Color','red');         
% % hold off; 
% % 
% % 
% % figure, imshow(mat2gray(aa4)), title('INPUT MAX GRADIENT Y DIRECTION')
% % hold on;
for i = 1:s2(1,1)
    [CCC,III]=max(Gy2(i,:));
    Rcp2y(i,2)=i;
    Rcp2y(i,3)=III;
    
    Rcp2y(i,1)=round(CCC);
end
% % plot(Rcp2y(:,3),Rcp2y(:,2),'x','LineWidth',2,'Color','green');
% % hold off;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%STEP 5
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Combines both maximum gradient of the x and y direction of the base image
%onto one image.
diffxy = sum(Rcp1(:,2:3)-Rcp1y(:,2:3),2);
indxy = find(diffxy);
baseRcp = [Rcp1;Rcp1y(indxy,:)];
% % figure, imshow(mat2gray(a4)), title('Base Control Pts')
% % hold on;               
% % plot(baseRcp(:,3),baseRcp(:,2),'x','LineWidth',2,'Color','red');
% % hold off;

%Combines both maximum gradient of the x and y direction of the input image
%onto one image.
diffxy = sum(Rcp2(:,2:3)-Rcp2y(:,2:3),2);
indxy = find(diffxy);
inputRcp = [Rcp2;Rcp2y(indxy,:)];
% % figure, imshow(mat2gray(aa4)), title('Input Control Pts')
% % hold on;               
% % plot(inputRcp(:,3),inputRcp(:,2),'x','LineWidth',2,'Color','green');
% % hold off;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%STEP 6
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%This reconstruct the smaller base and input energy images into the original
%image size.
[br bc] = size(a4);
newrgxb = zeros(br,bc);
newrgxi = zeros(br,bc);

%This finds the wavelet coefficients in base image that corresponds to the
%location of the control points.  So newrgxb will be zero everywhere except
%the where the control points are located and the value that is in the
%matrix are the wavelet coefficients.
for j=1:size(baseRcp,1)
    indxb = baseRcp(j,2:3);
    newrgxb(indxb(1,1),indxb(1,2))=a4(indxb(1,1),indxb(1,2));
end

%This finds the wavelet coefficients in base image that corresponds to the
%location of the control points.  So newrgxi will be zero everywhere except
%the where the control points are located and the value that is in the
%matrix are the wavelet coefficients.
for j=1:size(inputRcp,1)
    indxi = inputRcp(j,2:3);
    newrgxi(indxi(1,1),indxi(1,2))=aa4(indxi(1,1),indxi(1,2));
end

% This transform the matrix into a row vector because waverec2 only deals
% with a row vector and not a matrix.
colnewrgxb=reshape(newrgxb, 1, size(newrgxb,1)*size(newrgxb,2));
colnewrgxi=reshape(newrgxi, 1, size(newrgxi,1)*size(newrgxi,2));

% The base energy image is now being reconstructed to the original size.
newc1=c1;
newc1(1:size(newrgxb,1)*size(newrgxb,2))=colnewrgxb; 
newc1(size(newrgxb,1)*size(newrgxb,2)+1:length(c1))=0;
XI = waverec2(newc1,s1,'db1');
% % figure, imshow(mat2gray(XI)), title('base energy');

% The input energy image is now being reconstructed to the original size.
newc2=c2;
newc2(1:size(newrgxi,1)*size(newrgxi,2))=colnewrgxi; 
newc2(size(newrgxi,1)*size(newrgxi,2)+1:length(c2))=0;
XXI = waverec2(newc2,s2,'db1');
% % figure, imshow(mat2gray(XXI)), title('input energy');

clear newc1 newc2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%STEP 7
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Mapping between the energy images and actual images, so finds the
% location of energy points and maps the point onto the actual image.
[rowcpt colcpt] = find(XI);
loccpt = [rowcpt colcpt];
[rowcpti colcpti] = find(XXI);
loccpti = [rowcpti colcpti];

% % iptsetpref ( 'ImshowAxesVisible',  'on')
% % figure, imshow(mat2gray(x)), title('Base Control Pts')
% % hold on;               
% % plot(loccpt(:,2),loccpt(:,1),'x','LineWidth',2,'Color','red');
% % hold off;
% % 
% % iptsetpref ( 'ImshowAxesVisible',  'on')
% % figure, imshow(mat2gray(xx)), title('Input Control Pts')
% % hold on;               
% % plot(loccpti(:,2),loccpti(:,1),'x','LineWidth',2,'Color','green');
% % hold off;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%STEP 8
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Since one point in the smaller image equals 16 points in the larger
% image, the algorithm needs to filter out all the unnecessary points and
% only picks the points that lie on the edges of the object.

% The window size is used to make sure that the algorithm does not use
% point on the edges of the image or else the algoritm will fail because
% the algorithm will go outside the actual size of the image.
fwindsize = 9; 
halffws = floor(fwindsize/2);
lev =4;
numpts = 10;
% findedge is a function that was created to find the edges of the object.
% This finds the edges of the base image.
bedgepts=findedge(baseRcp(:,2:3),x,numpts,lev);
[bedger bedgec] = find(bedgepts(:,1) > halffws & bedgepts(:,2) > halffws);
bedgepts=bedgepts(bedger,:);
% % iptsetpref ( 'ImshowAxesVisible',  'on')
% % figure, imshow(mat2gray(x)), title('revised Base Control Pts')
% % hold on;               
% % plot(bedgepts(:,2),bedgepts(:,1),'x','LineWidth',2,'Color','red');
% % hold off;

% This finds the edges of the input image.
iedgepts=findedge(inputRcp(:,2:3),xx,numpts,lev);
[iedger iedgec] = find(iedgepts(:,1) > halffws & iedgepts(:,2) > halffws);
iedgepts=iedgepts(iedger,:);
% % iptsetpref ( 'ImshowAxesVisible',  'on')
% % figure, imshow(mat2gray(xx)), title('revised Input Control Pts')
% % hold on;               
% % plot(iedgepts(:,2),iedgepts(:,1),'x','LineWidth',2,'Color','green');
% % hold off;   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%STEP 9
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Filters the corresponding points by using a threshold, so if the
% correspondence is below thresh then the algorithm will not use that
% point.  The algorithm take one point from the base image and tries to
% find the best correspondence to the input image.  The algorithm
% calculates the correspondence by using a function created called
% correspond.
iwindsize = 500;
hiwinsize =  iwindsize/2;
thrs = .995;

bedgepts = bedgepts(find(bedgepts(:,1) > halffws & ...
    bedgepts(:,1) < imWidth - halffws),:);

bedgepts = bedgepts(find(bedgepts(:,2)> halffws & ...
    bedgepts(:,2) < imHeight - halffws),:);

iedgepts = iedgepts(find(iedgepts(:,1) > halffws & ...
    iedgepts(:,1) < imWidth - halffws),:);

iedgepts = iedgepts(find(iedgepts(:,2)> halffws & ...
    iedgepts(:,2) < imHeight - halffws),:);

corrcntr=1;
for bind=1:size(bedgepts,1)
    bcoor = bedgepts(bind,:);
    bmat = double(x(bcoor(1,1)-halffws:bcoor(1,1)+halffws,bcoor(1,2)-halffws:bcoor(1,2)+halffws));
    if bcoor(1,1) <= hiwinsize
        winind = find(iedgepts(:,1) <= iwindsize);
    elseif bcoor(1,1) >= imWidth-hiwinsize
        winind = find(iedgepts(:,1) >= imWidth-iwindsize);
    else
        winind = find(iedgepts(:,1) >= bcoor(1,1)-hiwinsize & iedgepts(:,1) <= bcoor(1,1)+hiwinsize);
    end

    wind = iedgepts(winind,:);

    if bcoor(1,2) <= hiwinsize
        winind = find(wind(:,2) <= iwindsize);
    elseif bcoor(1,2) >= imHeight-hiwinsize
        winind = find(wind(:,2) >= imHeight-iwindsize);
    else
        winind = find(wind(:,2) >= bcoor(1,2)-hiwinsize & wind(:,2) <= bcoor(1,2)+hiwinsize);
    end

    wind = wind(winind,:);
    if size(wind,1)~=0
        corrpts = correspond(bmat,wind,thrs,xx,halffws,lev); %This is the function that was created.
        if sum(corrpts) ~= 0
            ifinalcorpts(corrcntr,:)=corrpts;
            bfinalcorpts(corrcntr,:)=bcoor;
            corrcntr=corrcntr+1;
        end
    end
end

% % iptsetpref ( 'ImshowAxesVisible',  'on')
% % figure, imshow(mat2gray(x)), title('Correspondence Base Control Pts')
% % hold on;               
% % plot(bfinalcorpts(:,2),bfinalcorpts(:,1),'x','LineWidth',2,'Color','red');
% % hold off;
% % 
% % iptsetpref ( 'ImshowAxesVisible',  'on')
% % figure, imshow(mat2gray(xx)), title('Correspondence Input Control Pts')
% % hold on;               
% % plot(ifinalcorpts(:,2),ifinalcorpts(:,1),'x','LineWidth',2,'Color','green');
% % hold off;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%STEP 10
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Here we use RANSAC to improve the corresponding points to make sure that
% algorithm selects the best points possible.
finxb=[bfinalcorpts(:,2) bfinalcorpts(:,1)];
finxi=[ifinalcorpts(:,2) ifinalcorpts(:,1)];
[T,best_consensus,best_error] = GetTransformRANSAC( finxb', finxi', 1 );

ranfinxb=finxb(best_consensus',:);
ranfinxi=finxi(best_consensus',:);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%STEP 11
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Now Registration is being performed on the image.
Ttype='projective'; 

BP=ranfinxb;
IP=ranfinxi;
t=cp2tform(IP, BP,  Ttype);
   
 tt=imtransform(xx, t,'bilinear','XData',[1 size(x,2)], 'YData',[1 size(x,1)] );
 figure, imshow(mat2gray(tt)), title('REGISTERED IMAGE by PROJECTIVE')

 ttt=imtransform(xx, t, 'bilinear','XData',[1 size(x,2)], 'YData',[1 size(x,1)], 'FillValues',255 );
            iptsetpref ( 'ImshowAxesVisible',  'on')
            figure;
            imshow(mat2gray(ttt));
            hold on;
            hh=imshow(x,gray(256));
            set(hh, 'AlphaData',0.6);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%STEP 12
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculating the RMSE and PSNR to see how well the algorithm performed.
MM=size(ttt);
Sum=0;
rmscnt=1;
for ii=1:MM(1,1);
    for jj=1:MM(1,2);
        if ttt(ii,jj)<255
            Sum=Sum+(abs(double(x(ii,jj))-double(tt(ii,jj))))^2;
            rmscnt=rmscnt+1;
        end
    end   
end
MSE= Sum/(rmscnt)
RMSE= sqrt(MSE)
PSNR= 10* log10(255^2/MSE)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%