estimate_dem_robust

% robustly estimate correction to dem

 Optimization function using curvature for regularization
   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

 [A,weights,dx,v,W,Nmatrix]=...
   estimate_dem_robust...
    (N,U,Np,Nr,Mc,poi,xa,weights,sigma_k,iter,iter_switch,type_robust,L1,...
    out_in,print_type,plot_type)

 N           = number of observations (points+second derivatives)
 U           = number of unknowns (dem)
 Np          = number of points
 Nr          = number of rows
 Mc          = number oc columns 
 poi         = Npx4 matrix of points (x,y,h,sigma), x,y, referring to grid
 xa          = approximate values for dem
 weights     = Nx1 vector of weights in [0,1]
 sigma_k     = standard deviation of second derivatives
 iter        = current iteration for controling weighting
 iter_switch = iteration number where to switch type of weighting
 type_robust = 0,1,2,3, (00,01,10,11) for points and dem
 L1          = 0/1 choice of weight function (0 = exponential, 1 = L1)
 out_in      = boolean Np-vector indicating inliers
 print_type  = 0,1,2
 plot_type   = 0,1,2

 A         = NxU sparse design matrix
 weights   = new weights
 dx        = delta dem
 v         = normalized residuals
 W         = weigths of obs. combining uncertainty and robust factor
 Nmatrix   = normal equation matrix

 wf 7/2014, 2/2018

0001 %% robustly estimate correction to dem
0002 %
0003 % Optimization function using curvature for regularization
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 % [A,weights,dx,v,W,Nmatrix]=...
0010 %   estimate_dem_robust...
0011 %    (N,U,Np,Nr,Mc,poi,xa,weights,sigma_k,iter,iter_switch,type_robust,L1,...
0012 %    out_in,print_type,plot_type)
0013 %
0014 % N           = number of observations (points+second derivatives)
0015 % U           = number of unknowns (dem)
0016 % Np          = number of points
0017 % Nr          = number of rows
0018 % Mc          = number oc columns
0019 % poi         = Npx4 matrix of points (x,y,h,sigma), x,y, referring to grid
0020 % xa          = approximate values for dem
0021 % weights     = Nx1 vector of weights in [0,1]
0022 % sigma_k     = standard deviation of second derivatives
0023 % iter        = current iteration for controling weighting
0024 % iter_switch = iteration number where to switch type of weighting
0025 % type_robust = 0,1,2,3, (00,01,10,11) for points and dem
0026 % L1          = 0/1 choice of weight function (0 = exponential, 1 = L1)
0027 % out_in      = boolean Np-vector indicating inliers
0028 % print_type  = 0,1,2
0029 % plot_type   = 0,1,2
0030 %
0031 % A         = NxU sparse design matrix
0032 % weights   = new weights
0033 % dx        = delta dem
0034 % v         = normalized residuals
0035 % W         = weigths of obs. combining uncertainty and robust factor
0036 % Nmatrix   = normal equation matrix
0037 %
0038 % wf 7/2014, 2/2018
0039 %
0040 
0041 function [A,weights,dx,v,W,Nmatrix]=...
0042     estimate_dem_robust...
0043     (N,U,Np,Nr,Mc,poi,xa,weights,sigma_k,iter,iter_switch,type_robust,L1,...
0044     out_in,print_type,plot_type);
0045     
0046 typec=1;
0047 
0048 %% factor for update weights indicating outliers
0049 k= 2;
0050 
0051 %% start estimation
0052 % initiate size of A
0053             if print_type > 0
0054                 start_time_sol = cputime;
0055             end
0056 A = spalloc(N,U,6*N);
0057 b = zeros(N,1); 
0058 W = spalloc(N,N,N);
0059 Ac = spalloc(N,U,6*N);
0060 bc = zeros(N,1); 
0061 Wc = spalloc(N,N,N);
0062 
0063         % observations, normalized with 1/sqrt of variances
0064         tic
0065         for p = 1:Np
0066             x = poi(p,1);
0067             y = poi(p,2);
0068             w = 1/poi(p,4)*sqrt(weights(p))*out_in(p);
0069             i = floor(x);
0070             j = floor(y);
0071             aa = x-i;
0072             bb = y-j;
0073             maa = 1-aa;
0074             mbb=  1-bb;
0075             g1=maa*mbb;
0076             g2=aa*mbb;
0077             g3=maa*bb;
0078             g4=aa*bb;
0079 %             %delta_x
0080 %             A(p,i+j*Nr+1)       = (1-aa)*(1-bb)*w;
0081 %             A(p,i+j*Nr+2)       =    aa *(1-bb)*w;
0082 %
0083 %             A(p,i+(j+1)*Nr+1)   = (1-aa)* bb   *w;
0084 %             A(p,i+(j+1)*Nr+2)   =    aa * bb   *w;
0085 %             delta_l =  poi(p,3) - ...
0086 %                 ((1-aa)*(1-bb)*xa(i+j*Nr+1)    +...
0087 %                 aa *(1-bb)*xa(i+j*Nr+2)    +...
0088 %                 (1-aa)* bb   *xa(i+(j+1)*Nr+1)+...
0089 %                 aa * bb   *xa(i+(j+1)*Nr+2));
0090             A(p,i+j*Nr+1)       = g1*w;
0091             A(p,i+j*Nr+2)       = g2*w;
0092             A(p,i+(j+1)*Nr+1)   = g3*w;
0093             A(p,i+(j+1)*Nr+2)   = g4*w;
0094             delta_l =  poi(p,3) - ...
0095                 (g1*xa(i+j*Nr+1)    +...
0096                  g2*xa(i+j*Nr+2)    +...
0097                  g3*xa(i+(j+1)*Nr+1)+...
0098                  g4*xa(i+(j+1)*Nr+2));
0099             b(p)                =   delta_l * w;
0100             W(p,p)=w;
0101         end
0102         
0103         time_for_A_points=toc
0104 
0105 
0106             if print_type > 0
0107                 time_for_building_An=toc
0108             end
0109 
0110 %% priors
0111 tic
0112 Icc = Np;                    % first index for column curvatures
0113 Irr = Np + (Nr-2)*Mc;        % first index for row curvature
0114 Irc = Np + (Nr-2)*Mc + Nr*(Mc-2);    % first index for torsion
0115 % coefficients for column curvature
0116         % cc  1
0117         %    -2
0118         %     1
0119 Jcc = [-1,0,+1];  % index vector
0120 Vcc = [ 1 -2 1];                    % coefficients
0121 wcc = 1/sigma_k;%/sqrt(6);  % /norm(Vcc)
0122 % coefficients for row curvature
0123         % rr  1 -2  1
0124 Jrr = [-Nr,0,+Nr];  % index vector
0125 Vrr = [1 -2  1];                    % coefficients
0126 wrr = 1/sigma_k;% /sqrt(6); % /norm(Vrr)
0127 % coefficients for torsion
0128         % rc  1  -1
0129         %    -1   1
0130 %                  ^
0131 Jrc = [-Nr-1,-Nr,-1,0];      % indices
0132 Vrc = [1 -1 -1 1];                % coefficients
0133 %wrc = 1/sigma_k*2;  %/sqrt(2); %/2; % /norm(Vrc)
0134 wrc = 1/sigma_k/sqrt(2);      % quadratic variation
0135 % built up of priors in A
0136 % column curvature
0137 for j=1:Mc
0138     for i=2:Nr-1
0139         IU  = i+(j-1)*Nr;
0140         %[i,j,IU]
0141         Icc = Icc+1;
0142         A(Icc,IU+Jcc) = Vcc * wcc * sqrt(weights(Icc));
0143         delta_cc      = 0-xa(IU+Jcc)' * Vcc';
0144         b(Icc)        = delta_cc * wcc * sqrt(weights(Icc));
0145         W(Icc,Icc)    = wcc * sqrt(weights(Icc));
0146     end
0147 end
0148 
0149 % row curvature
0150 for j=2:Mc-1
0151     for i=1:Nr
0152         IU  = i+(j-1)*Nr;
0153         %[i,j,IU]
0154         Irr = Irr+1;
0155         A(Irr,IU+Jrr) = Vrr * wrr * sqrt(weights(Irr));
0156         delta_rr      = 0-xa(IU+Jrr)' * Vrr';
0157         b(Irr)        = delta_rr * wrr * sqrt(weights(Irr));
0158         W(Irr,Irr)    = wrr * sqrt(weights(Irr));
0159     end
0160 end
0161 
0162 
0163 
0164 % torsion
0165 for j=2:Mc
0166     for i=2:Nr
0167         IU  = i+(j-1)*Nr;
0168         %[i,j,IU]
0169         Irc = Irc+1;
0170         A(Irc,IU+Jrc) = Vrc * wrc * sqrt(weights(Irc));
0171         delta_rc      = 0-xa(IU+Jrc)' * Vrc';
0172         b(Irc)        = delta_rc * wrc * sqrt(weights(Irc));
0173         W(Irc,Irc)    = wrc * sqrt(weights(Irc));
0174     end
0175 end
0176 time_for_A_prior=toc
0177 %
0178             if print_type > 1
0179                 if U < 1000
0180                     Ab = 2*[full(A)]
0181                     null_A_prior = U-rank(full(A(U+1:end,:)));
0182                     rankA = rank(full(A(U+1:end,:)));
0183                 end
0184 
0185 
0186             end
0187 %time_for_building_Ae=toc
0188 %% solve
0189 
0190 % sort
0191 tic
0192 Nmatrix  = A'*A;
0193 
0194 
0195             if iter==1 && plot_type > 1
0196                 size_N_matrix = size(Nmatrix);
0197                 non_zeros_N_matrix =nnz(Nmatrix);
0198                 figure
0199                 subplot(1,3,1)
0200                 spy(Nmatrix)
0201                 title('N')
0202             end
0203 
0204 permx    = symamd(Nmatrix);
0205 Aperm    = A(:,permx);
0206 
0207 
0208 
0209 
0210             if iter==1 && plot_type > 1
0211                 subplot(1,3,2)
0212                 spy(Aperm'*Aperm)
0213                 title('N, sorted')
0214             end
0215 
0216 
0217 [C,R]      = qr(Aperm,b,0);
0218 
0219 dxperm   = R\C;
0220 Pm       = speye(U);
0221 Pm       = Pm(:,permx);
0222 dx       = Pm*dxperm;
0223 
0224 
0225 
0226                 time_for_QR_sorted=toc
0227 
0228             if iter==1 && plot_type > 1
0229                 subplot(1,3,3)
0230                 spy(R)
0231                 fill_in = nnz(R)-(nnz(Nmatrix)/2+U/2)
0232                 percentage_nnz   = nnz(R)/(U*(U+1)/2)
0233                 relative_fill_in = fill_in/(nnz(Nmatrix)/2+U/2)
0234                 title(strcat('reduced N sorted, fill in =',num2str(fill_in)))
0235             end
0236 
0237 
0238             if iter==1 && plot_type > 1
0239                 figure
0240                 subplot(1,2,1)
0241                 spy(A)
0242                 title('A')
0243                 subplot(1,2,2)
0244                 spy(Aperm)
0245                 title('A sorted')
0246             end
0247 
0248 dx_sorted = dx;
0249 
0250             if U < 1600 && plot_type > 0
0251                 Aqt = R\Aperm';
0252                 figure
0253                 spy(Aqt')
0254                 title('R\A sorted')
0255             end
0256 
0257             if print_type > 0
0258                 time_for_solution=cputime-start_time_sol
0259             end
0260 
0261 % residuals (normalized)
0262 v = A*dx-b;
0263 
0264 
0265 
0266 % estimated variance factor
0267 eso = norm(v)/sqrt(N-U);
0268 
0269 if type_robust > 0
0270     
0271     
0272     % original non-normalized residuals
0273     vori  = v;
0274     vori(1:Np)     = (v(1:Np).*poi(1:Np,4)./sqrt(weights(1:Np)));
0275     vori(Np+1,end) = (v(Np+1,end)*sigma_k./sqrt(weights(Np+1,end)));
0276     vs=vori;
0277     %vs=v;
0278         
0279     % sigma_0 for points
0280     eso_p = median(abs(v(1:Np)))*1.48;
0281     %es_p  = median(abs(vs(1:Np)))*1.48;
0282     
0283     % sigma_0 for curvatures
0284     eso_k = median(abs(v(Np+1:end)))*1.48;
0285     %es_k  = median(abs(vs(Np+1:end)))*1.48;
0286                 if print_type > 0
0287                     est_s0_s0p_s0k=[eso,eso_p,eso_k]
0288                 end
0289 
0290     check_AtV_null=norm((A'*v));
0291     max_v_points = max(abs(v(1:Np)));
0292     max_v_curvat = max(abs(vs(Np+1,end)));
0293 
0294     % critical values
0295     kp = k*eso_p;
0296     kk = k*eso_k;
0297 %     kp = k*es_p;
0298 %     kk = k*es_k;
0299    
0300     %kp = k;
0301     
0302     
0303                 if print_type > 0
0304                     criticalvalue_kp_sigmap_kk_sigma_k=[kp,poi(1,4),kk,sigma_k]
0305                 end
0306 end
0307 
0308 if iter == 1 && type_robust > 0 
0309     weights=ones(N,1);
0310 else
0311     if type_robust > 0 && L1
0312         % dem
0313         if type_robust == 1 || (type_robust == 3 && iter > 2*iter_switch)
0314             %weights(Np+1:end) = min(1,1./(abs(vs(Np+1:end))/kk+eps));
0315             weights(Np+1:end) = min(1,1./(abs(v(Np+1:end))/kk+eps));
0316         end
0317         % points
0318         if type_robust == 2 || (type_robust ==3 && iter <= iter_switch)
0319             weights(1:Np) = min(1,1./(abs(v(1:Np))/kp+eps)).*out_in(1:Np);
0320         end
0321     else
0322         if type_robust == 1 || (type_robust == 3 && iter > 2*iter_switch)
0323             %weights(Np+1:end) = exp(-abs(vs(Np+1:end)).^2/kk^2/2)+0.0001;
0324             weights(Np+1:end) = exp(-abs(v(Np+1:end)).^2/kk^2/2)+0.0001;
0325             %weights(Np+1:end) = exp(-abs(vs(Np+1:end))./kk+0.0001);
0326         end
0327         if type_robust == 2 || (type_robust ==3 && iter <= iter_switch)          
0328             weights(1:Np) = (exp(-abs(v(1:Np)).^2/kp^2/2)+0.0001).*out_in(1:Np);
0329         end
0330     end
0331 end
0332 %iter=iter
0333 abs_dx = norm(dx);
0334 min_w_p=min(weights(1:Np));
0335 min_w_c=min(weights(Np+1:end));
0336 
0337             if print_type > 1
0338                full(inv(W)*A)
0339             end
0340 
0341 return