sugr_perturb_3D_2D_point_pairs

% Perturb sampled 3D 2D points pairs

 [X,y] = sugr_perturb_3D_2D_point_pairs(Xt,yt,sigma_X,sigma_y,fp);
 
 Xt,yt     = true point pairs, sugr-objects, 3D and 2D points, resp.
 sigma_x   = std for perturbation of X's: CovM = sigma_x^2 I_3
 sigma_y   = std for perturbation of y's: CovM = sigma_y^2 I_2
 fp        = magnification factor for plot

 [X,y]      perturbed point pairs, sugr-objects, 3D and 2D points, resp.

 Wolfgang Förstner 02/2013
 wfoerstn@uni-bonn.de

 See also sugr_perturb_2D_point_pairs, sugr_perturb_2D_point_pairs_spherical,
 sugr_perturb_Lines_2D

0001 %% Perturb sampled 3D 2D points pairs
0002 %
0003 % [X,y] = sugr_perturb_3D_2D_point_pairs(Xt,yt,sigma_X,sigma_y,fp);
0004 %
0005 % Xt,yt     = true point pairs, sugr-objects, 3D and 2D points, resp.
0006 % sigma_x   = std for perturbation of X's: CovM = sigma_x^2 I_3
0007 % sigma_y   = std for perturbation of y's: CovM = sigma_y^2 I_2
0008 % fp        = magnification factor for plot
0009 %
0010 % [X,y]      perturbed point pairs, sugr-objects, 3D and 2D points, resp.
0011 %
0012 % Wolfgang Förstner 02/2013
0013 % wfoerstn@uni-bonn.de
0014 %
0015 % See also sugr_perturb_2D_point_pairs, sugr_perturb_2D_point_pairs_spherical,
0016 % sugr_perturb_Lines_2D
0017 
0018 function [X,y] = sugr_perturb_3D_2D_point_pairs(Xt,yt,sigma_X,sigma_y,fp)
0019 
0020 global plot_option 
0021 
0022 Xte   = Xt.e;
0023 CXXe  = sigma_X^2*eye(3);
0024 yte   = yt.e;
0025 Cyye  = sigma_y^2*eye(2);
0026 
0027 N = size(Xte,1);
0028 
0029 X.e   = zeros(N,3);
0030 X.Cee = zeros(N,3,3);
0031 y.e   = zeros(N,2);
0032 y.Cee = zeros(N,2,2);
0033 for n=1:N
0034     % true Euclidean coordinates
0035     X_true_e = Xte(n,1:3)';
0036     y_true_e = yte(n,1:2)';
0037     % perturbed point pair
0038     X.e(n,:)       = sugr_rand_gauss(X_true_e,CXXe, 1)';
0039     y.e(n,:)       = sugr_rand_gauss(y_true_e,Cyye, 1)';
0040     X.Cee(n,:,:) = CXXe;
0041     y.Cee(n,:,:) = Cyye;
0042 end
0043 X.e;
0044 y.e;
0045 
0046 % plot
0047 ss = plot_init;
0048 if plot_option > 0
0049     figure('Color','w','Position',[ss(1)/4,ss(2)/3,ss(1)/2,ss(2)/2])
0050    
0051     subplot(1,2,1)
0052     hold on
0053     for n=1:N
0054         plot3(X.e(n,1),X.e(n,2),X.e(n,3),'.b');
0055     end
0056     title('3D points')
0057     axis equal
0058     
0059     subplot(1,2,2)
0060     hold on
0061     % 2D
0062     for n=1:N
0063         ye = y.e(n,:)';
0064         Cyy = squeeze(y.Cee(n,:,:));
0065         yn = sugr_Point_2D(ye,Cyy);
0066         sugr_plot_Point_2D(yn,'.k','-b',2,fp);
0067     end
0068     title('image points')
0069     axis equal
0070 end