title: Find the distance from camera to vanishing point in matlab
body: <p>I have this program that finds the vanishing point for a given set of images. Is there a way to find the distance from the camera and the vanishing point?</p>
  
  <p>Also once the vanishing point is found out, I manually need to find the X and Y coordinates using the tool provided in matlab. How can i code a snippet that writes all the X and Y coordinates into a text or excel file?</p>
  
  <p>Also is there a better and simpler way to find the vanishing point in matlab?</p>
  
  <p><em><strong>Matlab Calling Function to find Vanishing Point:</em></strong></p>
  
  <pre><code>clear all; close all;
  dname = 'Height';
  files = dir(dname);
  files(1) = [];
  files(1) = [];
  for i=1:size(files, 1)
      original = imread(fullfile(dname, files(i).name));
      original = imresize(original,0.35);    
      im = im2double(rgb2gray(original)); 
      [row, col] = findVanishingPoint(im);
      imshow(original);hold;plot(col,row,'rx');
      saveas(gcf,strcat('Height_Result',num2str(i)),'jpg');
      close
  end 
  </code></pre>
  
  <p><em><strong>The findVanishingPoint function:</em></strong></p>
  
  <pre><code>function [row, col] = findVanishingPoint(im)
  
  DEBUG = 0;
  
  IM = fft2(im);
  ROWS = size(IM,1); COLS = size(IM,2);
  
  
  PERIOD = 2^floor(log2(COLS)-5)+2;
  SIZE = floor(10*PERIOD/pi);  
  SIGMA = SIZE/9; 
  NORIENT = 72; 
  E = 8;  
  [C, S] = createGaborBank(SIZE, PERIOD, SIGMA, NORIENT, ROWS, COLS, E);
  
  
  D = ones(ROWS, COLS); 
  AMAX = ifftshift(real(ifft2(C{1}.*IM)).^2+real(ifft2(S{1}.*IM))).^2; 
  for n=2:NORIENT
      A = ifftshift(real(ifft2(C{n}.*IM)).^2+real(ifft2(S{n}.*IM))).^2;
      D(find(A &gt; AMAX)) = n;        
      AMAX = max(A, AMAX);        
      if (DEBUG==1)
          colormap('hot');subplot(131);imagesc(real(A));subplot(132);imagesc(real(AMAX));colorbar;
          subplot(133);imagesc(D);
          pause
  end
  end
  
  if (DEBUG==2)
  figure('DoubleBuffer','on');
  end
  T = mean(AMAX(:))-3*std(AMAX(:));
  VOTE = zeros(ROWS, COLS);
  for row=round(1+SIZE/2):round(ROWS-SIZE/2) 
      for col=round(1+SIZE/2):round(COLS-SIZE/2)
          if (AMAX(row,col) &gt; T)
              indices = lineBresenham(ROWS, COLS, col, row, D(row, col)*pi/NORIENT-pi/2);
              VOTE(indices) = VOTE(indices)+AMAX(row,col);
          end
      end
      if (DEBUG==2)
          colormap('hot');imagesc(VOTE);pause;                                        
  end
  end
  if (DEBUG==2)
  close
  end
  
  M=1;
  [b index] = sort(-VOTE(:));
  col = floor((index(1:M)-1) / ROWS)+1;
  row = mod(index(1:M)-1, ROWS)+1;
  col = round(mean(col));
  row = round(mean(row));
  </code></pre>
  
  <p><em><strong>The creatGaborBank function:</em></strong></p>
  
  <pre><code>function [C, S] = createGaborBank(SIZE, PERIOD, SIGMA, NORIENT, ROWS, COLS, E)
  
  if (length(NORIENT)==1)
      orientations=[1:NORIENT];
  else
      orientations = NORIENT;
      NORIENT = max(orientations);
  end
  
  for n=orientations
      [C{n}, S{n}] = gabormask(SIZE, SIGMA, PERIOD, n*pi/NORIENT);
      C{n} = fft2(padWithZeros(C{n}, ROWS, COLS)); 
      S{n} = fft2(padWithZeros(S{n}, ROWS, COLS));
  end
  </code></pre>
  
  <p><em><strong>The gabormask function:</em></strong></p>
  
  <pre><code>function [cmask, smask] = gabormask(Size, sigma, period, orient, E)
  if nargin &lt; 5; E = 8; end;
  if nargin &lt; 4; orient = 0; end;
  if nargin &lt; 3; period = []; end;
  if nargin &lt; 2; sigma = []; end;
  if nargin &lt; 1; Size = []; end;
  
  if isempty(period) &amp; isempty(sigma); sigma = 5; end;
  if isempty(period); period = sigma*2*sqrt(2); end;
  if isempty(sigma); sigma = period/(2*sqrt(2)); end;
  if isempty(Size); Size = 2*round(2.575*sigma) + 1; end; 
  
  if length(Size) == 1
      sx = Size-1; sy = sx;
  elseif all(size(Size) == [1 2])
      sy = Size(1)-1; sx = Size(2)-1;
  else
      error('Size must be scalar or 1-by-2 vector');
  end;
  
  hy = sy/2; hx = sx/2;
  [x, y] = meshgrid(-hx:sx-hx, -hy:sy-hy);
  
  omega = 2*pi/period;
  cs = omega * cos(orient);
  sn = omega * sin(orient);
  k = -1/(E*sigma*sigma);
  
  g = exp(k * (E*x.*x + y.*y));   
  xp = x * cs + y * sn;           
  cx = cos(xp);                   
  cmask = g .* cx;               
  sx = sin(xp);                  
  smask = g .* sx;               
  
  cmask = cmask - mean(cmask(:));
  cmask = cmask/sum(abs(cmask(:)));
  smask = smask - mean(smask(:));
  smask = smask/sum(abs(smask(:)));
  </code></pre>
  
  <p><em><strong>The padWithZeros function:</em></strong></p>
  
  <pre><code>function out = padWithZeros(in, ROWS, COLS)
  out = padarray(in,[floor((ROWS-size(in,1))/2) floor((COLS-size(in,2))/2)],0,'both');
  if size(out,1) == ROWS-1
      out = padarray(out,[1 0],0,'pre');
  end
  if size(out,2) == COLS-1
      out = padarray(out,[0 1],0,'pre');
  end
  </code></pre>
  
  <p><em><strong>The findHorizonEdge function:</em></strong></p>
  
  <pre><code>function row = findHorizon(im)
  DEBUG = 2;
  
  ROWS = size(im,1); COLS = size(im,2);
  e = edge(im,'sobel', [], 'horizontal');
  dd = sum(e, 2);
  N=3;
  row = 1; 
  M = 0;
  for i=1+N:length(dd)-N
      m = sum(dd(i-N:i+N));    
      if (m &gt; M)
          M = m;
          row = i;
      end
  end
  imshow(e);pause
  </code></pre>
  
  <p><em><strong>The findHorizon function:</em></strong></p>
  
  <pre><code>function row = findHorizon(im)
  DEBUG = 2;
  
  IM = fft2(im);
  ROWS = size(IM,1); COLS = size(IM,2);
  
  PERIOD = 2^floor(log2(COLS)-5)+2; 
  SIZE = floor(10*PERIOD/pi);  
  SIGMA = SIZE/9; 
  NORIENT = 72; 
  E = 16; 
  orientations = [NORIENT/2-10:NORIENT/2+10]; 
  
  [C, S] = createGaborBank(SIZE, PERIOD, SIGMA, orientations, ROWS, COLS, E);
  
  ASUM = zeros(ROWS, COLS);
  for n=orientations
      A = ifftshift(real(ifft2(C{n}.*IM)).^2+real(ifft2(S{n}.*IM))).^2;
      ASUM = ASUM + A;        
      if (DEBUG==1)
          colormap('hot');subplot(131);imagesc(real(A));subplot(132);imagesc(real(AMAX));colorbar;
          pause
  end
  end
  
  ASUM(1:round(1+SIZE/2), :)=0; ASUM(end-round(SIZE/2):end, :)=0;
  ASUM(:,end-round(SIZE/2):end)=0; ASUM(:, 1:1+round(SIZE/2))=0;
  dd = sum(ASUM, 2);
  [temp, row] = sort(-dd);
  row = round(mean(row(1:10)));
  if (DEBUG == 2)
      imagesc(ASUM);hold on;line([1:COLS],repmat(row,COLS));
      pause
  end
  </code></pre>
  
  <p><em><strong>The lineImage function:</em></strong></p>
  
  <pre><code>function v = lineimage(x0, y0, angle, s) 
  
  if (abs(tan(angle)) &gt; 1e015)
      a(1,:) = repmat(x0,s(1),1)';
      a(2,:) = [1:s(1)];
  elseif (abs(tan(angle)) &lt; 1e-015)
      a(2,:) = repmat(y0,s(2),1)';
      a(1,:) = [1:s(2)];
  else
  
      k = tan(angle);
      hiX = round((1-(s(1)-y0+1)+k*x0)/k);
      loX = round((s(1)-(s(1)-y0+1)+k*x0)/k);
      temp = max(loX, hiX);
      loX = max(min(loX, hiX), 1);
      hiX = min(s(2),temp);
      a(1,:) = [loX:hiX];
      a(2,:) = max(1, floor(s(1)-(k*a(1,:)+(s(1)-y0+1)-k*x0)));
  end
  v = (a(1,:)-1).*s(1)+a(2,:);
  </code></pre>
  
  <p><em><strong>The lineVector function:</em></strong></p>
  
  <pre><code>function [abscissa, ordinate] = linevector(x0, y0, angle, s) 
  
  if (rad2deg(angle) == 90) 
          abscissa = repmat(x0,s(1),1);
          ordinate = [1:s(1)];
  else
      k = tan(angle);
      hiX = round((1-(s(1)-y0+1)+k*x0)/k);
      loX = round((s(1)-(s(1)-y0+1)+k*x0)/k);
      temp = max(loX, hiX);
      loX = max(min(loX, hiX), 1);
      hiX = min(s(2),temp);
  
      abscissa = [loX:hiX];
      ordinate = k*abscissa+((s(1)-y0+1)-k*x0);
  end
  </code></pre>
  
  <p><em><strong>The lineBresenham function:</em></strong></p>
  
  <pre><code>function [i] = lineBresenham(H,W,Sx,Sy,angle)
  
  k = tan(angle);
  if (angle == pi || angle == 0)
      Ex = W;
      Ey = Sy;
      Sx = 1;
  elseif (angle == pi/2)
      Ey = 1;
      i = (Sx-1)*H+[Ey:Sy];
      return;
  elseif k&gt;0 &amp; k &lt; (Sy-1)/(W-Sx) 
      Ex = W;
      Ey = round(Sy-tan(angle)*(Ex-Sx));
  elseif k &lt; 0 &amp; abs(k) &lt; (Sy-1)/(Sx-1) 
      Ex = 1;
      Ey = round(Sy-tan(angle)*(Ex-Sx));
  else
      Ey = 1;   
      Ex = round((Sy-1)/tan(angle)+Sx);
  end
  Dx = Ex - Sx;
  Dy = Ey - Sy;
  iCoords=1;
  if(abs(Dy) &lt;= abs(Dx))
      if(Ex &gt;= Sx)
          D = 2*Dy + Dx;
          IncH = 2*Dy;
          IncD = 2*(Dy + Dx);
          X = Sx;
          Y = Sy;
          i(iCoords) = (Sx-1)*H+Sy;
          iCoords = iCoords + 1;
          while(X &lt; Ex)
              if(D &gt;= 0)
                  D = D + IncH;
                  X = X + 1;
              else
                  D = D + IncD;
                  X = X + 1;
                  Y = Y - 1;
              end
              i(iCoords) = (X-1)*H+Y;
              iCoords = iCoords + 1;                
          end
      else 
          D = -2*Dy + Dx;
          IncH = -2*Dy;
          IncD = 2*(-Dy + Dx);
          X = Sx;
          Y = Sy;
          i(iCoords) = (Sx-1)*H+Sy;
          iCoords = iCoords + 1;
          while(X &gt; Ex)
              if(D &lt;= 0)
                  D = D + IncH;
                  X = X - 1;
              else
                  D = D + IncD;
                  X = X - 1;
                  Y = Y - 1;
              end
              i(iCoords) = (X-1)*H+Y;
              iCoords = iCoords + 1;   
          end
      end
  
  
  else 
      Tmp = Ex;
      Ex = Ey;
      Ey = Tmp;
      Tmp = Sx;
      Sx = Sy;
      Sy = Tmp;
      Dx = Ex - Sx;
      Dy = Ey - Sy;
      if(Ex &gt;= Sx)
          D = 2*Dy + Dx;
          IncH = 2*Dy;
          IncD = 2*(Dy + Dx);
          X = Sx;
          Y = Sy;
          i(iCoords) = (Sy-1)*H+Sx;
          iCoords = iCoords + 1;
          while(X &lt; Ex)
              if(D &gt;= 0)
                  D = D + IncH;
                  X = X + 1;
              else
                  D = D + IncD;
                  X = X + 1;
                  Y = Y - 1;
              end
              i(iCoords) = (Y-1)*H+X;
              iCoords = iCoords + 1;   
          end
      else
          D = -2*Dy + Dx;
          IncH = -2*Dy;
          IncD = 2*(-Dy + Dx);
          X = Sx;
          Y = Sy;
          i(iCoords) = (Sy-1)*H+Sx;
          iCoords = iCoords + 1;
          while(X &gt; Ex)
              if(D &lt;= 0)
                  D = D + IncH;
                  X = X - 1;
              else
                  D = D + IncD;
                  X = X - 1;
                  Y = Y - 1;
              end
              i(iCoords) = (Y-1)*H+X;
              iCoords = iCoords + 1;   
          end
      end        
  end
  </code></pre>