%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function do_DTM_CHM lataa hyytialan datat ja tekee niistä CHM        
% (c) Juha Hyyppä; Code changes Ilkka Korpela : This routine computes DTM, DSM, CHM and finds local maxima of CHM.
% The obtained raster dtm is stored as binary file with a HDR-file; these can be read into KUVAMITT.
% The local maxima of CHM are stored into an ASCII-file, which can be used for superimposing points on the aerial images
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%     

% Set the directory where data and m-files are
cd c:\temp\lidar

% Clear any matlab arrays from memory
clear

% Load the full lidar ASCII-data
A=load('lidar_pisteet_LH_metsa.txt');

%*****************
% First pulse data
%*****************
% Determine minima and maxima of x,y of first pulse data
x_lower=min(A(:,9))-1
y_lower=min(A(:,10))-1
x_up=max(A(:,9))+1
y_up=max(A(:,10))+1

% Dimensions of the grids
x_size=ceil(x_up-x_lower)*2
y_size=ceil(y_up-y_lower)*2

% Malli with "two layers" will hold the minima and maxima of Z
malli=zeros(x_size,y_size,2);

% Assign minima with value "500" m
malli(:,:,1)=500;

% Obtain the number of lidar pulses
l=length(A);

% Find minima and maxima of Z for each cell and place into malli()
for i=1:l
    x_coord=A(i,9)-x_lower;
    y_coord=A(i,10)-y_lower;
    z_coord=A(i,11);
    x_coord=fix(x_coord*2+0.5);
    y_coord=fix(y_coord*2+0.5);
    if x_coord<x_size+1 & y_coord<y_size+1 & x_coord>0 & y_coord>0
      % maxima
      if malli(x_coord,y_coord,1)>z_coord
         malli(x_coord,y_coord,1)=z_coord;
      end
      % minima
      if malli(x_coord,y_coord,2)<z_coord
         malli(x_coord,y_coord,2)=z_coord;
     end  
    end
end

%%%%%%%%%%%%%
% Last pulses
%%%%%%%%%%%%%
% Find minima and maxima of Z for each 0.5 m cell and place these into malli()
% Recall, malli has lots of no-data values

l=length(A);
for i=1:l
    x_coord=A(i,14)-x_lower;
    y_coord=A(i,15)-y_lower;
    z_coord=A(i,16);
    x_coord=fix(x_coord*2+0.5);
    y_coord=fix(y_coord*2+0.5);
    if x_coord<x_size+1 & y_coord<y_size+1 & x_coord>0 & y_coord>0
      if malli(x_coord,y_coord,1)>z_coord
         malli(x_coord,y_coord,1)=z_coord;
      end
      if malli(x_coord,y_coord,2)<z_coord
         malli(x_coord,y_coord,2)=z_coord;
     end  
    end
end


% Now malli holds the minima and maxima and no-data values assigned either at "500 m" or "0 m"
% Get dimensions of malli
[col,row,d]=size(malli);

% Declare matrix a with zero-valued elements. Its size equals the size of the grids
a=zeros(col,row);

% Copy the minima and no-data values (500) (minima layer of malli() )  into a new array osa()
osa=malli(:,:,1);
% Display it, all "red elements" equal to 500 m, press return to continue
imagesc(osa)
colorbar
pause

% Now let's process the grid osa() with minima into a DTM

[x,y]=size(osa);
for i=1:x
   for j=1:y
      % For a DTM element (i,j); find the minima in a 21x21 neighborhood, store it in a()
      % Here we should omit the "500" values; having a large window makes probability of getting minima at 500 low.
      if i>10 & j>10 & i<x-10 & j<y-10
               taul=osa(i-10:i+10,j-10:j+10);
           a(i,j)=min(min(taul));
      end
      b=0;
      % For the same DTM element, find the minima in a smaller neighborhood, store its value in scalar b
      % To avoid cases "b = 500" keep this window large, 5x5 seems to perform well, 3x3 causes the grid to leak (no data, NaN-values)  in the upper right corner
      if i>5 & j>5 & i<x-5 & j<y-5
         taul=osa(i-5:i+5,j-5:j+5);
         b=min(min(taul));
      end
      % Compare these two minima; if differerence is less than 1 m, "believe in b"
      if abs(a(i,j)-b)<1
         a(i,j)=b;
      end
      % Compare the obtained value with osa(), which stores the raw minima and "500" for no-data values
      % If difference is less than a meter, take the value from the original 0.5 m cell in osa() matrix
      if abs(osa(i,j)-a(i,j))<1
         a(i,j)=osa(i,j);
      end 
   end
end

% Compute difference of original and processed grids into ero()
ero=osa-a;
% Find the cell locations with small difference (< 1 m)
[x,y]=find(abs(ero)<1);
% Copy original elevations into vector z for points with small difference
z=osa(abs(ero)<1);
% Assign x1 and y1 values (1,2,..,rows); is here a problem? the dimensions of x1 and y1 do not match those of other surfaces?
[x1,y1]=meshgrid(1:col,1:row);
clear osa

% Interpolate points x,y,z into a dtm
dtm=griddata(x,y,z,x1,y1,'linear')';
imagesc(dtm)
colorbar
pause

% Process 2nd layer of malli() into a DSM, just take non-zero elements and filter them into a DSM
% Substract DTM from DSM to get CHM
[x,y]=find(malli(:,:,2)>0);
z=malli(malli(:,:,2)>0);
dsm=griddata(x,y,z,x1,y1,'linear')';
imagesc(dsm)
colorbar
pause
chm=dsm-dtm;
imagesc(chm)
colorbar
pause
mal=malli(:,:,2)-dtm;
clear malli
malli=mal;

% ****************************************************
% OUTPUT of a binary DTM-file for KUVAMITT
% The dtm() dimensions (row,col) are interpreted as  rows = easting, cols = northing
% Open a file into binary mode
fid = fopen('c:\data\dtm.bin','w');
% Get dimensions of dtm
[x,y] = size(dtm)
% x = rows, y = cols
% Loop columns and rows, and store the pixels of the DTM as binary "single precision", i.e. 4-byte floating point numbers
% The order must be reversed from column into row-order, because arrays are stored differently in C/C++/VB.
% Check if the pixels have finite (defined) values, if not, output elevation as 140 m a.s.l

for j = 1:y
 for i = 1:x
    if isfinite(dtm(i,j))
      fwrite(fid,dtm(i,j),'single');
    end
    if isnan(dtm(i,j))
      fwrite(fid,140,'single');
    end
 end
end
fclose(fid)

% The raster DEM's in KUVAMITT require an ASCII -hdr file with the num of cols, rows, XY-origin of model, cellsize, nodata value and string that refers to the binary file.
fid = fopen('c:\data\dtm.hdr','w');
count = fprintf(fid,'%d \r\n',x_size)
count = fprintf(fid,'%d \r\n',y_size)
count = fprintf(fid,'%10.2f \r\n',x_lower)
count = fprintf(fid,'%10.2f \r\n',y_lower)
count = fprintf(fid,'%6.2f \r\n',0.5)
count = fprintf(fid,'%6.2f \r\n',0)
count = fprintf(fid,'%s \r\n','c:\data\dtm.bin') ;
fclose(fid)


% *********************************
% Find trees as local maxima of chm
% *********************************
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function find_trees etsii annetusta tiedosta puiden keskipisteet
% käyttäen konvoluutiota ja 3x3 maksimin etsintää.
%
% Copyright Juha Hyyppä 5.3.2006
% Additions/alterations Ilkka Korpela
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[x,y]=size(chm);
latva=zeros(x,y);
chm2=chm;

% Prepare for a matrix puut()
puut=[];

% define 3x3 low pass (smoothing)) convolution kernel
filt=[1 4 1
      4 16 4
      1 4 1]/36;

% Run convolution i.e. perform low pass filtering twice!
% first
chm1=conv2(chm,filt);
% second pass
% chm3=conv2(chm1,filt);
% returm chm3 into chm1
% chm1=chm3;

% Find maxima in a larger, 5 x 5 neighborhood
for i=3:x-2
    for j=3:y-2
        piste=chm1(i,j);
        A=max(max(chm1(i-2:i+2,j-2:j+2)));
        if abs(piste-A)<0.00001
            %oletettu latvapiste
            latva(i,j)=1;
            chm2(i-1,j-1)=100;
            puut=[puut
                 i j chm(i,j)];
        end
    end
end   
% Plot CHM, CHM with trees, and histogram of z-values of "found trees" in three separate figures
figure(1)
imagesc(chm)
colorbar
figure(2)
imagesc(chm2)
figure(3)
hist(puut(:,3),20);
pause


% **************************
% Print the results for projecting the found points on aerial images in KUVAMITT
% ASCII-file
% dtm (i,j) stores elevation of terrain
% puut() stores the (i,j) coordinates and "tree height" as third element
% x_lower stores the x-coordinate of all surfaces when i=1
% y_lower stores the y-coordinate of all surfaces when j=1
% length(puut) gives the number of maxima (tree tops)
% .5 m is the cell size in x (i-direction)
% .5 m is the cell size in y (j-direction)

% Open ASCII file for output; lets make a tree-file for KUVAMITT
fid = fopen('c:\data\lidar_puut.txt','w');
% Start with seven lines of zeros; i.e. the "plot" rotation, XYZ origin of plot, and shifts in XYZ.
% These are all zeros since data is directly in the 3D object coordinate frame.
for k = 1:7
 count = fprintf(fid,'%d \r\n',0) ;
end

% Tree-records in kuvamitt have (x,y,z,d13,h,id,sp,status) record structure, loop all trees in puut()
% and print the records. Make all trees into living (status=11) pines (sp=1)

for k = 1:length(puut)
 id = k;
 xbutt = x_lower + puut(k,1) * .5 - 1 ;
 ybutt = y_lower + puut(k,2) * .5 - 1  ;
 zbutt = dtm(puut(k,1),puut(k,2));
 d13 = 0.2;
 h = puut(k,3);
 sp = 1;
 status = 11;
 count = fprintf(fid,'%10.2f,%10.2f,%5.2f,%5.2f,%5.2f,%d,%d,%d \r\n', xbutt,ybutt,zbutt,d13,h,id,sp,status) ;
% pause
end
fclose(fid)

% In kuvamitt, the raster DTM can be read by reading the DTM.hdr -file
% The tree tops can be superimposed using the CTRL-Q command and reading the "lidar_puut.txt" file


% ******************************************
% Section for testing which trees were found
% ******************************************
% Read the reference photogrammetric measurements
FotoTrees =load('LH_Metsa_IK_ref.txt');
FotoTreesIn=[];

% Define a plot center for a circular plot
X_cent=2514865
Y_cent=6860960

% Assign values 0 for counters 
InTrees = 0;
Found = 0;

% Open an ASCII file for output
fid = fopen('c:\data\lidar_osumat.txt','w');

% Loop thru all reference trees; is tree is inside the circular plot; test which lidar
% maxima in puut() array hits it. Hit is defined in a cylinder. Allow also lidar maxima
% outside circular plot to hit reference trees.

PlotRad = 20;

for k = 1:length(FotoTrees)
  EroXY = [X_cent-FotoTrees(k,2),Y_cent-FotoTrees(k,3)];
  % Check inclusion
  if norm(EroXY) < PlotRad
   InTrees = InTrees + 1;
   FotoTreesIn(InTrees,1) = FotoTrees(k,2);
   FotoTreesIn(InTrees,2) = FotoTrees(k,3);
   FotoTreesIn(InTrees,3) = FotoTrees(k,4);

   % Check all maxima, omit duplicates i.e. one tree can be hit by several maxima. Distorts performance measurement!
   for l = 1:length(puut)
    id = l;
    xbutt = x_lower + puut(l,1) * .5 - 1 ;
    ybutt = y_lower + puut(l,2) * .5 - 1  ;
    ztop = dtm(puut(l,1),puut(l,2)) + puut(l,3);
    EroXY = [xbutt-FotoTrees(k,2),ybutt-FotoTrees(k,3)];
    EroZ = [ztop-FotoTrees(k,4)];
    % Check if inside the cylinder
    if (norm(EroXY) < 2) & (abs(EroZ) < 5)
      Found = Found + 1;
      % This reference tree(k) was found, print reference data (id,X,Y,Z) and lidar maxima (id,X,Y,Z)
      count = fprintf(fid,'%d,%10.2f,%10.2f,%5.2f,%d,%10.2f,%10.2f,%5.2f \r\n', FotoTrees(k,1),FotoTrees(k,2), FotoTrees(k,3), FotoTrees(k,4), id, xbutt,ybutt,ztop) ;
    end 
   end
  end
end

fclose(fid)


% Compute how many lidar CHM maxima were there inside the plot
InMaxima = 0;
 for l = 1:length(puut)
    xbutt = x_lower + puut(l,1) * .5 - 1 ;
    ybutt = y_lower + puut(l,2) * .5 - 1  ;
    ztop = dtm(puut(l,1),puut(l,2)) + puut(l,3);
    EroXY = [xbutt-X_cent,ybutt-Y_cent];
    if norm(EroXY) < PlotRad
     InMaxima = InMaxima + 1;
    end
 end

InTrees
InMaxima
Found

% Look at the error vectors
C = load ('c:\data\lidar_osumat.txt');
% XY-errors
quiver(C(:,2),C(:,3),C(:,2)-C(:,6),C(:,3)-C(:,7),0);
pause
% XYZ-errors
quiver3(C(:,2),C(:,3),C(:,4),-(C(:,2)-C(:,6)),-(C(:,3)-C(:,7)),-(C(:,4)-C(:,8)),0);
pause
hold
% Add reference trees inside the plot into the same plot
quiver3(FotoTreesIn(:,1),FotoTreesIn(:,2),FotoTreesIn(:,3),0,0,0,0,'+');
hold