demo_GHM_similarity

% GHM with similarity

 Wolfgang Förstner
 wfoerstn@uni-bonn.de 

 see GMM for similarity
 See also demo_GHM_2D_lines

0001 %% GHM with similarity
0002 %
0003 % Wolfgang Förstner
0004 % wfoerstn@uni-bonn.de
0005 %
0006 % see GMM for similarity
0007 % See also demo_GHM_2D_lines
0008 
0009 close all
0010 clearvars
0011 % clear all                                                                  %#ok<CLALL>
0012 
0013 addpath(genpath('../../../General-Functions/'));
0014 addpath(genpath('../Functions-GHM/'));
0015  
0016 simulate_GMM_similarity = true;  % choose transformed coordinates CovM=0
0017 %simulate_GMM_similarity = false; % choose transformed coordinates CovM~=0
0018 
0019  % initialization for random numbers
0020 init_rand = 63;            
0021 init_rand_seed(init_rand);
0022 
0023 %data_type = -10;         % type of generated data
0024 data_type =   2;         % type of generated data
0025 if data_type == 0
0026     data_type = -10;
0027 end
0028 
0029 % statistical parameters
0030 S = 0.99;                % Significance level
0031 tol = chi2inv(S,2);
0032 
0033 % controlling convergence
0034 Tx      = 0.00001; % threshold for convergence
0035 maxiter = 10;   % maximum number of iterations
0036 
0037 % selected parameters for sensititvity analysis
0038 %
0039 % 1,2 = translation (alternative choice)
0040 % 3,4 = rotation, scale
0041 rU = [3,4];              
0042 
0043 
0044 disp('============================================')
0045 disp(' Planar similarity with Gauss-Helmert model ')
0046 disp('--------------------------------------------')
0047 
0048 if simulate_GMM_similarity
0049     disp('Simulate similarity with Gauss-Markov model');
0050     disp('*******************************************')
0051 end
0052 
0053 %% model ------------------------------------------------------------------
0054 % xis = a xi - b yi + c
0055 % yis = b xi + a yi + d
0056 switch data_type
0057     case 1
0058         tm  = [  1, 1; ...
0059          2, 2; ...
0060         -1,-2; ...
0061         -2,-1 ...
0062        ];
0063     case 2
0064         tm  = [  -7, 7 ;...
0065             1, 1; ...
0066          2, 2; ...
0067         -1,-2; ...
0068         -2,-1; ...
0069        ];
0070    otherwise
0071         tm=rand(-data_type,2)*5;
0072 
0073 end
0074 % for plotting: bounding box
0075 bb   = [min(tm(:,1)),max(tm(:,1)),min(tm(:,2)),max(tm(:,2))];
0076 maxd = max(max(tm(:,1))-min(tm(:,1)),max(tm(:,2))-min(tm(:,2)));
0077 
0078 xt  = [2,0.5,3,-2]';
0079 disp('Parameters')
0080 disp('   [ x(1)     -x(2)      x(3) ]');
0081 disp('   [ x(2)      x(1)      x(4) ]');
0082 True_transformation = [xt(1) -xt(2) xt(3);xt(2) xt(1) xt(4)]                %#ok<NOPTS>
0083 
0084 % number of observations
0085 N   = 2*length(tm(:));
0086 % number of observations per correspondence
0087 d_I = 4;
0088 % number of correspondences = numbe rof points
0089 I   = N/d_I;
0090 Number_of_points = I                                                        %#ok<NOPTS>
0091 % number of unknown parameters
0092 U   = 4;
0093 % standard deviations for generation and estimation
0094 sigma_x = 0.01;                                                             %#ok<NASGU>
0095 sigma_y  = 0.01;
0096 if simulate_GMM_similarity
0097     sigma_x  = 0.00;
0098 end
0099 Standard_deviation_observations_m = [sigma_x,sigma_y]                       %#ok<NOPTS>
0100 
0101 %% generate observations --------------------------------------------------
0102 lm         = zeros(I,d_I);           % Ix4 matrix
0103 true_error = zeros(I,d_I);
0104 Cov_ll_m   = zeros(I,d_I^2);         % I x 16 matrix
0105 for i =1:I
0106     % random perturbations
0107     if simulate_GMM_similarity
0108          true_error(i,:)  = [[0 0],randn(1,2)*sigma_y]; 
0109     else
0110         true_error(i,:) = [randn(1,2)*sigma_x,randn(1,2)*sigma_y];          %#ok<UNRCH>
0111     end
0112     
0113     lm(i,:)= [tm(i,:),...
0114              ([xt(1) -xt(2) xt(3);xt(2) xt(1) xt(4)]*[tm(i,:)';1])'] ...
0115            + true_error(i,:);
0116     Am(2*i-1:2*i,:) = [tm(i,1) -tm(i,2) 1 0;...
0117                        tm(i,2)  tm(i,1) 0 1];
0118     Cov_ll_m(i,:)= [sigma_x^2*[1 0 0 0 0 1 0 0], ...
0119                     sigma_y^2*[0 0 1 0 0 0 0 1]];
0120 end
0121 true_errors = true_error                                                    %#ok<NOPTS>
0122 observed_coordinates = lm                                                   %#ok<NOPTS>
0123 
0124 %% factors for plotting ---------------------------------------------------
0125 factor_v  = 0.5*maxd/sqrt(N/2)/max([sigma_x,sigma_y]);
0126 factor_r  = 0.5*maxd/sqrt(N/2);
0127 factor_X  = maxd/sqrt(N/2)/6;
0128 factor_mu = maxd/sqrt(N/2)/4;
0129 factor_nv = factor_v/20;
0130 
0131 %% estimate parameters ----------------------------------------------------
0132 
0133 % xa = [1.8,0.6,2.5,-3]';    % approximate values, true [2,0.5,3,-2]';
0134 xa = zeros(4,1);
0135 sa = max([sigma_x,sigma_y]) * [ [1,1]/maxd, [1,1]/sqrt(I) ];
0136 
0137 [est_x,Cov_xx,sigma_0q,R,vm,Xv,Rim,nabla_lv,muv,muv1,uv1q,uv2] = ...
0138             GaussHelmertModelGroups(lm, Cov_ll_m, @cg_similarity_2D, xa, ...
0139             @ux_similarity_2D, sa, Tx, maxiter, rU);
0140           
0141 %% plot results -----------------------------------------------------------
0142 
0143 % for normalizing the test statistic of the observations
0144 % assuming it ist the same for all observations
0145 rk_Cov_ll = rank(reshape(Cov_ll_m(1,:),4,4));
0146 
0147 ScrS = plot_init;  
0148 figure('Color','w','Position',[100 100  ScrS+[ -600 -300]])
0149 subplot(2,3,1)
0150 hold on
0151 plot(tm(:,1),tm(:,2),'.r','Markersize',12)       % given data
0152 for i = 1:I
0153     plot([tm(i,1),tm(i,1)+factor_v*(vm(i,3))],[tm(i,2),tm(i,2)+factor_v*(vm(i,4))],'-k');
0154 end
0155 title({'residuals [m]', strcat('max=',num2str(max(norm(vm(:,1:4)))))})
0156 xlim([min(tm(:,1))-1,max(tm(:,1))+1]);
0157 ylim([min(tm(:,2))-1,max(tm(:,2))+1]);
0158 axis equal
0159 
0160 
0161 subplot(2,3,2)
0162 hold on
0163 for i = 1:I
0164     plot_circle(tm(i,1),tm(i,2),factor_r*Rim(i,1),'-r');
0165 end
0166 title({'redundancy number', strcat('min=',num2str(min(Rim(:,1))))})
0167 xlim([min(tm(:,1))-factor_r*1,max(tm(:,1))+factor_r*1]);
0168 ylim([min(tm(:,2))-factor_r*1,max(tm(:,2))+factor_r*1]);
0169 axis equal
0170 
0171 subplot(2,3,4)
0172 hold on
0173 for i=1:I
0174     plot_circle(tm(i,1),tm(i,2),factor_X*sqrt(Xv(i)/rk_Cov_ll),'-k','LineWidth',2);
0175     plot_circle(tm(i,1),tm(i,2),factor_X*sqrt(tol/rk_Cov_ll),'--r');
0176 end
0177 title({'test statistics', strcat('max=',num2str(max(sqrt(Xv/rk_Cov_ll)))),'- - - critical values'});
0178 xlim([min(tm(:,1))-factor_X*5,max(tm(:,1))+factor_X*5]);
0179 ylim([min(tm(:,2))-factor_X*5,max(tm(:,2))+factor_X*5]);
0180 axis equal
0181 
0182 subplot(2,3,3)
0183 hold on
0184 for i = 1:I
0185     plot_circle(tm(i,1),tm(i,2),factor_mu*muv(i),'-k');
0186 end
0187 title({'sensitivity factors', strcat('max=',num2str(max(muv)))})
0188 xlim([min(tm(:,1))-factor_mu*5,max(tm(:,1))+factor_mu*5]);
0189 ylim([min(tm(:,2))-factor_mu*5,max(tm(:,2))+factor_mu*5]);
0190 axis equal
0191 
0192 subplot(2,3,5)
0193 hold on
0194 for i = 1:I
0195     plot_circle(tm(i,1),tm(i,2),factor_nv*nabla_lv(i),'-r');
0196 end
0197 title({'min. detectable outliers [m]';strcat('max=',num2str(max(nabla_lv)))})
0198 xlim([min(tm(:,1))-1,max(tm(:,1))+1]);
0199 ylim([min(tm(:,2))-1,max(tm(:,2))+1]);
0200 axis equal
0201 
0202 if ~isempty(rU)
0203     subplot(2,3,6)
0204     hold on
0205     for i = 1:I
0206         plot_circle(tm(i,1),tm(i,2),factor_mu*muv1(i),'-k');
0207     end
0208     title({'sens. factors partial';strcat('max=',num2str(max(muv1)),', param=',num2str(rU))})
0209     xlim([min(tm(:,1))-factor_mu*5,max(tm(:,1))+factor_mu*5]);
0210     ylim([min(tm(:,2))-factor_mu*5,max(tm(:,2))+factor_mu*5]);
0211     axis equal
0212 end
0213 
0214 %% diagnostics ------------------------------------------------------------
0215 disp('diagnostics                  ')
0216 disp('.............................')
0217 Estimated_transformation = ...
0218     [est_x(1) -est_x(2) est_x(3);est_x(2) est_x(1) est_x(4)]                %#ok<NOPTS>
0219 Theoretical_precision   = sqrt(diag(full(Cov_xx)))'                         %#ok<NOPTS>
0220 Redundancy              = R                                                 %#ok<NOPTS>
0221 Estimated_sigma_0       = sqrt(sigma_0q)                                    %#ok<NOPTS>
0222 a________I_______vxl_______vyl_______vxr_______vyr_________X = ...
0223 [(1:I)',vm,sqrt(Xv/rk_Cov_ll)]                                              %#ok<NOPTS>
0224 
0225 [m,i] = max(sqrt(diag(true_error*true_error')));
0226     disp(horzcat('Maximal true error                 = ',num2str(m,'% 12.5f'),...
0227                    ' at: ', num2str(i,'% 3.0f')));
0228 [m,i] = max(sqrt(diag(vm*vm')));
0229     disp(horzcat('Maximal residual                   = ',num2str(m,'% 12.5f'),...
0230                    ' at: ', num2str(i,'% 3.0f')));
0231 [m,i] = min(Rim(:,1));
0232     disp(['Minimal redundancy number          = ',num2str(m,'% 12.5f'),...
0233                    ' at: ', num2str(i,'% 3.0f')]);
0234 
0235 [m,i] = max(sqrt(Xv/rk_Cov_ll));
0236 if max(Xv) > tol 
0237     disp(['Maximal test statistic             = ',num2str(m,'% 12.5f'),...
0238                    ' at: ', num2str(i,'% 3.0f'), ' ***']);
0239 else
0240     disp(['Maximal test statistic             = ',num2str(m,'% 12.5f'),...
0241                    ' at: ', num2str(i,'% 3.0f')]);
0242 end
0243 [m,i] = max(nabla_lv);
0244     disp(['Max. of minimal detectable outlier = ',num2str(m,'% 12.5f'),...
0245                    ' at: ', num2str(i,'% 3.0f')]);
0246                
0247 [m,i] = max(muv);  
0248     disp(['Max. sensitivity factor            = ',num2str(m,'% 12.5f'),...
0249                    ' at: ', num2str(i,'% 3.0f')]);
0250 if ~isempty(rU) 
0251     [m,i] = max(muv1); 
0252     disp(['Max. sensitivity factor [',num2str(rU),']     = ',num2str(m,'% 12.5f'),...
0253                    ' at: ', num2str(i,'% 3.0f')]);
0254 end
0255 
0256 
0257