demo_triangulation_spherical

% demo triangulation

 simulate:
 X        = 4x1 vector
 [b,R]    = relative orientation
 sigma    = radial error odf direction
 max_iter = maximum iterations
 Tol      = tolerance for stopping iterations
 k        = critical value

 derive 
 u, v     = 3x1 vectors, spherically normalized

 evaluate estimated 3D point with its covaraince matrix

 Wolfgang Förstner
 wfoerstn@uni-bonn.de

 last changes: Susanne Wenzel 06/18
 wenzel@igg.uni-bonn.de

0001 %% demo triangulation
0002 %
0003 % simulate:
0004 % X        = 4x1 vector
0005 % [b,R]    = relative orientation
0006 % sigma    = radial error odf direction
0007 % max_iter = maximum iterations
0008 % Tol      = tolerance for stopping iterations
0009 % k        = critical value
0010 %
0011 % derive
0012 % u, v     = 3x1 vectors, spherically normalized
0013 %
0014 % evaluate estimated 3D point with its covaraince matrix
0015 %
0016 % Wolfgang Förstner
0017 % wfoerstn@uni-bonn.de
0018 %
0019 % last changes: Susanne Wenzel 06/18
0020 % wenzel@igg.uni-bonn.de
0021 
0022 
0023 % clear all
0024 clearvars
0025 close all
0026 
0027 addpath(genpath('../../General-Functions'))
0028 addpath('../Functions')
0029 
0030 sugr_INIT
0031 
0032 ss = plot_init();
0033 
0034 disp(' --------------------------------------');
0035 disp(' ------ Triangulation (Alg. 21) -------');
0036 disp(' --------------------------------------');
0037 
0038 N_samples = 1000;
0039 
0040 sigma = 0.0001;
0041 
0042 % random seed
0043 % init_rand      = 1;
0044 init_rand      = 215741;
0045 
0046 disp(['Number of samples                      : ', num2str(N_samples)])
0047 disp(['Standard deviation of directions [rad] : ', num2str(sigma)])
0048 
0049 %% random number intialization
0050 init_randn = init_rand_seed(init_rand);
0051 disp(['Random seed                            : ' , num2str(init_randn)])
0052 
0053 %%
0054 fcond = 1;
0055 X_true = [[0.5,1.0,1.5]*fcond,1]';
0056 Jr_true = null(X_true');
0057 % b = [1,0,0]';
0058 % b = [0,0,1]';
0059 b = randn(3,1);
0060 b = b/norm(b);
0061 
0062 xi = X_true(4)/norm(X_true(1:3));
0063 infinite = xi < 0.0001; % only for plotting
0064 
0065 Rot = calc_Rot_r(randn(3,1));
0066 sigma = 0.0001;
0067 max_iter = 10;
0068 Tol = 0.0001;
0069 k = 10;
0070 print_option = N_samples == 1; 
0071 
0072 X_est_all = zeros(N_samples,3);
0073 C_est_all = zeros(N_samples,3,3);
0074 s_all     = zeros(N_samples,1);
0075 
0076 %% for all samples
0077 t = cputime;
0078 for n = 1:N_samples
0079     
0080     % derive u, v
0081     [u,v] = generate_point_pair(X_true,b,Rot,sigma);
0082     
0083     % E        = sugr_SkewMatrix(b)*Rot';
0084     % sigma_c  = sigma*sqrt(u'*E*E'*u+v'*E'*E*v);
0085     % cgl      = u'*E*v;
0086     % s_all(n) = abs(cgl)/sigma_c;
0087     % estimate X
0088     
0089     [X_est, est_sigma_0, est_u, est_v, f] = ...
0090         sugr_triangulate_two_spherical_cameras(...
0091         b, Rot, u, v, sigma, Tol, max_iter, k, print_option);
0092     
0093     X_est_all(n,:) = (Jr_true'*X_est.h)';
0094     
0095     s_all(n)       = est_sigma_0; 
0096 end
0097 
0098 %%
0099 disp(['CPU time for ', num2str(N_samples),' samples              : ',num2str(cputime-t)]);
0100 m_X      = mean(X_est_all);    
0101 cov_X    = cov(X_est_all);
0102 disp(['mean reduced coordinates :', num2str(m_X)]);
0103 
0104 %% Plot
0105 % plotting of not too far
0106 if ~infinite
0107     figure('name','est.cov','Color','w', 'Position',[100 0.52*ss(2) ss(1)/3 0.4*ss(2)])
0108     hold on
0109     plot3(X_true(1),X_true(2),X_true(3),'or');
0110     plot3([0,b(1)],[0,b(2)],[0,b(3)],'-b','LineWidth',3);
0111     plot3([0,X_true(1)],[0,X_true(2)],[0,X_true(3)],'--r','LineWidth',2);
0112     plot3([b(1),X_true(1)],[b(2),X_true(2)],[b(3),X_true(3)],'--r','LineWidth',2);
0113     box on
0114     title('estimated covariance matrix')
0115     axis equal
0116 
0117     Xemp.h = X_est.h;
0118     Xemp.Crr = cov_X;
0119     [~,Cee] = sugr_get_Euclidean_Point_3D(Xemp);
0120     [R,D] = eig(Cee);
0121 
0122     f = 0.4/sqrt(D(1,1));
0123     [X,Y,Z] = ellipsoid(0,0,0,sqrt(D(1,1))*f,sqrt(D(2,2))'*f,sqrt(D(3,3))*f);
0124     K = [X(:),Y(:),Z(:)]*R'+ones(441,1)*X_true(1:3)';
0125 
0126     surfl(reshape(K(:,1),21,21), reshape(K(:,2),21,21), reshape(K(:,3),21,21))
0127     colormap cool %copper
0128     axis equal
0129 
0130     figure('name','true.cov','Color','w', 'Position',[300+ss(1)/3 0.52*ss(2) ss(1)/3 0.4*ss(2)])
0131     hold on
0132     plot3(X_true(1),X_true(2),X_true(3),'or');
0133     plot3([0,b(1)],[0,b(2)],[0,b(3)],'-b','LineWidth',3);
0134     plot3([0,X_true(1)],[0,X_true(2)],[0,X_true(3)],'--r','LineWidth',2);
0135     plot3([b(1),X_true(1)],[b(2),X_true(2)],[b(3),X_true(3)],'--r','LineWidth',2);
0136     box on
0137     title('true covariance matrix')
0138     axis equal
0139         
0140     [~,Cee] = sugr_get_Euclidean_Point_3D(X_est);
0141     [R,D] = eig(Cee);
0142 
0143     f = 0.4/sqrt(D(1,1));
0144 
0145     [X,Y,Z] = ellipsoid(0,0,0,sqrt(D(1,1))*f,sqrt(D(2,2))'*f,sqrt(D(3,3))*f);
0146     K = [X(:),Y(:),Z(:)]*R'+ones(441,1)*X_true(1:3)';
0147 
0148     surfl(reshape(K(:,1),21,21), reshape(K(:,2),21,21), reshape(K(:,3),21,21))
0149     colormap copper
0150     axis equal
0151 
0152 end
0153 
0154 %% Analyse
0155 check_estimation_result(1,zeros(3,1),X_est.Crr,s_all.^2,X_est_all,0.999,'Triangulation');
0156 set(gcf,'Position',[100 20 ss(1)/2 0.4*ss(2)])
0157