Description of demo_estimate_E_Matrix_from_point

0001 %% test_estimate_E_Matrix_from_point_pairs
0002 %
0003 % direct solution following Sect. 13.3.2.3, p. 575 ff
0004 %   with covariance matrix following Sect. 4.9.2.4
0005 % iterative solution following Sect. 13.3.5, p. 585 ff
0006 %
0007 % Wolfgang Förstner
0008 % wfoerstn@uni-bonn.de
0009 %
0010 % last changes: Susanne Wenzel 06/18
0011 % wenzel@igg.uni-bonn.de
0012 
0013 %clear all
0014 clearvars
0015 close all
0016 
0017 disp(' ')
0018 disp(' ')
0019 disp('=================================================')
0020 disp('---- Demo_estimate_E_Matrix_from_point_pairs ----')
0021 disp('-------------------------------------------------')
0022 
0023 global min_redundancy
0024 global plot_option
0025 global print_option
0026 global print_option_estimation
0027 
0028 addpath(genpath('../../General-Functions'))
0029 
0030 sugr_INIT
0031 ss = plot_init;
0032 
0033 %% control parameters
0034 
0035 %% random number intialization
0036 init_rand      = 2;            
0037 % init_rand      = 0;
0038 init_rand_seed(init_rand);
0039 
0040 % number of corresponding points
0041 % N             = 100;
0042 N             = 30;              
0043 % N             = 15;
0044 
0045 % number of samples for checking covariance matrix
0046 %N_samples     = 1000;
0047 N_samples     = 50;         
0048 %N_samples     = 1;
0049 
0050 %% standard deviations
0051 % sigma_x       = 0.003;                  % std. dev.
0052 % sigma_y       = 0.003;                  % std. dev.
0053 %
0054 % for Ladybug 3:  2*pi/(5*1400) rad/pixel, Lowe 0.3 pixel -> std radiant = 2.0944e-004
0055 % sigma_x       = 0.00021;                  % std. dev.
0056 % sigma_y       = 0.00021;                  % std. dev.
0057 %
0058 % for testing program
0059 sigma_x       = 0.000001;                  % std. dev.
0060 sigma_y       = 0.000001;                  % std. dev.
0061 
0062 % correlation between points in left and right image
0063 rho           = 0; %0.99;
0064 
0065 % Threshold for iteration
0066 %(can be reduced to T = 0.1 and maxiter = 2 for practical purposes)
0067 T       = 10^(-6);
0068 maxiter = 5;
0069 S = 0.999;
0070 
0071 % in order to check also for low redundacies
0072 min_redundancy = 2;
0073 
0074 %% Generate points in unit cube
0075 
0076 r_tilde       = [3.1,-2,-1.3];          %zeros(3,1); %
0077 % r_tilde       = [0 0 0];                %zeros(3,1); %
0078 R_tilde       = calc_Rot_r(r_tilde);    % true rotation matrix
0079 b_tilde       = [0.2,1.0,1.35]';
0080 % b_tilde       = [0 1 0]';
0081 b_tilde       = b_tilde/norm(b_tilde);        % true basis
0082 b_l_true      = 1;
0083 
0084 % Representation of relative orientation without uncertainty
0085 b_R = [b_tilde,R_tilde];
0086 true_basis_Rotation_matrix = b_R                                           %#ok<*NOPTS>
0087 % structure for relative orientation
0088 E_tilde       = sugr_E_Matrix(b_tilde,R_tilde);   % true basis, rotation
0089 E_t           = E_tilde.E;
0090 true_E_matrix = E_t
0091 
0092 factor_p      = 12;                   % factor for plotting standard ellipses
0093 b_random      = 1;                    % box for random points in [-1,+1]^3
0094 Z0            = -1;                  % shift of bounding box (best to be negative)
0095 
0096 print_option_estimation = 1;            % medium output
0097 if N_samples >1 
0098     print_option_estimation = 0;
0099 end
0100 
0101 % do not plot samples
0102 print_option = 2;
0103 if N_samples > 1
0104     plot_option  = 0;
0105     print_option = 0;
0106 end
0107 
0108 %% generate true points
0109 [PPt,Xt] = sugr_generate_true_2D_point_pairs_E_matrix(b_tilde*b_l_true,R_tilde,N,b_random,Z0);
0110 set(gcf,'Name','Points and cameras','Position',[100 0.25*ss(2) 0.3*ss(1) 0.4*ss(2)])
0111 set(gca,'CameraPosition',[-15, -8, 6])
0112 %% initiate
0113 est_E_samples_a  = zeros(N_samples,5);
0114 est_bR_samples_a = zeros(N_samples,12);
0115 est_E_samples_ml  = zeros(N_samples,5);
0116 est_bR_samples_ml = zeros(N_samples,12);
0117 
0118 total_time_a=0;
0119 total_time_ml=0;
0120 
0121 %% estimate
0122 disp('Monitor samples:')
0123 iii = 0;
0124 s_a = zeros(1,N_samples);
0125 s_ml = zeros(1,N_samples);
0126 start = cputime;
0127 for ii = 1:N_samples
0128     
0129     iii = iii+1;
0130     
0131     if N_samples < 100
0132         fprintf(num2str(ii)),fprintf(',')
0133         if mod(ii,10) == 0
0134             disp(' ')
0135         end
0136     else
0137         if mod(ii,10) == 0
0138             fprintf(num2str(ii)),fprintf(',')
0139         end
0140         if mod(ii,100) == 0
0141             disp(' ')
0142         end
0143     end
0144     
0145     %% perturb point pairs
0146     PP = sugr_perturb_2D_point_pairs_spherical(PPt,sigma_x,sigma_y,rho,factor_p);
0147 
0148     %% check
0149     % diag(PP.h(:,1:3)*calc_S(b_tilde)*R_tilde'*PP.h(:,4:6)')';
0150     
0151     %% Estimate E_matrix algebraically
0152     start = cputime;
0153 
0154     [est_E_a,sigma_0_a,error] = sugr_estimation_algebraic_E_Matrix_from_point_pairs(PP);
0155     if error > 0
0156         disp(['sample ',num2str(ii),' failed'])
0157         iii = iii-1;
0158     else
0159         check_algebraic = diag(PP.h(:,1:3)*est_E_a.E*PP.h(:,4:6)')';
0160         
0161         b_a_r                  = null(b_tilde')' * est_E_a.bR(:,1);
0162         [r_a,a_a]              = calc_r_phi_from_R(est_E_a.bR(:,2:4)*R_tilde');
0163         est_E_samples_a(ii,:)  = [b_a_r',r_a'*a_a];
0164         est_bR_samples_a(ii,:) = est_E_a.bR(:)';
0165 
0166         s_a(ii)  = 1;
0167 
0168         total_time_a = total_time_a + cputime-start;
0169         %% Estimate E_matrix maximum likelihood
0170         start = cputime;
0171 
0172         [est_E_ml,sigma_0_ml,R] = sugr_estimation_ml_E_Matrix_from_point_pairs(PP,est_E_a.bR,T,maxiter);
0173         total_time_ml           = total_time_ml + cputime-start;
0174         
0175 
0176         b_ml_r                  = null(b_tilde')' * est_E_ml.bR(:,1);
0177         [r_ml,a_ml]             = calc_r_phi_from_R(est_E_ml.bR(:,2:4)*R_tilde');
0178         est_E_samples_ml(ii,:)  = [b_ml_r',r_ml'*a_ml];
0179         est_bR_samples_ml(ii,:) = est_E_ml.bR(:)';
0180 
0181         s_ml(ii) = sigma_0_ml;
0182     end
0183 end
0184 disp(['CPU time for ', num2str(N_samples),' samples              : ',num2str(cputime-start)]);
0185 disp(' ')
0186 
0187 %%
0188 rho_degree = 180/pi;
0189 disp(strcat('sigma basis direction [mrad] = ',...
0190     num2str(1000*sqrt(diag(est_E_ml.CbRbR(1:2,1:2)))')));
0191 disp(strcat('sigma rotation        [mrad] = ',...
0192     num2str(1000*sqrt(diag(est_E_ml.CbRbR(3:5,3:5)))')));
0193 disp(strcat('sigma basis direction [degree] = ',...
0194     num2str(rho_degree*sqrt(diag(est_E_ml.CbRbR(1:2,1:2)))')));
0195 disp(strcat('sigma rotation        [degree] = ',...
0196     num2str(rho_degree*sqrt(diag(est_E_ml.CbRbR(3:5,3:5)))')));
0197 
0198 %% check algorithm
0199 if N_samples > 5 
0200     
0201     % check of ALG
0202     check_estimation_result(N-5,zeros(5,1),est_E_a.CbRbR,s_a.^2,...
0203         est_E_samples_a,S,' ALG')
0204    
0205     % ML
0206     check_estimation_result(N-5,zeros(5,1),est_E_ml.CbRbR,s_ml.^2,...
0207         est_E_samples_ml,S,' ML');
0208     
0209     set(gcf,'Name','Histogram var.factors','Position',[0.4*ss(1) 0.25*ss(2) ss(1)/2 0.4*ss(2)])
0210 end
0211 
0212 total_CPUtime_alg_mle_seconds  = [total_time_a,total_time_ml]
0213 
0214 CovM_dir = sugr_CovM_E_matrix(Xt, PP.Crr, b_tilde, R_tilde);
0215 CovM_comparison_generalized_eigenvalues_should_be_one = ...
0216     eigs(inv(est_E_ml.CbRbR)*CovM_dir)'                                    %#ok<MINV>
0217 
0218 disp('          ========= End of Demo ===========')
0219 disp(' ')
0220 disp(' ')
demo_estimate_E_Matrix_from_point_pairs

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SOURCE CODE
