demo_estimate_two_symmetric_planes_from_points

% test_two_symmetric_planes_from_points

 example PCV Sect. 10.6.4, Fig. 10.29

 The parameter model_II is used to choose the estimation model

 % Wolfgang Förstner 7/2012
 wfoerstn@uni-bonn.de

0001 %% test_two_symmetric_planes_from_points
0002 %
0003 % example PCV Sect. 10.6.4, Fig. 10.29
0004 %
0005 % The parameter model_II is used to choose the estimation model
0006 %
0007 % % Wolfgang Förstner 7/2012
0008 % wfoerstn@uni-bonn.de
0009 
0010 %%
0011 close all
0012 clearvars
0013 clc
0014 
0015 addpath(genpath('../General-Functions'))
0016 addpath(genpath('Functions'))
0017 
0018 global min_redundancy
0019 global plot_option
0020 global print_option_estimation
0021 ss = plot_init;
0022 
0023 disp('---------------------------------------')
0024 disp(' Estimate parameters of symmetric roof ')
0025 disp(' example PCV Sect. 10.6.4, Fig. 10.29  ')
0026 disp('---------------------------------------')
0027 
0028 %% Choose estimation model
0029 
0030 % model_II = 0; % One step estimation GHM with
0031 model_II = 1; % Estimate individually, Model D + C
0032 
0033 switch model_II
0034     case 0
0035         disp('One step estimation GHM with constraints')
0036     case 1
0037         disp('Estimate individually, Model D + C')
0038 end
0039 
0040 % read coordinates according to PCV?
0041 read_PCV_coordinates = 0; % coordinates from book, only 4-digits
0042 % read_PCV_coordinates = 0; % generate random points
0043 
0044 %% initialize
0045 factor = 2;                  % for scaling the complete data set
0046 slope = -0.6;                % slope of roofs
0047 NL    = 5;                   % number of points on left roof
0048 NR    = 6;                   % number of points on left roof
0049 sigma =  factor*0.05;        % std. dev. of points
0050 maxiter = 5;                 % iteration for constraints
0051 Tol     = 0.0001;
0052 disp(strcat('Tolerance for convergence :', num2str(Tol)))
0053 
0054 rangeL = factor*[-1,0,0,2]; % x-range, y-range of roofs
0055 rangeR = factor*[ 0,1,0,2];
0056 
0057 plot_option = 1;
0058 print_option_estimation = 2;
0059 
0060 % initialize SUGR
0061 sugr_INIT
0062 min_redundancy = 100000;
0063 
0064 init_rand      = 3;          % random seed (default = 3)
0065 init_rand = init_rand_seed(init_rand);
0066 %% Generate planes
0067 disp('Generate planes...');
0068 % common horizontal line: height 1.5, in Y-direction
0069 X = [0,0,factor*1.5,1]';             % start points
0070 Y = [0,factor*1,factor*1.5,1]';      % end point
0071 L = calc_Pi(X)*Y;                    % roof line
0072 L_true = L;
0073 disp(['   Roof edge L = [',num2str(L'),']'])
0074 
0075 % slope, tan(alpha)
0076 X_L = X + factor*[1,0,-slope,0]';
0077 X_R = X + factor*[-1,0,-slope,0]';
0078 AL  = +calc_Pidual(X_L)'*L;
0079 AR  = -calc_Pidual(X_R)'*L;
0080 AL_true = AL;
0081 AR_true = AR;
0082 disp(['   Left roof plane AL  : [',num2str(AL'),']'])
0083 disp(['   Right roof plane AR : [',num2str(AR'),']'])
0084 
0085 %% generate points
0086 disp(' ')
0087 disp('Generate points...');
0088 if plot_option > 0    
0089     figure('Color','w','Position',[0.2*ss(1), 0.35*ss(2),ss(1)/2,ss(2)/2])
0090     hold on
0091     axis equal
0092 end
0093 
0094 disp('---Generate points on left plane')
0095 XL = generate_points_on_plane(NL,AL,sigma,rangeL,0,read_PCV_coordinates);
0096 disp('---Generate points on right plane')
0097 XR = generate_points_on_plane(NR,AR,sigma,rangeR,1,read_PCV_coordinates);
0098 
0099 xlim(factor*[-1.5,1.5]);
0100    ylim(factor*[-0.5,2.5]);
0101    zlim(factor*[0,2.5]);
0102    axis equal
0103 set(gca,'CameraPosition',[-14,31,27])
0104 
0105 %%  approximate values of the two planes: algebraic solution X_i' * A = 0
0106 [~,~,V] = svd(XL.h);  %
0107 ALa  =V(:,4);
0108 [U,D,V] = svd(XR.h);
0109 ARa = V(:,4);
0110 
0111 planes_appr = [ALa'/ALa(3);ARa'/ARa(3)];
0112 disp(['Approximate plane parameters left  : [',num2str(planes_appr(1,:)),']'])
0113 disp(['Approximate plane parameters right : [',num2str(planes_appr(2,:)),']'])
0114 
0115 % Possibly show coordinates
0116 % XL.h(:,1:3)./(XL.h(:,4)*[1,1,1]);
0117 % XR.h(:,1:3)./(XR.h(:,4)*[1,1,1]);
0118 
0119 %% Estimate complete roof
0120 disp(' ')
0121 disp('Perform estimation...');
0122 if model_II==1
0123     
0124     disp('-------- Estimate individually, Model D + C');
0125     
0126     %% First step: estimate individually using model D
0127     disp(' ')
0128     disp('ML-estimation left plane ---------------------------------------')
0129     [AL_est,sigma_0L,RL] = sugr_estimation_ml_Plane_from_points(XL,ALa,0.01,5);
0130     disp(' ')
0131     disp('ML-estimation right plane --------------------------------------')
0132     [AR_est,sigma_0R,RR] = sugr_estimation_ml_Plane_from_points(XR,ARa,0.01,5);
0133     disp(['   left plane: R = ',num2str(RL), ', sigma_0 = ',num2str(sigma_0L)]);
0134     disp(['   right plane: R = ',num2str(RR), ', sigma_0 = ',num2str(sigma_0R)]);
0135     
0136 %     AL_est.h/AL_est.h(3)
0137 %     AR_est.h/AR_est.h(3)
0138     
0139     disp(['   Estimated left plane  (1): ',num2str(AL_est.h'/AL_est.h(3))])
0140     disp(['   Estimated right plane (1): ',num2str(AR_est.h'/AR_est.h(3))])
0141     
0142     Crr  = [AL_est.Crr zeros(3)   ;...
0143             zeros(3)   AR_est.Crr];
0144 %     Crr_x_10000 = Crr*10000
0145     
0146     %% Second step: Impose constraints on plane parameters using model C
0147     % see PCV Sect. 4.8.2.6
0148     disp('-------- -------- Impose constraints: Model C');
0149     Nm  = zeros(2);
0150     hv  = zeros(2,1);
0151     l  = [AL_est.h;AR_est.h];   % observations == plane parameters
0152     l0 = l;
0153     AL = l(1:4);
0154     AR  =l(5:8); 
0155     for iter = 1:maxiter
0156 
0157         disp(['-------- -------- -------- C Iteration ',num2str(iter),'----------------'])
0158 
0159         AL0 = l0(1:4);                    % left plane A
0160         AR0 = l0(5:8);                    % right plane B
0161         est_L0 = calc_Pidual(AL0)*AR0;  % roof edge L
0162         eL0C = est_L0'/norm(est_L0(1:3));
0163 
0164         % Jacobian of constraints, see PCV (10.318)
0165         Bt=[2*[-AL0(1)*AR0(3)^2,...
0166             -AL0(2)*AR0(3)^2,...
0167             +AL0(3)*(AR0(1)^2+AR0(2)^2),...
0168             0]*null(AL0'),...
0169             2*[+AR0(1)*AL0(3)^2,...
0170             +AR0(2)*AL0(3)^2,...
0171             -AR0(3)*(AL0(1)^2+AL0(2)^2),...
0172             0]*null(AR0'),
0173             [[+AR0(2),-AR0(1),0,0]*null(AL0'),...
0174             [-AL0(2),+AL0(1),0,0]*null(AR0')]];                            %#ok<COMNL>
0175         % Jacobian for reducing the plane coordinates
0176         Jrh = [null(l0(1:4)')' zeros(3,4); zeros(3,4) null(l0(5:8)')'];
0177         % constraints,  see PCV (10.318)
0178         cg = [-(AL0(3)^2*(AR0(1)^2+AR0(2)^2)-AR0(3)^2*(AL0(1)^2+AL0(2)^2));...
0179             -(AL0(1)*AR0(2)-AR0(1)*AL0(2))] + Bt*Jrh*(l0-l);
0180         % normal equation matrix (4.463)
0181         Nm = Bt*Crr*Bt';
0182         % correction to fitted observations (4.465)
0183         Dlr = Crr * Bt' * inv(Nm) * cg + ...
0184             Jrh * l;                                                       %#ok<MINV>
0185         disp(['Correction of observations (reduced plane parameters):', num2str(Dlr')])
0186         
0187         % corrected fitted observations,  see PCV (10.255)
0188         l0(1:4)=sugr_ghm_update_vector(l0(1:4),Dlr(1:3)); 
0189         l0(5:8)=sugr_ghm_update_vector(l0(5:8),Dlr(4:6));
0190         
0191         if (abs(Dlr(:))./sqrt(diag(Crr)) < Tol) | (iter==maxiter)         %#ok<OR2,BDSCA>
0192             disp('-------- -------- -------- after last iteration')
0193             % covariance matrix of fitted observations
0194             % CovM(est_l) = CovM(l) - CovM(v), see (4.68) with (4.467)
0195             estCrr = Crr - Crr * Bt' * inv(Nm) * Bt * Crr;                 %#ok<MINV>
0196             ver = Jrh * (l0-l);
0197             Omega    = ver' * inv(Crr) * ver;                              %#ok<MINV>
0198             sigma_0 = sqrt(Omega/2);
0199             % redundancy is R=2 (2 constraints)
0200             disp(['    R = 2 sigma_0 = ',num2str(sigma_0)]);
0201             break
0202         end
0203     end
0204     est_AL = l0(1:4);
0205     est_AR = l0(5:8);
0206 
0207     disp(' ')
0208     disp(['Estimated left plane  : [',num2str(est_AL'/est_AL(3)),']'])
0209     disp(['Estimated lright plane: [',num2str(est_AR'/est_AR(3)),']'])
0210 
0211     est_L = calc_Pidual(est_AL)*est_AR;
0212     
0213     disp(['Estimated roof line   : [',num2str(est_L'/norm(est_L(1:3))),']'])
0214 
0215     % covariance matrix of planes (from reduced to homogeneous coord.)
0216     CestAestA = Jrh' * estCrr * Jrh;
0217 
0218     % covariance matrix of 3D line
0219     JLA = [-calc_Pidual(est_AR),calc_Pidual(est_AL)];
0220     CLL = JLA * CestAestA * JLA';
0221         
0222     % take vector L(1:3) as point in P^2, spherically normalized
0223     dir = sugr_Point_2D(est_L(1:3),CLL(1:3,1:3));
0224     % eigenvalues yield uncertainty of direction vector,
0225     % one ev is 0, the slope of the fitter roof edge
0226     % the other is the variance of the azimuth
0227     ev = eigs(dir.Crr)*180/pi;
0228     disp(['Std.dev. of azimuth (degrees): ', num2str(sqrt(max(ev(:))))])
0229     
0230 else
0231     % one step estimation GHM with constraints
0232     disp('-------- One-step estimation Model E')
0233     
0234     [estA,estX,sigma_0,R] = ...
0235         sugr_estimation_ml_symmetric_Planes_from_points...
0236         (XL,XR,[ALa;ARa],Tol,maxiter);
0237 
0238     disp('-------- -------- -------- after last iteration')
0239     
0240     disp(['Estimated sigma_0^2  :',num2str(sigma_0),'^2'])
0241     % fitted planes and points
0242     AL0 = estA.h(1:4);
0243     AR0 = estA.h(5:8);
0244     XL0 = estX(1:NL,:);
0245     XR0 = estX(NL+1:NL+NR,:);
0246     est_AL = AL0;
0247     est_AR = AR0;
0248     
0249     XLe = XL0(:,1:4)./(XL0(:,4)*[1,1,1,1]);
0250     XRe = XR0(:,1:4)./(XR0(:,4)*[1,1,1,1]);
0251 %     XLe*AL0/norm(AL0(1:3));
0252 %     XRe*AR0/norm(AR0(1:3));
0253 
0254     eAL = est_AL'/est_AL(3);
0255     eAR = est_AR'/est_AR(3);
0256     disp(['Estimated left plane  :',num2str(eAL)])
0257     disp(['Estimated right plane :',num2str(eAR)])
0258 
0259     est_L = calc_Pidual(est_AL)*est_AR;
0260     estL = est_L'/norm(est_L(1:3));
0261     disp(['Estimated roof line   :',num2str(estL)])
0262 
0263     % covariance matrix of 3D line
0264     JLA = [-calc_Pidual(est_AR),calc_Pidual(est_AL)];
0265     CLL = JLA * estA.Cxx * JLA';
0266     
0267     % take elements L(1:3) as direction on S^2 (see above)
0268     dir = sugr_Point_2D(est_L(1:3),CLL(1:3,1:3));
0269     ev = eigs(dir.Crr)*180/pi;
0270     disp(['Std.dev. of azimuth (degrees): ', num2str(sqrt(max(ev(:))))])
0271     
0272 %%
0273 if plot_option > 0
0274     figure('Color','w','Position',[0.2*ss(1), 0.1*ss(2),ss(1)/2,ss(2)/2])
0275     hold on
0276     r = rangeL;
0277     A = AL_true;
0278     z = -(A(1)*r(1:2) + A(2)*r(3:4) + A(4)) / A(3);
0279    
0280    plot3([r(1),r(2),r(2),r(1),r(1)],[r(3),r(3),r(4),r(4),r(3)],[z(1),z(2),z(2),z(1),z(1)],'--k');
0281    for n=1:NL
0282        plot3(XLe(n,1),XLe(n,2),XLe(n,3),'.k','MarkerSize',15);
0283    end
0284    
0285    r=rangeR;
0286    A=AR_true;
0287    z=-(A(1)*r(1:2) + A(2)*r(3:4) + A(4)) / A(3);
0288    plot3([r(1),r(2),r(2),r(1),r(1)],[r(3),r(3),r(4),r(4),r(3)],[z(1),z(2),z(2),z(1),z(1)],'--k');
0289    
0290    for n=1:NR
0291        plot3(XRe(n,1),XRe(n,2),XRe(n,3),'.k','MarkerSize',15);
0292    end
0293   
0294    xlim(factor*[-1.3,1.3]);
0295    ylim(factor*[-0.3,2.3]);
0296    zlim(factor*[0,2.5]);
0297    xlabel('X')
0298    ylabel('Y')
0299    zlabel('Z')
0300    axis equal
0301    
0302    set(gca,'CameraPosition',[-14,31,27])
0303 end
0304    
0305 end