im0 = double(imread('img0.pgm'));
im1 = double(imread('img1.pgm'));
im1t = imresize(imrotate(im1,10),1.2);

% Window specification in im0
wh = 31;  ww = 31;
whh = floor(wh/2);  wwh = floor(ww/2);
ws = 40;
[x,y] = meshgrid(ww:ws:size(im0,2)-ww, wh:ws:size(im0,1)-wh);
wxy = [reshape(x,1,prod(size(x))); reshape(y,1,prod(size(x)))];

% Draw problem
fh = figure;
subplot(1,2,1);  imagesc(im0);  colormap(gray);  title('Image 1');
subplot(1,2,2);  imagesc(im1t);  colormap(gray);  title('Image 2');
exportfig(gcf,'ransac_imagematch_fig05.eps','width',12.5,'height',6,'fontmode','fixed', 'fontsize',8,'Color','cmyk');

% Draw initial
fh = figure;
subplot(1,2,1);  imagesc(im0);  colormap(gray);
hold on;  sh = scatter(wxy(1,:),wxy(2,:),'r+');  hold off;
set(sh,'sizedata',100);  set(sh,'linewidth',2);
hv = sh;
subplot(1,2,2);  imagesc(im1t);  colormap(gray);
drawnow;

% Match windows in im1
mxy = zeros(size(wxy));
for i=1:size(wxy,2)
  disp(sprintf('Done %i/%i',i,size(mxy,2)));
  templ = im0(wxy(2,i)-whh:wxy(2,i)+whh,wxy(1,i)-wwh:wxy(1,i)+wwh);
  ncres = normxcorr2(templ,im1t);
  ncres = ncres(whh:end-whh-1,wwh:end-wwh-1);
  [ir,jr] = find(ncres==max(max(ncres)));
  mxy(:,i) = [jr(1); ir(1)];
  subplot(1,2,2);
  hold on;  sh = scatter(mxy(1,i),mxy(2,i),'r+');  hold off;
  set(sh,'sizedata',100);  set(sh,'linewidth',2);  hv = [hv; sh];
  
  % Annotate correspondence
  set(gcf,'Units','normalized');
  [mxf,myf] = axescoord2figurecoord(mxy(1,i),mxy(2,i));
  subplot(1,2,1);  [wxf,wyf] = axescoord2figurecoord(wxy(1,i),wxy(2,i));
  ah = annotation('line',[wxf mxf],[wyf myf]);  hv = [hv; ah];
  drawnow;
end
subplot(1,2,1);  title('Points');
subplot(1,2,2);  title('Putative matches');
exportfig(gcf,'ransac_imagematch_fig01.eps','width',12.5,'height',6,'fontmode','fixed', 'fontsize',8,'Color','cmyk');

% RANSAC
inldist = 7;  
binliers = [];
for i=1:20
  rp = randperm(size(mxy,2));
  [s,R,t,rmse] = similarity_est2d(mxy(:,rp(1:2)),wxy(:,rp(1:2))); 
  mxys = s*R*wxy + repmat(t,1,size(mxy,2));
  dist = sqrt(sum((mxys - mxy).^2));
  inliers = dist<inldist;
  if sum(inliers)>sum(binliers), binliers = inliers; end
end

%% Show inliers and outliers
fh = figure;
subplot(1,2,1);  imagesc(im0);  colormap(gray);
title('Points');
subplot(1,2,2);  imagesc(im1t);  colormap(gray);
title('Good matches from RANSAC');
subplot(1,2,1);
hv = [];
hold on;  sh = scatter(wxy(1,binliers),wxy(2,binliers),'g+');  hold off;
set(sh,'sizedata',100);  set(sh,'linewidth',2);  hv = [hv; sh];
hold on;  sh = scatter(wxy(1,~binliers),wxy(2,~binliers),'r+');  hold off;
set(sh,'sizedata',100);  set(sh,'linewidth',2);  hv = [hv; sh];
subplot(1,2,2);
hold on;  sh = scatter(mxy(1,binliers),mxy(2,binliers),'g+');  hold off;
set(sh,'sizedata',100);  set(sh,'linewidth',2);  hv = [hv; sh];
%hold on;  sh = scatter(mxy(1,~binliers),mxy(2,~binliers),'r+');  hold off;
%set(sh,'sizedata',100);  set(sh,'linewidth',2);  hv = [hv; sh];
set(gcf,'Units','normalized');
for i=find(binliers)
  subplot(1,2,2);  [mxf,myf] = axescoord2figurecoord(mxy(1,i),mxy(2,i));
  subplot(1,2,1);  [wxf,wyf] = axescoord2figurecoord(wxy(1,i),wxy(2,i));
  ah = annotation('line',[wxf mxf],[wyf myf]);  hv = [hv; ah];
end
exportfig(gcf,'ransac_imagematch_fig02.eps','width',12.5,'height',6,'fontmode','fixed', 'fontsize',8,'Color','cmyk');

%% Display RANSAC estimated correspondences
[s,R,t,rmse] = similarity_est2d(mxy(:,binliers),wxy(:,binliers));
mxys = s*R*wxy + repmat(t,1,size(mxy,2));
fh2 = figure;
subplot(1,2,1);  imagesc(im0);  colormap(gray);
hold on;  sh = scatter(wxy(1,:),wxy(2,:),'r+');  hold off;
set(sh,'sizedata',100);  set(sh,'linewidth',2);
title('Points');
subplot(1,2,2);  imagesc(im1t);  colormap(gray);
hold on;  sh = scatter(mxys(1,:),mxys(2,:),'r+');  hold off;
set(sh,'sizedata',100);  set(sh,'linewidth',2);
title('Transformed points (RANSAC)');
drawnow;
exportfig(gcf,'ransac_imagematch_fig03.eps','width',12.5,'height',6,'fontmode','fixed', 'fontsize',8,'Color','cmyk');

%% Display least squares estimated correspondences
[s,R,t,rmse] = similarity_est2d(mxy,wxy);
mxys = s*R*wxy + repmat(t,1,size(mxy,2));
fh3 = figure;
subplot(1,2,1);  imagesc(im0);  colormap(gray);
hold on;  sh = scatter(wxy(1,:),wxy(2,:),'r+');  hold off;
set(sh,'sizedata',100);  set(sh,'linewidth',2);
title('Points');
subplot(1,2,2);  imagesc(im1t);  colormap(gray);
hold on;  sh = scatter(mxys(1,:),mxys(2,:),'r+');  hold off;
set(sh,'sizedata',100);  set(sh,'linewidth',2);
title('Transformed points (Least squares)');
exportfig(gcf,'ransac_imagematch_fig04.eps','width',12.5,'height',6,'fontmode','fixed', 'fontsize',8,'Color','cmyk');
drawnow;
