%%% NAME:         PhotoTop, 1.0
%%% 
%%% DATE:         12.04.02
%%%
%%% REQUIREMENTS: MatLab 5.3 or greater. The code should run on any
%%%               platform as there are no MEX files.
%%%
%%% INSTRUCTIONS: To begin: at the matlab prompt, type "phototop"
%%%               To build a DEM:
%%%                 step 0. load (at least 3) images
%%%                 step 1. - pick a corresponding point in all images
%%%                         - repeat between 50 and 100 times
%%%                         - points should be selected with a
%%%                           sub-pixel accuracy -- use Matlab's zoom
%%%                           function to zoom in on the area of
%%%                           interest before picking each point.
%%%                 step 2. save points to a .mat file
%%%                 step 3. estimate DEM
%%%                  
%%%                 step 4. if you make a mistake choosing points, 
%%%                         return to the menu and choose option 4 
%%%
%%%                 step 5. if you would like to reload a previously
%%%                         saved collection of points and images,
%%%                         choose option 5
%%%
%%%               If you are having trouble getting an accurate DEM:
%%%                  - select more points
%%%                  - increase the number of iterations in the
%%%                    non-linear minimization (see PARAMIN below)
%%%                  - force a smoothness constraint (see SMOOTHMIN below)
%%%
%%% KNOWN BUGS:   The non-linear minimization occasionaly yields 
%%%               unstable solutions under Matlab 6 and greater. This
%%%               is not a problem with Matlab 5
%%%
%%% REFERENCE:    Hillslope Topography from Unconstrained Photographs 
%%%               A. Heimsath and H. Farid 
%%%               Mathematical Geology, 34(8):929-952, 2002 
%%%
%%% CONTACT:      arjun.heimsath@dartmouth.edu, hany.farid@dartmouth.edu
%%%



%%% Clear if first time through ---------------------------------------------
if( ~exist( 'FIRST', 'var' ) )
   clear;
end


%%% Menu options ------------------------------------------------------------
RESP = input( '0) load  1) pick  2) save   3) estimate  4) delete  5) reload  6) quit : ' );
FIRST = 1;


%%% Load images -------------------------------------------------------------
if( RESP == 0 )
   close all;

   %%% Number of images
   N = input( 'number of images: ' );
   C = 1; % for number of points (see below)
   if( N < 3 )
      fprintf( 'load images: must have at least 3 images - aborting\n' );
      return;
   end

   %%% Read images
   for k = 1 : N
      filename = input( 'image name: ', 's' );
      if( exist( filename, 'file' ) )
	 I(k).im = imread( filename );
	 k = k + 1;
      else
	 fprintf( 'load images: %s does not exist - aborting\n', filename );
	 return;
      end
   end

   %%% Check that all image sizes are the same
   [ydim,xdim,zdim] = size(I(1).im);
   for k = 2 : N
      [ydim2,xdim2,zdim] = size(I(k).im);
      if( (xdim ~= xdim2) | (ydim ~= ydim2) )
	  fprintf( 'load images: all images must be the same size - aborting\n' );
	  return;
      end
   end

   %%% Display images
   for k = 1 : N
      figure(k);
      imagesc( I(k).im ); axis image off; colormap gray;
      set( gcf, 'Renderer', 'zbuffer' );
      hold on;
   end
end


%%% Pick point -------------------------------------------------------------
if( RESP == 1 )
   for k = 1 : N
      figure(k);
      [x,y] = ginput(1);
      handle(k) = plot( x, y, 'g+' );
      P(k,C).x = x;
      P(k,C).y = y;
   end
   C = C + 1;
end


%%% Save point -------------------------------------------------------------
if( RESP == 2 )
   datafilename = input('data filename: ', 's' );
   Ncamera = N;
   Npoint = C - 1;
   eval( sprintf( 'save %s P Npoint Ncamera xdim ydim I;', datafilename ) );
end


%%% Estimate----------------------------------------------------------------
if( RESP == 3 )
   %%% Data filename
   datafilename = input('data filename: ', 's' );
   if( ~exist( datafilename, 'file' ) )
      fprintf( 'estimate: %s does not exist - aborting\n', datafilename );
      return;
   end
   eval( sprintf( 'load %s;', datafilename ) );

   %%% Make sure there are enough points
   if( size(P,2) < 10 )
      fprintf( 'estimate: must have more than 10 points - aborting\n' );
      return;
   end

   %%% NON-LINEAR OPTIMIZATION PARAMETERS
   % ========================================
   PARAMIN = 1;   % number of iterations for minimizing
                  % motion (camera) and shape parameters under a  
		  % perspective projection imaging model
   MOTIONMIN = 1; % minimize for motion (camera) parameters  (1 or 0)
   SHAPEMIN = 1;  % minimize for shape parameters  (1 or 0)
   SMOOTHMIN = 0; % minimize for smoothness in elevation (1 or 0)
   % ========================================
   
   fprintf( 'Paraperspective estimate... ' );

   %%% Build measurement matrix
   for k = 1 : Npoint
      for j = 1 : Ncamera
	 U(j,k) = 2/xdim*(P(j,k).x - xdim/2);
	 V(j,k) = 2/ydim*(P(j,k).y - ydim/2);
      end
   end
   W = [U ; V];
   
   %%% Estimate translation vector
   for c = 1 : Ncamera
      tx(c) = mean( W(c,:) );
      ty(c) = mean( W(Ncamera+c,:) );
   end
   Test = [tx' ; ty'];
   W    = W - Test * ones(1,Npoint);

   %%% Decompose the measurement matrix 
   [O1,D,O2] = svd(W);
   O2   = O2';
   O1   = O1(:,1:3);
   D    = D(1:3,1:3);
   O2   = O2(1:3,:);
   Mest = O1 * sqrt(D);
   Sest = sqrt(D) * O2;
   
   %%% Impose metric constraints
   c = 1;
   for k = 1 : Ncamera
      m1 = Mest(k,1); m2 = Mest(k,2); m3 = Mest(k,3);
      n1 = Mest(Ncamera+k,1); n2 = Mest(Ncamera+k,2); n3 = Mest(Ncamera+k,3);
      x = 1+Test(k)^2;
      y = 1+Test(Ncamera+k)^2;
      a = [m1^2 2*m1*m2 2*m1*m3 m2^2 2*m2*m3 m3^2]/x;
      b = [n1^2 2*n1*n2 2*n1*n3 n2^2 2*n2*n3 n3^2]/y;
      q(c,:) = a-b;
      c = c + 1;
   end
   for k = 1 : Ncamera
      m1 = Mest(k,1); m2 = Mest(k,2); m3 = Mest(k,3);
      n1 = Mest(Ncamera+k,1); n2 = Mest(Ncamera+k,2); n3 = Mest(Ncamera+k,3);
      x = 1+Test(k)^2;
      y = 1+Test(Ncamera+k)^2;
      a = [m1*n1 n1*m2+n2*m1 n1*m3+n3*m1 n2*m2 n2*m3+n3*m2 n3*m3];
      b = [m1^2 2*m1*m2 2*m1*m3 m2^2 2*m2*m3 m3^2]/x + ...
	  [n1^2 2*n1*n2 2*n1*n3 n2^2 2*n2*n3 n3^2]/y;
      x = Test(k);
      y = Test(Ncamera+k);
      b = 1/2*x*y*b;
      q(c,:) = a-b;
      c = c + 1;
   end
   [v,d] = eig(q'*q);
   [minval,minind] = min(diag(d));
   v = v(:,minind);
   Q = [v(1) v(2) v(3) ; v(2) v(4) v(5) ; v(3) v(5) v(6)];
   [u,s,v] = svd(Q);
   A = (u*sqrt(s))';
   Sest = inv(A)*Sest;
   Mest = Mest*A;
   
   %%% Final centering
   for c = 1 : 3
      Sest(c,:) = Sest(c,:) - mean(Sest(c,:));
   end
   
   %%% Recover camera position
   for c = 1 : Ncamera
      m  = Mest(c,:);
      n  = Mest(Ncamera+c,:);
      tx = Test(c);
      ty = Test(Ncamera+c);
      m2 = sqrt(1+tx^2) * m / norm(m);
      n2 = sqrt(1+ty^2) * n / norm(n);
      G  = [cross(n2,m2) ; m2 ; n2];
      H  = [1 ; -tx ; -ty];
      k  = (inv(G)*H)';
      k  = k / norm(k);
      i  = cross(n2,k);
      j  = cross(k,m2);
      i  = i / norm(i);
      j  = j / norm(j);
      
      %%% Gram-Schmidt orthonormalization
      H = [i ; j ; k];
      for k = 1 : 3
	 t = zeros( 1,3 );
	 for j = 1 : k-1
	    t = t + (H(j,:)*H(k,:)')*H(j,:);
	 end
	 H(k,:) = H(k,:) - t;
      end
      i = H(1,:);
      j = H(2,:);
      k = H(3,:);
      i  = i / norm(i);
      j  = j / norm(j);
      k  = k / norm(k);
      
      tz = -sqrt( 1/2*( (1+tx^2)/norm(m)^2 + (1+ty^2)/norm(n)^2 ) );
      t  = (inv([i ; j ; -k]) * [-tx*tz ; -ty*tz ; -tz] )';
      CameraEst(c,:) = [i j -k t];
   end
   fprintf( 'done\n' );
   
   %%% Estimate perspective shape/motion (non-linear minimization)
   for k = 1 : PARAMIN   
      if(MOTIONMIN)
         fprintf( 'Perspective estimate (motion) %d-%d... ', k, PARAMIN );
	 %%% Motion
	 options = optimset;
	 options = optimset( 'MaxFunEvals', 10000, 'MaxIter', 10000 );
	 options = optimset( options, 'LevenbergMarquardt', 'on' );
	 options = optimset( options, 'Display', 'off' );
	 for c = 1 : Ncamera
	    rot = [CameraEst(c,1:3) ; CameraEst(c,4:6) ; CameraEst(c,7:9) ];
	    trans = CameraEst(c,10:12);
	    beta = -real( asin(rot(3,1)) );
	    gamma = real( acos(rot(1,1)/cos(beta)) );
	    alpha = real( acos(rot(3,3)/cos(beta)) );
	    alpha = alpha*180/pi; beta = beta*180/pi; gamma = gamma*180/pi;
	    
	    X = fminsearch( 'errfuncperspM', [alpha beta gamma trans], ...
			    options, [W(c,:);W(Ncamera+c,:)], Sest, ...
			    [Test(c,:);Test(Ncamera+c,:)], Ncamera, Npoint ...
			    );
	    
	    rot = rotmat( X(1), X(2), X(3) );
	    CameraEst(c,:) = [rot(1,:) rot(2,:) rot(3,:) X(4:6)];
	 end     
	 fprintf( 'done\n' );
      end
      
      if(SHAPEMIN)
	 %%% Shape
	 fprintf( 'Perspective estimate (shape) %d-%d... ', k, PARAMIN );
	 options = optimset;
	 options = optimset( 'MaxFunEvals', 10000, 'MaxIter', 10000 );
	 options = optimset( options, 'LevenbergMarquardt', 'on' );
	 options = optimset( options, 'Display', 'off' );
	 for c = 1 : Npoint
	    X = fminsearch( 'errfuncperspS', Sest(:,c), options, W(:,c), ...
			    CameraEst, Test, Ncamera );
	    Sest(:,c) = X;
	 end
	 fprintf( 'done\n' );
      end
      
      if(SMOOTHMIN)
	 %%% Smoothness in elevation
	 fprintf( 'Perspective estimate (smoothness) %d-%d... ', k, PARAMIN );	
	 options = optimset;
	 options = optimset( 'MaxFunEvals', 10000, 'MaxIter', 10000 );
	 %options = optimset( options, 'LevenbergMarquardt', 'on' );
	 options = optimset( options, 'Display', 'off' );
	 for c = 1 : Npoint
	    X = fminsearch( 'errfuncperspSmooth', Sest(3,c), options, ...
			    Sest(:,c), Sest );
	    Sest(3,c) = X;
	 end
	 fprintf( 'done\n' );
      end
   end

   %%% Re-project onto image plane using estimated camera position
   for c = 1 : Ncamera
      rot = [CameraEst(c,1:3) ; CameraEst(c,4:6) ; CameraEst(c,7:9) ];
      rot = rot;
      trans = CameraEst(c,10:12);
      i = rot(1,:);
      j = rot(2,:);
      k = rot(3,:);
      den = k*Sest - k*trans'*ones(1,Npoint);
      U(c,:) = (i*Sest - i*trans'*ones(1,Npoint))./den;
      V(c,:) = (j*Sest - j*trans'*ones(1,Npoint))./den;
   end
   West = [U ; V];
   West = West - Test * ones(1,Npoint);
   
   %%% Display everything
   if(0)
      figure; % image
      set( gcf, 'Renderer', 'Zbuffer' );
      sp = ceil( sqrt(Ncamera) );
      for c = 1 : Ncamera
	 subplot(sp,sp,c);
	 plot( W(c,:), W(Ncamera+c,:), 'bo' ); hold on;
	 plot( West(c,:), West(Ncamera+c,:), 'rx' ); hold off;
	 axis( max(max(abs(W(:))),max(abs(West(:))))*[-1 1 -1 1] ); 
	 set( gca, 'ydir', 'reverse' );
	 axis equal;
	 drawnow;
	 rmserr = sum((W(c,:)-West(c,:)).^2 + ...
		      (W(Ncamera+c,:)-West(Ncamera+c,:)).^2);
	 title( sprintf( '%f', rmserr ) );
      end
   end

   figure; % world (shape)
   subplot(111);
   set( gcf, 'Renderer', 'Zbuffer' );
   h = plot3( Sest(1,:), Sest(2,:), Sest(3,:), 'b.' );
   set( h, 'MarkerSize', 16 );
   axis equal; box on; grid on;
   xlabel('X'); ylabel('Y'); zlabel('Z');
   set( gca, 'Xdir','reverse','Zdir','reverse' );
   
   Nsamp = 30; % interpolated surface
   x = Sest(1,:);
   y = Sest(2,:);
   z = Sest(3,:);
   [xx,yy] = meshgrid( [min(x):(max(x)-min(x))/Nsamp:max(x)], ...
		       [min(y):(max(y)-min(y))/Nsamp:max(y)] );
   [xi,yi,zi] = griddata( x,y,z, xx,yy, 'cubic' );
   hold on;
   h = mesh(xi,yi,zi);
   set(h, 'FaceColor', 'none', 'EdgeColor', [1 0 0] );
   hold off;
end


%%% Delete point ------------------------------------------------------------
if( RESP == 4 )
   for k = 1 : N
      set( handle(k), 'Visible', 'off' );
   end
   C = C - 1;
   P = P(:,1:C);
end


%%% Reload data --------------------------------------------------------------
if( RESP == 5 )
   close all;

   %%% Data filename
   datafilename = input('data filename: ', 's' );
   if( ~exist( datafilename, 'file' ) )
      fprintf( 'reload point: %s does not exist - aborting\n', datafilename );
      return;
   end
   eval( sprintf( 'load %s;', datafilename ) );
   N = Ncamera;
   C = Npoint;
   
   %%% Re-display images
   for k = 1 : N
      figure(k);
      imagesc( I(k).im ); axis image off; colormap gray;
      set( gcf, 'Renderer', 'zbuffer' );
      hold on;
   end

   %%% Re-plot points
   for j = 1 : Ncamera
      figure(j);
      for k = 1 : C
	 h = plot( P(j,k).x, P(j,k).y, 'g+' );
	 set( h, 'LineWidth', 2 );
      end
   end
   C = C + 1;
end


%%% Quit ----------------------------------------------------------------
if( RESP == 6 )
   close all;
end


%%% Repeat ----------------------------------------------------------------
if( RESP ~= 6 )
   phototop;
end