smooth_dem_robust_bilinear

% smooth_dem_robust_bilinear

 reconstruct surface with 
   energy = sum_m rho(l_m-a_m)/sigma_n) + 
             1/sigma_d sum_ij d_ii^2+2d_ij^2+djj^2
 l_m = observed heights, interpolated bilinearly
 a_m unknown grid heights, d_ij second derivatives
 
 [dem_smoothed,S,Sigma,Np,Nr,Mc,v,A,weights,weights_f,W] = ...
     smooth_dem_robust_bilinear...
     (points,BB,delta_x,sigma_k,out_C,type_robust,...
     out_in,print_type,plot_type)

 points      = Nx4-matrix of (x,y,h,sigma) values, 
 BB          = [xmin,ymin,xmax,ymax] bounding box of square grid
 delta_x     = grid size, squared 
               xmax, ymax will be adapted, not decreased
               points not inside BB will be discarded
 sigms_k     = std. dev. of curvature of surface, referring to delta_x
 out_C       = 0 no covariance matrix as output, (default if U>1600)
               1 covariance matrix as output
 type_robust = 0,1,2 (00,01,10) for points and dem
 out_in      = boolean N-vector indicating inliers
 print_type  = 0 no output
               1 timing
               2 little output
               2 much output
 plot_type   = 0,1,2,3

 dem_smoothed = Nr x Mc matrix of smoothed heigths
 S            = corresponding standard deviations, if required
 Sigma        = full covariance matrix, if required
 Np           = number of points
 Nr, Mc       = number of grid points in x- and y-direction
 v            = residuals (Np + (Nr-2)*Mc + Nr*(Mc-2) + (Nr-1)*(Mc-1)) for
                points
                second derivatives cc
                second derivatives rr
                torsion
 A            = design matrix (sparse) 
 weights      = weight-factors after robust estimation in [0,1]
 weights_f    = weight-factors after robust estimation (0 or 1)
 W            = weight of observations including both, 
                 uncertainty and
 robust factor 

 grid is matrix Nr x Mc and stored column wise

 wf 7/2014, 4/2018

0001 %% smooth_dem_robust_bilinear
0002 %
0003 % reconstruct surface with
0004 %   energy = sum_m rho(l_m-a_m)/sigma_n) +
0005 %             1/sigma_d sum_ij d_ii^2+2d_ij^2+djj^2
0006 % l_m = observed heights, interpolated bilinearly
0007 % a_m unknown grid heights, d_ij second derivatives
0008 %
0009 % [dem_smoothed,S,Sigma,Np,Nr,Mc,v,A,weights,weights_f,W] = ...
0010 %     smooth_dem_robust_bilinear...
0011 %     (points,BB,delta_x,sigma_k,out_C,type_robust,...
0012 %     out_in,print_type,plot_type)
0013 %
0014 % points      = Nx4-matrix of (x,y,h,sigma) values,
0015 % BB          = [xmin,ymin,xmax,ymax] bounding box of square grid
0016 % delta_x     = grid size, squared
0017 %               xmax, ymax will be adapted, not decreased
0018 %               points not inside BB will be discarded
0019 % sigms_k     = std. dev. of curvature of surface, referring to delta_x
0020 % out_C       = 0 no covariance matrix as output, (default if U>1600)
0021 %               1 covariance matrix as output
0022 % type_robust = 0,1,2 (00,01,10) for points and dem
0023 % out_in      = boolean N-vector indicating inliers
0024 % print_type  = 0 no output
0025 %               1 timing
0026 %               2 little output
0027 %               2 much output
0028 % plot_type   = 0,1,2,3
0029 %
0030 % dem_smoothed = Nr x Mc matrix of smoothed heigths
0031 % S            = corresponding standard deviations, if required
0032 % Sigma        = full covariance matrix, if required
0033 % Np           = number of points
0034 % Nr, Mc       = number of grid points in x- and y-direction
0035 % v            = residuals (Np + (Nr-2)*Mc + Nr*(Mc-2) + (Nr-1)*(Mc-1)) for
0036 %                points
0037 %                second derivatives cc
0038 %                second derivatives rr
0039 %                torsion
0040 % A            = design matrix (sparse)
0041 % weights      = weight-factors after robust estimation in [0,1]
0042 % weights_f    = weight-factors after robust estimation (0 or 1)
0043 % W            = weight of observations including both,
0044 %                 uncertainty and
0045 % robust factor
0046 %
0047 % grid is matrix Nr x Mc and stored column wise
0048 %
0049 % wf 7/2014, 4/2018
0050 %
0051  
0052 function [dem_smoothed,S,Sigma,Np,Nr,Mc,v,A,weights,weights_f,W] = ...
0053     smooth_dem_robust_bilinear...
0054     (points,BB,delta_x,sigma_k,out_C,type_robust,...
0055     out_in_0,print_type,plot_type)
0056 
0057     tic
0058 
0059 %% number of points, preliminary --> final (which are in BB)
0060 [Npp,tmp] = size(points);
0061 
0062 % eliminate all points outside BB
0063 xmin = BB(1);
0064 ymin = BB(2);
0065 xmax = BB(3);
0066 ymax = BB(4);
0067 Nr =ceil((xmax-xmin)/delta_x)+1;
0068 Mc =ceil((ymax-ymin)/delta_x)+1;
0069 
0070 
0071 ind = find((points(:,1)>=BB(1))&(points(:,1)<BB(3))...
0072           &(points(:,2)>=BB(2))&(points(:,2)<BB(4)));
0073 Np  = size(ind,1);
0074 poi = [(points(ind,1:2)-ones(Np,1)*[xmin,ymin])/delta_x,...
0075              points(ind,3:4)];
0076 out_in=out_in_0(ind);
0077 
0078 
0079 %% number of unknowns and observations incl. prior
0080 U = Nr*Mc;
0081 N = Np + (Nr-2)*Mc + Nr*(Mc-2) + (Nr-1)*(Mc-1);
0082 
0083 %% Provide Meta data
0084 
0085 if print_type>0 
0086     display(['Number of points              ',num2str(Npp)])
0087     if Npp ~= Np
0088         display(['Number of points reduced from ',num2str(Npp),' to ',num2str(Np)])
0089     end    
0090     display(['Grid size                     ',num2str(Nr),' x ',num2str(Mc)])   
0091     display(['Number of unknowns            ',num2str(U)])
0092     display(['Number of observations        ',num2str(N)])
0093     if type_robust(1) > 0        
0094         display(['Robust estimation        '])
0095     else
0096         display(['Non-robust estimation        '])
0097     end
0098     
0099 end
0100 %% do not give covariance matrix for U>40401.
0101             if U > 40401
0102                 out_C = 0;
0103             end
0104             if print_type > 0 
0105                 tic
0106             end
0107 %% estimate iteratively
0108 
0109 iter_switch=3;
0110 if type_robust == 0
0111        N_iter = 1;
0112 else
0113        N_iter = 2*iter_switch; % 3 non-robust, 3 robust
0114 end
0115 
0116 xa        = zeros(U,1);               % initial unknown parameters
0117 weights   = ones(N,1);                % weight-factors for robust estimation
0118 weights_f = weights;
0119 sigma_k0  = 1*sigma_k;
0120 gain=(sigma_k/sigma_k0)^(1/(N_iter-1));
0121 
0122 time_for_checking=toc;
0123 
0124 startcputime=cputime;
0125 
0126 x_prev = zeros(U,1);
0127 
0128 for iter = 1:N_iter
0129     if print_type > 0
0130         display(['Iteration: ',num2str(iter),' ------------------- '])
0131     end
0132     xa = zeros(U,1);
0133     L1 = 0;
0134 
0135 if type_robust > 0 && iter <= iter_switch
0136     % the first three iterations use L1 (smoothed)
0137     L1=1;
0138 else
0139     % the last three iterations use the exponential reweighting
0140     L1=0;
0141 end
0142     
0143     if type_robust > 0
0144         sigma_k=sigma_k0*gain^(iter);     % adapt prior sigma
0145     end
0146     
0147     if iter == 1
0148         % store design matrix A and right hand sides b
0149         [A,b,permx,weights,xa,v,W,Nmatrix]=estimate_dem_robust_1...
0150             (N,U,Np,Nr,Mc,poi,weights,sigma_k,type_robust,...
0151             out_in,print_type,plot_type);
0152            
0153     else       
0154         % use design matrix A and right hand sides b
0155         [weights,xa,v,W,Nmatrix]=estimate_dem_robust_2_plus...
0156             (A,b,permx,N,U,Np,Nr,Mc,poi,weights,sigma_k,...
0157             type_robust,L1,out_in,print_type);
0158            
0159            
0160     end
0161     
0162      
0163     if print_type > 1
0164         iter_xa = [iter,norm(xa-x_prev)]
0165     end
0166     %% plot
0167                 if plot_type > 0
0168                     if iter == 1
0169                         if type_robust > 0
0170                             figure
0171                             hold on
0172                             X=([4*Nr:4*Nr+Nr-1]'*ones(1,Mc))*delta_x+BB(1);
0173                             Y=(ones(Nr,1)*[0:Mc-1])*delta_x+BB(2);
0174                             
0175                             imagesc(reshape(xa,Nr,Mc));
0176                             colormap(gray)
0177                             view(0,90)
0178                             axis equal;axis off
0179                             title('1.iteration, fitted dem as image','FontSize',16)
0180                           
0181                             xlim([0,70])
0182                             ylim([0,70])
0183                         end
0184                     else
0185                         figure
0186                         subplot(2,2,1)
0187                         hold on
0188                         X=([0:Nr-1]'*ones(1,Mc))*delta_x+BB(1);
0189                         Y=(ones(Nr,1)*[0:Mc-1])*delta_x+BB(2);
0190                         colormap(gray)
0191                         mesh(X,Y,reshape(xa,Nr,Mc),'EdgeColor',[0,0,0])
0192                         view([-29,65])
0193                         zlim([0,60])
0194                         grid off
0195 
0196                         title(strcat('iteration=',num2str(iter),' fitted dem'))
0197 
0198                         Icc = Np;                    % first index for column curvatures
0199                         Irr = Np + (Nr-2)*Mc;        % first index for row curvature
0200                         Irc = Np + (Nr-2)*Mc + Nr*(Mc-2);    % first index for torsion
0201                         subplot(2,2,2)
0202                         imagesc(reshape(weights(Icc+1:Irr),Nr-2,Mc));
0203                         % figure transposed ->
0204                         title(strcat('iteration=',num2str(iter),' weights z_r_r'))
0205                         
0206                         subplot(2,2,3)
0207                         if type_robust ~= 2
0208                             imagesc(reshape(weights(Irr+1:Irc),Nr,Mc-2));
0209                         else
0210                             rr=floor(sqrt(Np));
0211                             imagesc(reshape(1-weights(1:rr^2),rr,rr))
0212                         end
0213                         % figure transposed ->
0214                         title(strcat('iteration=',num2str(iter),' weights z_r_r'))
0215                         
0216                         subplot(2,2,4)
0217                         if type_robust ~= 2
0218                             imagesc(reshape(weights(Irc+1:end),Nr-1,Mc-1));
0219                         else
0220                             rr=floor(sqrt(Np));
0221                             imagesc(reshape(out_in(1:rr^2),rr,rr))
0222                         end
0223                         title(strcat('iteration=',num2str(iter),' weights z_r_c'))
0224                     end
0225                 end
0226     x_prev = xa;
0227 end
0228 if type_robust > 0
0229     %final estimate
0230     if print_type > 0
0231         display('Last iteration -----------------')
0232     end
0233     xa        = zeros(U,1);
0234     weights_f = 0.9999*ceil(weights-0.5)+0.0001;
0235     if print_type > 2
0236         weights_final = weights_f(1:20)' 
0237     end
0238     xa=zeros(length(xa),1);
0239     
0240     % use design matrix A and right hand sides b
0241     [weights_f,dx,v,W,Nmatrix]=estimate_dem_robust_2_plus...
0242         (A,b,permx,N,U,Np,Nr,Mc,poi,weights_f,sigma_k,0,0,...
0243         out_in,print_type);
0244        
0245     %xa = xa + dx;
0246     xa = dx;
0247                 if print_type > 1
0248                     iter_dx = [iter+1,norm(dx)] 
0249                 end
0250 
0251                 if plot_type > 0
0252                 figure
0253                     subplot(2,2,1)
0254                     hold on
0255                     X=([0:Nr-1]'*ones(1,Mc))*delta_x+BB(1);
0256                     Y=(ones(Nr,1)*[0:Mc-1])*delta_x+BB(2);
0257                     colormap(gray)
0258                     mesh(X,Y,reshape(xa,Nr,Mc),'EdgeColor',[0,0,0])%
0259                     %axis equal
0260                     view([-29,65])
0261                     zlim([0,60])
0262                     grid off
0263                     %alpha(0.3);
0264                     %mesh(ds);
0265                     %plot3(points(:,1),points(:,2),points(:,3),'.r','Marker
0266                     %Size',5);
0267 
0268                     title(strcat('last iteration, fitted dem'))
0269 
0270                     Icc = Np;                    % first index for column curvatures
0271                     Irr = Np + (Nr-2)*Mc;        % first index for row curvature
0272                     Irc = Np + (Nr-2)*Mc + Nr*(Mc-2);    % first index for torsion
0273                     subplot(2,2,2)
0274                     imagesc(reshape(weights_f(Icc+1:Irr),Nr-2,Mc));
0275                     % figure transposed ->
0276                     title(strcat('last iteration, weights z_c_c'))
0277                         
0278                     subplot(2,2,3)
0279                     if type_robust ~= 2
0280                         imagesc(reshape(weights_f(Irr+1:Irc),Nr,Mc-2));
0281                     else
0282                        rr=floor(sqrt(Np));
0283                        imagesc(reshape(1-weights_f(1:rr^2),rr,rr))
0284                     end
0285                     % figure transposed ->
0286                     title(strcat('last iteration, weights z_r_r'))
0287                         
0288                     subplot(2,2,4)       
0289                     if type_robust ~= 2
0290                        imagesc(reshape(weights_f(Irc+1:end),Nr-1,Mc-1));
0291                     else
0292                        rr=floor(sqrt(Np));
0293                        imagesc(reshape(out_in(1:rr^2),rr,rr))
0294                     end
0295                     title(strcat('last iteration, weights z_r_c'))
0296                 end
0297 end
0298 
0299 dem_smoothed = reshape(xa,Nr,Mc);
0300 
0301 %% determine covariance matrix for quality analysis
0302 Sigma=0;
0303 S = 0;
0304 
0305 if out_C > 0    
0306                   
0307     Sigma = sparseinv(Nmatrix);     
0308                 
0309     S     = full(reshape(diag(Sigma),Nr,Mc));
0310 end
0311 if print_type > 0
0312     display(['Total CPU time     = ',num2str(cputime-startcputime)])
0313 end
0314 return