%%%%%%%%%%%%%%%%%
%---Code pour l'interpolation
%%%%%%%%%%%%%%%%%
xyz = mesh_ENU;%The reconstructed point cloud (3xNpoints) in the north-east-down system
xmin=-130;
xmax=50;
ymin=-130;
ymax=10;
dgrid=.05; %crucial question: what is the pixel size?

X  = xmin:dgrid:xmax;
Nx = length(X);

Y  = ymin:dgrid:ymax;
Ny = length(Y);

[XX,YY] = meshgrid(X,Y);
fprintf('DONE \n')

x = double(xyz(1,:)');
y = double(xyz(2,:)');
z = double(xyz(3,:)')+(S.Alt(imin)-Alt_GPS(imin)+Alt_Cameras);

    %%%%%%%%%%%
    %---Inspired from F.Leckler function
    %%%%%%%%%%%
P1 = importdata('P1cam.txt'); 
Npts = size(xyz,2);
 
pt2d = P1 * [xyz;ones(1,size(xyz,2))];
pt2d = pt2d ./ repmat( pt2d(3,:),3,1);
        
i = double(pt2d(1,:)');
j = double(pt2d(2,:)');
mask = ~isnan(x.*y.*z.*i.*j);

fprintf('Linear interpolation for Z ... ')
Fz = TriScatteredInterp(x(mask),y(mask),z(mask),'linear');
Z = Fz(XX,YY);
fprintf('DONE \n')
        
      
        % interpolation for iR
fprintf('Linear interpolation for iR ... ')
Fi = TriScatteredInterp(x(mask),y(mask),i(mask),'linear');
iR = Fi(XX,YY);
fprintf('DONE \n')
        
        % interpolation for jR
fprintf('Linear interpolation for jR ... ')
Fj = TriScatteredInterp(x(mask),y(mask),j(mask),'linear');
jR = Fj(XX,YY);
fprintf('DONE \n')
%%%%%%%%%%%%%%%
%----Plot
%%%%%%%%%%%%%%%
figure(56),clf
pcolor(iR,jR,Z)
shading flat
        