Image mosaicing and Homography Matrix

I have written the following code so far to determine the best Homography matrix, however i have not been successful in proceeding further , as my stiched images using homography Matrix are not as desired and fairly distorted,

I am not sure, if the Homography Matrix calculated is correct or not,

figure
subplot(1,2,1);
imshow('img1.pgm');
img1 = imread('img1.pgm');

[rows1, col1] = size(img1);
subplot(1,2,2);
imshow('img2.pgm');
img2 = imread('img2.pgm');
[rows2, col2] = size(img2);


[corresp1, corresp2] = sift_corresp('img1.pgm','img2.pgm');

% The check the no of point correspondances obtained

[r,~] = size(corresp1);

global xfactorHomo;

xfactorHomo = zeros(3,3);

global xfactor;

xfactor = 0; 

% to determine the no of 04 set of point correspondances available

r0 = floor(r/4);

% The main loop where all the action will take place


for i=1:4:r0

% Assigning the 04 point correspondances for calculating the Homography
% Matrix


x1 = corresp1(i,1);
y1 = corresp1(i,2);
X1 = corresp2(i,1);
Y1 = corresp2(i,2);

x2 = corresp1(i+1,1);
y2 = corresp1(i+1,2);
X2 = corresp2(i+1,1);
Y2 = corresp2(i+1,2);

x3 = corresp1(i+2,1);
y3 = corresp1(i+2,2);
X3 = corresp2(i+2,1);
Y3 = corresp2(i+2,2);

x4 = corresp1(i+3,1);
y4 = corresp1(i+3,2);
X4 = corresp2(i+3,1);
Y4 = corresp2(i+3,2);

% Constructing the A Matrix for calculating Homopgraphy

A =  [-X1 -Y1 -1  0  0  0  x1.*X1  x1.*Y1  x1 ;

0  0  0  -X1  -Y1  -1  y1.*X1   y1.*Y1  y1;

-X2 -Y2 -1  0  0  0  x2.*X2  x2.*Y2  x2 ;

0  0  0  -X2  -Y2  -1  y2.*X2   y2.*Y2  y2;

-X3 -Y3 -1  0  0  0  x3.*X3  x3.*Y3  x3 ;

0  0  0  -X3  -Y3  -1  y3.*X3   y3.*Y3  y3;

-X4 -Y4 -1  0  0  0  x4.*X4  x4.*Y4  x4;

0  0  0  -X4  -Y4  -1  y4.*X4   y4.*Y4  y4];

% Solving for homograhy using SVD

[~,~,V] = svd(A); 

xtemp = V(8*9+1:9*9);
x = xtemp/ xtemp(9);
H = [x(1) x(2) x(3); x(4) x(5) x(6); x(7) x(8) x(9) ] ;

H0 = V(:,end);

H0 = reshape(H,3,3);

% fprintf('Estimated homography matrix is:')

 %display(H);
 %display(H0);


iRansac(H,i,r,corresp1,corresp2); 

end


disp(xfactorHomo);
H = xfactorHomo ;
% Done with Homography , now to create Mosaic

stitchedImage = img2;
stitchedImage = padarray(stitchedImage, [0 size(img1, 2)], 0, 'post');
stitchedImage = padarray(stitchedImage, [size(img1, 1) 0], 0, 'both');
for i = 1:size(stitchedImage, 2)
    for j = 1:size(stitchedImage, 1)
        p2 = H * [i; j-floor(size(img1, 1)); 1];
        p2 = p2 ./ p2(3);
        x2 = floor(p2(1));
        y2 = floor(p2(2));
        if x2 > 0 && x2 <= size(img1, 2) && y2 > 0 && y2 <= size(img1, 1)
            stitchedImage(j, i) = img1(y2, x2);
        end
    end
end
%crop
[row,col] = find(stitchedImage);
c = max(col(:));
d = max(row(:));
st=imcrop(stitchedImage, [1 1 c d]);
[row,col] = find(stitchedImage ~= 0);
a = min(col(:));
b = min(row(:));
st=imcrop(st, [a b size(st,1) size(st,2)]);
stitchedImage = st;

figure, imshow(stitchedImage);



% function which will do the job of RANSAC

function Homo = iRansac(H,i1,r0,corresp1,corresp2) 

% Defining counter variable for inliers and outliers

inliers = 0;
outliers = 0;


global xfactorHomo;
global xfactor;

% the main loop for all other correspondances will be checked for validity
% of Homography Matrix 


for j=1:r0

    if ((j~=i1)||(j~=i1+1)||(j~=i1+2)||(j~=i1+3))
        
        originalpoint = ones(3,1);
        originalpoint(1,1) = corresp2(j,1);
        originalpoint(2,1) = corresp2(j,2);
        
        H_point = ones(3,1);
        
        H_point = H*originalpoint ;
        
        % to normalize Z'' 
        
        %disp(j);
        %disp(i1);
        
        H_point(1,1) = H_point(1,1)./ H_point(3,1);
        H_point(2,1) = H_point(2,1)./ H_point(3,1);
        H_point(3,1) = H_point(3,1)./ H_point(3,1);
        
        % Main equation for finding euclidean dist between actual and
        % homopgrahed correspondances
        
        D = sqrt(((H_point(1,1) - corresp1(j,1)).^2 )+((H_point(2,1) - corresp1(j,2)).^2));

        
        % disp(D1);
        
        % 10 has been chosen as reference value
        
        if (D < 6)
            inliers = inliers + 1 ;
           % fprintf('Inliers');
             %disp(inliers);
        else
            outliers = outliers + 1 ;
            % fprintf('Outliers');
           % disp(outliers);
        end
    end
    
end

% The main loop where final Homopgrahic matrix will be chosen

if (xfactor < inliers) 
    
    xfactorHomo = H;
    xfactor = inliers ; 
    Homo = xfactorHomo;
    
end
end