Home > 16-Surface-Reconstruction > Surfacereconstruction > Functions > estimate_dem_robust.m

estimate_dem_robust

PURPOSE ^

% robustly estimate correction to dem

SYNOPSIS ^

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

DESCRIPTION ^

% 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_curvature_dependent...
   (N,U,Np,Nr,Mc,poi,xa,weights,sigma_k,iter,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
 out_print = 0,1,2
 type_robust = 0,1,2,3, (00,01,10,11) for points and dem

 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

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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

Generated on Mon 08-Jan-2018 17:21:49 by m2html © 2005