estimate_dem_robust_1

% robustly estimate correction to dem, first iteration

 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
 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 (00,01,10) 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/204, 4/2018

0001 %% robustly estimate correction to dem, first iteration
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 % weights     = Nx1 vector of weights in [0,1]
0021 % sigma_k     = standard deviation of second derivatives
0022 % iter        = current iteration for controling weighting
0023 % iter_switch = iteration number where to switch type of weighting
0024 % type_robust = 0,1,2 (00,01,10) for points and dem
0025 % L1          = 0/1 choice of weight function (0 = exponential, 1 = L1)
0026 % out_in      = boolean Np-vector indicating inliers
0027 % print_type  = 0,1,2
0028 % plot_type   = 0,1,2
0029 %
0030 % A         = NxU sparse design matrix
0031 % weights   = new weights
0032 % dx        = delta dem
0033 % v         = normalized residuals
0034 % W         = weigths of obs. combining uncertainty and robust factor
0035 % Nmatrix   = normal equation matrix
0036 %
0037 % wf 7/204, 4/2018
0038 %
0039 
0040 function [A,b,permx,weights,dx,v,W,Nmatrix]=...
0041     estimate_dem_robust_1...
0042     (N,U,Np,Nr,Mc,poi,weights,sigma_k,type_robust,...
0043     out_in,print_type,plot_type)
0044 
0045 %% factor for update weights indicating outliers
0046 k= 3;
0047 
0048 %% start estimation
0049 % initiate size of A
0050 if print_type > 1
0051     start_time_sol = cputime;
0052 end
0053 
0054 A = spalloc(N,U,6*N);       % design matrix, setup once
0055 b = zeros(N,1);             % linearized observations, zero for prior
0056 %W = spalloc(N,N,N);
0057 W = zeros(N,1);
0058 
0059 if print_type > 0
0060     tic
0061 end
0062 for p = 1:Np
0063     x = poi(p,1);
0064     y = poi(p,2);
0065     w = 1/poi(p,4)*sqrt(weights(p))*out_in(p);
0066     i = floor(x);
0067     j = floor(y);
0068     aa = x-i;
0069     bb = y-j;
0070     maa = 1-aa;
0071     mbb=  1-bb;
0072     g1=maa*mbb;
0073     g2=aa*mbb;
0074     g3=maa*bb;
0075     g4=aa*bb;
0076     
0077     A(p,i+j*Nr+1)       = g1;
0078     A(p,i+j*Nr+2)       = g2;
0079     A(p,i+(j+1)*Nr+1)   = g3;
0080     A(p,i+(j+1)*Nr+2)   = g4;
0081     b(p)                =   poi(p,3);
0082     %W(p,p)=w;
0083     W(p)=w;
0084 end
0085 if print_type > 0
0086     display(['Time for A points  = ',num2str(toc)])
0087 end
0088 
0089 
0090 %% priors
0091 if print_type > 0
0092     tic
0093 end
0094 
0095 % switch typec
0096 %     case 0
0097 Icc = Np;                       % first index for column curvatures
0098 Irr = Np + (Nr-2)*Mc;                   % first index for row curvature
0099 Irc = Np + (Nr-2)*Mc + Nr*(Mc-2);       % first index for torsion
0100 % coefficients for column curvature
0101 % cc  1
0102 %    -2
0103 %     1
0104 Jcc = [-1,0,+1];                        % index vector
0105 Vcc = [ 1 -2 1];                        % coefficients
0106 wcc = 1/sigma_k;                        % /norm(Vcc)
0107 % coefficients for row curvature
0108 % rr  1 -2  1
0109 Jrr = [-Nr,0,+Nr];                      % index vector
0110 Vrr = [1 -2  1];                        % coefficients
0111 wrr = 1/sigma_k;                        % /norm(Vrr)
0112 % coefficients for torsion
0113 % rc  1  -1
0114 %    -1   1
0115 Jrc = [-Nr-1,-Nr,-1,0];                 % indices
0116 Vrc = [1 -1 -1 1];                      % coefficients
0117 wrc = 1/sigma_k/sqrt(2);                % quadratic variation
0118 % built up of priors in A
0119 % column curvature
0120 for j=1:Mc
0121     for i=2:Nr-1
0122         IU  = i+(j-1)*Nr;
0123         Icc = Icc+1;
0124         A(Icc,IU+Jcc) = Vcc;
0125         %W(Icc,Icc)    = wcc * sqrt(weights(Icc));
0126         W(Icc)    = wcc * sqrt(weights(Icc));
0127     end
0128 end
0129 
0130 % row curvature
0131 for j=2:Mc-1
0132     for i=1:Nr
0133         IU  = i+(j-1)*Nr;
0134         Irr = Irr+1;
0135         A(Irr,IU+Jrr) = Vrr;
0136         %W(Irr,Irr)    = wrr * sqrt(weights(Irr));
0137         W(Irr)    = wrr * sqrt(weights(Irr));
0138     end
0139 end
0140 
0141 % torsion
0142 for j=2:Mc
0143     for i=2:Nr
0144         IU  = i+(j-1)*Nr;
0145         Irc = Irc+1;
0146         A(Irc,IU+Jrc) = Vrc;
0147         %W(Irc,Irc)    = wrc * sqrt(weights(Irc));
0148         W(Irc)    = wrc * sqrt(weights(Irc));
0149     end
0150 end
0151 if print_type>0
0152     display(['Time for A priors  = ',num2str(toc)])
0153 end
0154 
0155 
0156 if print_type > 2
0157     if U < 1000
0158         Ab = 2*[full(A)]
0159         null_A_prior = U-rank(full(A(U+1:end,:)));
0160         rankA = rank(full(A(U+1:end,:)));
0161     end  
0162 end
0163 %% solve
0164 
0165 if print_type > 0
0166     tic
0167 end
0168 
0169 
0170 Aw =A;
0171 for u=1:U
0172     Aw(:,u)=W.*A(:,u);
0173 end
0174 Nmatrix = Aw'*Aw;
0175 
0176 if plot_type > 1
0177     size_N_matrix = size(Nmatrix);
0178     non_zeros_N_matrix =nnz(Nmatrix);
0179     figure
0180     subplot(1,3,1)
0181     spy(Nmatrix)
0182     title('N')
0183 end
0184 
0185 permx    = symamd(Nmatrix);
0186 Apermw   = Aw(:,permx);
0187 
0188 if plot_type > 1
0189     subplot(1,3,2)
0190     spy(Apermw'*Apermw)
0191     title('N, sorted')
0192 end
0193 
0194 %[C,R]    = qr(W*Aperm,W*b,0);
0195 
0196 [C,R]    = qr(Apermw,W.*b,0);
0197 dxperm   = R\C;
0198 Pm       = speye(U);
0199 Pm       = Pm(:,permx);
0200 dx       = Pm*dxperm;
0201 
0202 if plot_type > 1
0203     subplot(1,3,3)
0204     spy(R)
0205     fill_in = nnz(Rc)-(nnz(Nmatrix)/2+U/2)
0206     percentage_nnz   = nnz(R)/(U*(U+1)/2)
0207     relative_fill_in = fill_in/(nnz(Nmatrix)/2+U/2)
0208     title(strcat('reduced N sorted, fill in =',num2str(fill_in)))
0209 end
0210 
0211 if plot_type > 1
0212     figure
0213     subplot(1,2,1)
0214     spy(A)
0215     title('A')
0216     subplot(1,2,2)
0217     spy(Aperm)
0218     title('A sorted')
0219 end
0220 
0221 dx_sorted = dx;
0222 
0223 if U < 1600 && plot_type > 0
0224     Aqt = R\Apermw';
0225     figure
0226     spy(Aqt')
0227     title('R\A sorted')
0228 end
0229 
0230 if print_type > 1
0231     time_for_solution=cputime-start_time_sol
0232 end
0233 
0234 % residuals (non-normalized)
0235 v = A*dx-b;
0236 
0237 %eso = v*(W.^2)*v)/sqrt(N-U);
0238 eso = norm(W.*v)/sqrt(N-U);
0239 
0240 
0241 if type_robust > 0
0242     
0243     
0244     % normalized residuals
0245     %v= W*v;
0246     v= W.*v;
0247     
0248     
0249     
0250     % sigma_0 for points
0251     eso_p = median(abs(v(1:Np)))*1.48;
0252     
0253     % sigma_0 for curvatures
0254     eso_k = median(abs(v(Np+1:end)))*1.48;
0255     if print_type > 1
0256         est_s0_s0p_s0k=[eso,eso_p,eso_k]
0257     end
0258     
0259     
0260     % critical values
0261     kp = k*eso_p;
0262     kk = k*eso_k;
0263     
0264     
0265     if print_type > 1
0266         criticalvalue_kp_sigmap_kk_sigma_k=[kp,poi(1,4),kk,sigma_k]
0267     end
0268 end
0269 
0270 
0271 % dem
0272 if type_robust == 1 
0273     weights(Np+1:end) = min(1,1./(abs(v(Np+1:end))/kk+eps));
0274 end
0275 
0276 % points
0277 if type_robust == 2 
0278     weights(1:Np) = min(1,1./(abs(v(1:Np))/kp+eps)).*out_in(1:Np);
0279 end
0280 
0281 
0282 if print_type > 1
0283     full(A)
0284 end
0285 
0286 if print_type > 0
0287     display(['Time for solution  = ',num2str(toc)])
0288 end
0289 return