function [image, K] = read_range_grid_ply(fname)
%   Copyright (c) 2013, William Smith (University of York)

% This is fragile (used for rooster dataset)
fid = fopen(fname, 'r');
% Read relevant values from header (assumes lines 3-5 contain the height, width and number of vertices respectively)
fgetl(fid);
fgetl(fid);
a = fgetl(fid);
b = fgetl(fid);
c = fgetl(fid);
fclose(fid);
height = sscanf(a,'%*s %*s %d');
width = sscanf(b,'%*s %*s %d');
nverts = sscanf(c,'%*s %*s %d');
npixels = height*width;
clear a b c

fid = fopen(fname,'r','ieee-le.l64');
% find the end of the header again (using ftell on the old handle doesn't give the correct position)   
   BufSize = 8192;
   Buf = [blanks(10),char(fread(fid,BufSize,'uchar')')];
   i = [];
   tmp = -11;
   
   while isempty(i)
   	i = strfind(Buf,['end_header',13,10]);			% look for end_header + CR/LF
   	i = cat(2,i,strfind(Buf,['end_header',10]));	% look for end_header + LF
      
      if isempty(i)
         tmp = tmp + BufSize;
         Buf = [Buf(BufSize+1:BufSize+10),char(fread(fid,BufSize,'uchar')')];
      end
   end
   
   % seek to just after the line feed
   fseek(fid,i + tmp + 11 + (Buf(i + 10) == 13),-1);
   % Read the XYZs, skipping over the RGBs
   xyz = fread(fid,[3,nverts],'3*float',3)';

%    figure; plot3(xyz(:,1),xyz(:,2),xyz(:,3),'.')
   
   % seek to just after first XYZ
   fseek(fid,i + tmp + 11 + (Buf(i + 10) == 13) + 3*4,-1);
   % Read the RGBs, skipping over the XYZs
   rgb = (fread(fid,[3,nverts],'3*uchar',3*4)')./255;

   % seek to just after vertex data
   fseek(fid,i + tmp + 11 + (Buf(i + 10) == 13) + nverts*(3*4+3),-1);

   % read binary mask and index positions
   indices=zeros(1,npixels);
   mask=zeros(1,npixels);
   for i=1:npixels
       mask(i) = fread(fid,1,'uchar');
       if (mask(i)==1)
           idx=fread(fid,1,'int');
           indices(i)=idx+1;
       end
   end
   fclose(fid);

   % Reshape to image proportions
   mask = rot90(reshape(mask,height,width),2);
   indices = rot90(reshape(indices,height,width),2);
   
   % Store image
   image = zeros(size(indices,1),size(indices,2),3);
   for row=1:height
      for col=1:width
         if mask(row,col)==1
            image(row,col,:)=rgb(indices(row,col),:);
         else
            image(row,col,:)=NaN;
         end
      end
   end
   
   % Calculate camera intrinsics
   A = zeros(2*nverts,5);
   b = zeros(2*nverts,1);
   count = 0;
   for row=1:height
      for col=1:width
         if mask(row,col)==1
            count = count+1;
            A(2*count-1,:)=[xyz(indices(row,col),1)/xyz(indices(row,col),3) 0 1 0 xyz(indices(row,col),2)/xyz(indices(row,col),3)];
            A(2*count,:)=[0 xyz(indices(row,col),2)/xyz(indices(row,col),3) 0 1 0];
            b(2*count-1,1)=col;
            b(2*count,1)=row;
         end
      end
   end
   % Compute perspective params: x=[phi_x phi_y delta_x delta_y psi]
   x=A\(b-0.5); % (half-pixel offset between pixel grid and image plane)
   K = [x(1) x(5) x(3); 0 x(2) x(4); 0 0 1];
end