function [I] = panoramaNDLTN_10(varargin)
%panorama normalized DLT, n input, up to 
%10 correspondences between any two images.
%Given some number of image filenames.
%Images in jpg format.
%Displays two images at a time and collects
%point correspondences from the user.
%The first image input is the reference, and
%the panorama is the reference after that.
%reference: Hartley Zisserman p. 92.
disp ('Select a point on the second image, and a corresponding point on the first.');
disp ('Continue to select corresponding points, one at a time. Press return when done.');
disp ('The result panorama will be displayed.');
disp ('Then select points from the third image and the panorama as before.');
disp ('Continue until tired.  When all inputs have been registered, the program');
disp ('will return the resultant panorama image.');

%if nargin < 2
%    error('too few arguments');
%else
     n = nargin;
%end

iptsetpref 'ImshowAxesVisible' on;
h = zeros(n);
I = zeros(700,700,3);

    CI = imread(sprintf('%s',varargin{1}),'jpg');
    dim = size(CI);
    I(1:dim(1),1:dim(2),:) = CI;
    h(1) = figure('Name',varargin{1}); 
    imshow(varargin{1}, 'truesize');
    I = I./255;


for cur_im = 2:n

%Load next image.
disp(sprintf('%s points', varargin{cur_im}));
CI = imread(sprintf('%s',varargin{cur_im}),'jpg');
cdim = size(CI);
Cur_Image = zeros(cdim(1),cdim(2),3);
Cur_Image(:,:,:) = CI;

%display it.
h(cur_im) = figure('Name',varargin{cur_im});
imshow(CI, 'truesize');

%select points in this image.
[xs,ys] = ginput(10);    %x's
numxs = size(xs);
for i = 1:numxs(1)
    disp(sprintf('(%f , %f)', xs(i), ys(i)));
end

%and select points on the reference image.
disp('Reference image points.');
figure(h(1));
[xpp,ypp] = ginput(numxs(1));  %x primes.
for i = 1:numxs(1)
    disp(sprintf('(%f , %f)', xpp(i), ypp(i)));
end

%xpp = [150 250 250 150]';
%ypp = [100 100 200 200]';
%xs = [100 200 200 100]';
%ys = [100 100 200 200]';

disp('Points have been selected.');
%Now build and solve the system.
xp = [ypp'; xpp'];  % ones(1,4)];
x = [ys'; xs'];     % ones(1,4)];

INITIAL_X = x;
INITIAL_X(3,:) = ones(1,numxs(1));

%Normalize the points and compute T and T'. 
%x first
mx = mean(x');
x(1,:) = x(1,:)-mx(1);
x(2,:) = x(2,:)-mx(2);
%centroid is origin. Now sum of squares should
%average 2.
s = mean(sum(x.^2));
s = sqrt(2/s);
x = x*s;
T = [[s 0 0]' [0 s 0]' [-s*mx(1) -s*mx(2) 1]']; 
%Now the average magnitude is sqrt(2);
%same for xp.
mx = mean(xp');
xp(1,:) = xp(1,:)-mx(1);
xp(2,:) = xp(2,:)-mx(2);
%centroid is origin. Now sum of squares should
%average 2.
s = mean(sum(xp.^2));
s = sqrt(2/s);
xp = xp*s;
TP = [[s 0 0]' [0 s 0]' [-s*mx(1) -s*mx(2) 1]']; 

x(3,:) = ones(1,numxs(1));
xp(3,:) = ones(1,numxs(1));

A = zeros(2*numxs(1),9);
for i = 2:2:2*numxs(1)
    A(i-1,:) = [zeros(1,3) -xp(3,i/2)*x(:, i/2)' xp(2,i/2)*x(:, i/2)'];
    A(i,:) = [xp(3,i/2)*x(:, i/2)' zeros(1,3) -xp(1,i/2)*x(:, i/2)'];
end

%Now svd
[U,S,V] = svd(A'*A);
%[U,S,V] = svd(A);
HN = [V(1:3,9)'; V(4:6,9)'; V(7:9,9)'];
%Now denormalize the homography.
H = inv(TP)*HN*T;
%H is the better DLT homography.

minn = [0 0];  maxx = dim(1:2);
box = H*[[1 1 1]' [cdim(1) 1 1]' [1 cdim(2) 1]' [cdim(1:2) 1]'];
box(1,:) = box(1,:)./box(3,:);
box(2,:) = box(2,:)./box(3,:);

mins = min(box');
maxs = max(box');
if (mins(1) < minn(1)) 
    minn(1) = mins(1);
end
if (mins(2) < minn(2))
    minn(2) = mins(2);
end
if (maxs(1) > maxx(1))
    maxx(1) = maxs(1);
end
if (maxs(2) > maxx(2))
    maxx(2) = maxs(2);
end

%map the base image.
maxx = ceil(maxx);
minn = floor(minn);

newdims = [(maxx(1)-minn(1)) (maxx(2)-minn(2))];
newI = zeros(newdims(1),newdims(2),3);
for i = 1:dim(1)
    for j = 1:dim(2)
        newI(i-minn(1),j-minn(2),:) = I(i,j,:);
    end
end
dim = size(newI);
        
%overwrite parts and add the warped image to it.
%for i = 1:cdim(1)
%    for j = 1:cdim(2)
%        mapp = H*[i j 1]';
%        mapp(1:2) = mapp(1:2)/mapp(3);
%        if (mapp(1) >= minn(1)) & (mapp(1) <= maxx(1)) & (mapp(2) >= minn(2)) & (mapp(2) <= maxx(2))
%            newI(ceil(mapp(1))-minn(1),ceil(mapp(2))-minn(2),:) = Cur_Image(i,j,:)/255;
%        end
%    end
%end

%instead of the above forward projection, do a backward projection,
%to avoid the gridding.

HI = inv(H);
for i = 1:newdims(1)
    for j = 1:newdims(2)
        mapp = HI*[(i+minn(1)) (j+minn(2)) 1]';
        mapp(1:2) = mapp(1:2)/mapp(3);
        if (mapp(1) >= 1) & (mapp(1) <= cdim(1)) & (mapp(2) >= 1) & (mapp(2) <= cdim(2))
            newI(i,j,:) = Cur_Image(round(mapp(1)),round(mapp(2)),:)/255;
        end
    end
end

I = newI;

%update the referece image.

figure(h(1)), imshow(I, 'truesize');
close(h(cur_im));

end



























panoramGS















function [I, H] = panoramaGS(varargin)
%panorama Gold Standard error, n input, up to 
%10 correspondences between any two images.
%Given some number of image filenames.
%Images in jpg format.
%Displays two images at a time and collects
%point correspondences from the user.
%The first image input is the reference, and
%the panorama is the reference after that.
%reference: Hartley Zisserman p. 98.
disp ('Select a point on the second image, and a corresponding point on the first.');
disp ('Continue to select corresponding points, one at a time. Press return when done.');
disp ('The result panorama will be displayed.');
disp ('Then select points from the third image and the panorama as before.');
disp ('Continue until tired.  When all inputs have been registered, the program');
disp ('will return the resultant panorama image.');
global INITIAL_X
%if nargin < 2
%    error('too few arguments');
%else
     n = nargin;
%end

iptsetpref 'ImshowAxesVisible' on;
h = zeros(n);
I = zeros(700,700,3);

    CI = imread(sprintf('%s',varargin{1}),'jpg');
    dim = size(CI);
    I(1:dim(1),1:dim(2),:) = CI;
    h(1) = figure('Name',varargin{1}); 
    imshow(varargin{1}, 'truesize');
    I = I./255;


for cur_im = 2:n
%*******************
%Every iteration begins with much of the code from the 
%normalized DLT.  This serves that the initialization
%for the Gold Standard.
%*******************
    
%Load next image.
disp(sprintf('%s points', varargin{cur_im}));
CI = imread(sprintf('%s',varargin{cur_im}),'jpg');
cdim = size(CI);
Cur_Image = zeros(cdim(1),cdim(2),3);
Cur_Image(:,:,:) = CI;

%display it.
h(cur_im) = figure('Name',varargin{cur_im});
imshow(CI, 'truesize');

%select points in this image.
[xs,ys] = ginput;    %x's
numxs = size(xs);
for i = 1:numxs(1)
    disp(sprintf('(%f , %f)', xs(i), ys(i)));
end

%and select points on the reference image.
disp('Reference image points.');
figure(h(1));
[xpp,ypp] = ginput(numxs(1));  %x primes.
for i = 1:numxs(1)
    disp(sprintf('(%f , %f)', xpp(i), ypp(i)));
end

%xpp = [150 250 250 150]';
%ypp = [100 100 200 200]';
%xs = [100 200 200 100]';
%ys = [100 100 200 200]';

disp('Points have been selected.');
%Now build and solve the system.
xp = [ypp'; xpp'];  % ones(1,4)];
x = [ys'; xs'];     % ones(1,4)];

%Normalize the points and compute T and T'. 
%x first
x_backup = x;   %for the sampson correction.
mx = mean(x');
x(1,:) = x(1,:)-mx(1);
x(2,:) = x(2,:)-mx(2);
%centroid is origin. Now sum of squares should
%average 2.
s = mean(sum(x.^2));
s = sqrt(2/s);
x = x*s;
T = [[s 0 0]' [0 s 0]' [-s*mx(1) -s*mx(2) 1]']; 
%Now the average magnitude is sqrt(2);
%same for xp.
xp_backup = xp;
mx = mean(xp');
xp(1,:) = xp(1,:)-mx(1);
xp(2,:) = xp(2,:)-mx(2);
%centroid is origin. Now sum of squares should
%average 2.
s = mean(sum(xp.^2));
s = sqrt(2/s);
xp = xp*s;
TP = [[s 0 0]' [0 s 0]' [-s*mx(1) -s*mx(2) 1]']; 

x(3,:) = ones(1,numxs(1));
xp(3,:) = ones(1,numxs(1));


%**********IMPORTANT***
%We need to make the below building of A more robust,
%to account for when wi' is 0...
%*******************

A = zeros(2*numxs(1),9);
for i = 2:2:2*numxs(1)
    A(i-1,:) = [zeros(1,3) -xp(3,i/2)*x(:, i/2)' xp(2,i/2)*x(:, i/2)'];
    A(i,:) = [xp(3,i/2)*x(:, i/2)' zeros(1,3) -xp(1,i/2)*x(:, i/2)'];
end

%Now svd
[U,S,V] = svd(A'*A);
%[U,S,V] = svd(A);
HN = [V(1:3,9)'; V(4:6,9)'; V(7:9,9)'];
%Now denormalize the homography.
H = inv(TP)*HN*T;
hvec = [H(1,:) H(2,:) H(3,:)]';
%H is the better DLT homography.

%H is the initial estimate of H.  Now compute
%the Sampson correction to the x's.

%The only way I can think to initialize the subsidiary
%variables xi hats, as per p.98, is to specify the J matrix
%of p.83, the Sampson correction. To me J is specified only
%explicitly; it's a 2x4 matrix...
x = x_backup;
xp = xp_backup;
x(3,:) = ones(1,numxs(1));
xhat = x;
xp(3,:) = ones(1,numxs(1));
%Remake an unnormalized A.
A = zeros(2*numxs(1),9);
for i = 2:2:2*numxs(1)
    A(i-1,:) = [zeros(1,3) -xp(3,i/2)*x(:, i/2)' xp(2,i/2)*x(:, i/2)'];
    A(i,:) = [xp(3,i/2)*x(:, i/2)' zeros(1,3) -xp(1,i/2)*x(:, i/2)'];
end

for i = 1:numxs(1)
    %X(1:2,i) = x(:,i);
    %X(3:4,i) = xp(:,i);     %The x and xp are still (y,x) pairs from above.
    J = [(-hvec(4)+xp(2,i)*hvec(7)) (-hvec(5)+xp(2,i)*hvec(8)) 0 (x(1:2,i)'*hvec(7:8)+hvec(9));
        (hvec(1)-xp(1,i)*hvec(7)) (hvec(2)-xp(1,i)*hvec(8)) (-(x(1:2,i)'*hvec(7:8)+hvec(9))) 0];
    
    %Del(x) = -J'inv(JJ')e, where e is Ai*h
    e = A((2*i-1):2*i,:)*hvec;
    Delx = -J'*inv(J*J')*e;
    %Delx solves for both x,y and x',y' corrections.
    %for the gold standard though, initialization may
    %as well be just on x,y (xi hat).
    %This is why Marc said error in one image is fine...d'oh.
    xhat(1:2,i) = x(1:2,i) + Delx(1:2);
end

%Now we want to minimize 

INITIAL_X = x;

x0 = [xhat(1,:) xhat(2,:)];
x0(2*numxs(1)+1:2*numxs(1)+9) = hvec';
x0 = x0';
%x0 is the initialization of the variables over which we will
%optimize.  The H is all we really want at the end, though we
%optimized over x-hat as well.

x = lsqnonlin(@goldstandardcost, x0);
%x = fminsearch(@reduced_standard,x0);


H = [x(2*numxs(1)+1:2*numxs(1)+3)'; x(2*numxs(1)+4:2*numxs(1)+6)'; x(2*numxs(1)+7:2*numxs(1)+9)'];
%H is the GOLD STANDARD homography this time.


minn = [0 0];  maxx = dim(1:2);
box = H*[[1 1 1]' [cdim(1) 1 1]' [1 cdim(2) 1]' [cdim(1:2) 1]'];
box(1,:) = box(1,:)./box(3,:);
box(2,:) = box(2,:)./box(3,:);

mins = min(box');
maxs = max(box');
if (mins(1) < minn(1)) 
    minn(1) = mins(1);
end
if (mins(2) < minn(2))
    minn(2) = mins(2);
end
if (maxs(1) > maxx(1))
    maxx(1) = maxs(1);
end
if (maxs(2) > maxx(2))
    maxx(2) = maxs(2);
end

%map the base image.
maxx = ceil(maxx);
minn = floor(minn);

newdims = [(maxx(1)-minn(1)) (maxx(2)-minn(2))];
newI = zeros(newdims(1),newdims(2),3);
for i = 1:dim(1)
    for j = 1:dim(2)
        newI(i-minn(1),j-minn(2),:) = I(i,j,:);
    end
end
dim = size(newI);
        
%overwrite parts and add the warped image to it.
%for i = 1:cdim(1)
%    for j = 1:cdim(2)
%        mapp = H*[i j 1]';
%        mapp(1:2) = mapp(1:2)/mapp(3);
%        if (mapp(1) >= minn(1)) & (mapp(1) <= maxx(1)) & (mapp(2) >= minn(2)) & (mapp(2) <= maxx(2))
%            newI(ceil(mapp(1))-minn(1),ceil(mapp(2))-minn(2),:) = Cur_Image(i,j,:)/255;
%        end
%    end
%end

%instead of the above forward projection, do a backward projection,
%to avoid the gridding.

HI = inv(H);
%points = zeros(3,newdims(1)*newdims(2));
%[X,Y] = meshgrid(1:newdims(1),1:newdims(2));
%c = 0;
%for i = X
%    for j = Y
%        points(1:2,(c+1):(c+newdims(2))) = [i';j'];
%        c = c + newdims(2);
%    end
%end
        
for i = 1:newdims(1)
    for j = 1:newdims(2)
        mapp = HI*[(i+minn(1)) (j+minn(2)) 1]';
        mapp(1:2) = mapp(1:2)/mapp(3);
        if (mapp(1) >= 1) & (mapp(1) <= cdim(1)) & (mapp(2) >= 1) & (mapp(2) <= cdim(2))
            newI(i,j,:) = Cur_Image(round(mapp(1)),round(mapp(2)),:)/255;
        end
    end
end

I = newI;

%update the referece image.

figure(h(1)), imshow(I, 'truesize');
close(h(cur_im));

end

X = [x(1:numxs(1))'; x((numxs(1)+1):2*numxs(1))'; ones(1,numxs(1))];
%The total reprojection error.
PreImerr = (INITIAL_X - X);
PostImerr1 = H*INITIAL_X;
PostImerr1(1,:) = PostImerr1(1,:)./PostImerr1(3,:);
PostImerr1(2,:) = PostImerr1(2,:)./PostImerr1(3,:);
PostImerr = H*X;
PostImerr(1,:) = PostImerr(1,:)./PostImerr(3,:);
PostImerr(2,:) = PostImerr(2,:)./PostImerr(3,:);
PostImerr = PostImerr1 - PostImerr;

total_reproj_error = sum(sum([PreImerr(1,:) PostImerr(1,:) PreImerr(2,:) PostImerr(2,:)].^2))














goldstandardcost


















function F = goldstandardcost(x)
%This function is an input to lsqnonlin in panoramaGS.m
%the last nine columns of the 2*n+9 element x are expected to
%to be the homography H-hat.  all the x's then all the y's.
global INITIAL_X
s = size(x);
n = s(1)-9;
X = [x(1:n/2)'; x((n/2+1):n)'];  %%MAYBE I SHOULD SWAP...
X(3,:) = ones(1,n/2);
H = [x((n+1):(n+3))'; x((n+4):(n+6))'; x((n+7):(n+9))'];
%H is the current homography.
PreImerr = (INITIAL_X - X);
PostImerr1 = H*INITIAL_X;
PostImerr1(1,:) = PostImerr1(1,:)./PostImerr1(3,:);
PostImerr1(2,:) = PostImerr1(2,:)./PostImerr1(3,:);
PostImerr = H*X;
PostImerr(1,:) = PostImerr(1,:)./PostImerr(3,:);
PostImerr(2,:) = PostImerr(2,:)./PostImerr(3,:);
PostImerr = PostImerr1 - PostImerr;

%Should I make the coordinates inhomogeneous? Yes, so
%that the error functional can be 4n.
PreImerr(1,:) = PreImerr(1,:);%./PreImerr(3,:);
PreImerr(2,:) = PreImerr(2,:);%./PreImerr(3,:);

F = [PreImerr(1,:) PostImerr(1,:) PreImerr(2,:) PostImerr(2,:)]';