sugr_generate_true_2D_point_pairs_E_matrix

% Generate true 2D point pairs from Relative Orientation,
 i.e., for given b, R generate N point pairs

 PP = sugr_generate_true_2D_point_pairs_E_matrix(b,R,N,boolean_r,Z0);

 b        = 3 x 1 vector 
 R        = 3x3 matrix R
 N         = number of points (in square/cube [-1,1]^2  / 3
 boolean_r = boolean: points should sit random
                else: points sit in a square +- random (N should be square)
 Z0        = Z-shift of points from origin

 PP = point pairs
      PP.h = N x 6 matrix of pairs of homogeneous point coordinates
      PP.Crr = N x 4 x 4 sith 4 x 4 reduced CovM of point pairs
      PP.type = 8 * ones(N,1)
 X  = true 3D coordinates

 Wolfgang Förstner 09/2011
 wfoerstn@uni-bonn.de

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

 See also sugr_E_Matrix

0001 %% Generate true 2D point pairs from Relative Orientation,
0002 % i.e., for given b, R generate N point pairs
0003 %
0004 % PP = sugr_generate_true_2D_point_pairs_E_matrix(b,R,N,boolean_r,Z0);
0005 %
0006 % b        = 3 x 1 vector
0007 % R        = 3x3 matrix R
0008 % N         = number of points (in square/cube [-1,1]^2  / 3
0009 % boolean_r = boolean: points should sit random
0010 %                else: points sit in a square +- random (N should be square)
0011 % Z0        = Z-shift of points from origin
0012 %
0013 % PP = point pairs
0014 %      PP.h = N x 6 matrix of pairs of homogeneous point coordinates
0015 %      PP.Crr = N x 4 x 4 sith 4 x 4 reduced CovM of point pairs
0016 %      PP.type = 8 * ones(N,1)
0017 % X  = true 3D coordinates
0018 %
0019 % Wolfgang Förstner 09/2011
0020 % wfoerstn@uni-bonn.de
0021 %
0022 % last changes: Susanne Wenzel 06/18
0023 % wenzel@igg.uni-bonn.de
0024 %
0025 % See also sugr_E_Matrix
0026 
0027 function [PP,X] = sugr_generate_true_2D_point_pairs_E_matrix(b,R,N,br,Z0)
0028 
0029 K   = eye(3);
0030 P1  = K *     [eye(3),zeros(3,1)];
0031 P2  = K * R * [eye(3),-b];
0032 % random points in unit square shifted by Z0
0033 X = zeros(N,4);
0034 if br
0035     for n = 1:N
0036         %random points
0037         X(n,:)   = [rand(3,1)*2-1+[0,0,Z0]';1];
0038         % true image coordinates
0039         xs  = P1 * X(n,:)';
0040         xss = P2 * X(n,:)';
0041         xs  =xs/norm(xs);
0042         xss =xss/norm(xss);
0043         % struct of true point pairs
0044         PP.h(n,:)     = [xs',xss'];
0045         PP.Crr(n,:,:) = zeros(4);
0046         PP.type(n)    = 8;
0047     end
0048     cov(X);
0049 else
0050     M = ceil(sqrt(N));
0051     n = 0;
0052     for m = 1:M
0053         for k=1:M
0054            n = n+1;
0055            if n <= N
0056                X(n,:) = [-(M-1)/2+2*(m-1)/(M-1);...
0057                     -(M-1)/2+2*(k-1)/(M-1);...
0058                     Z0+rand(1)-0.5;...
0059                     1];
0060                % true image coordinates
0061                 xs  = P1 * X(n,:)';
0062                 xss = P2 * X(n,:)';
0063                 xs  = xs/norm(xs);
0064                 xss = xss/norm(xss);
0065                 % struct of true point pairs
0066                 PP.h(n,:)     = [xs',xss'];
0067                 PP.Crr(n,:,:) = zeros(4);
0068                 PP.type(n)    = 8;
0069            end
0070         end
0071     end
0072 end
0073 
0074 figure('Color','w')
0075 hold on
0076 scatter3(X(:,1),X(:,2),X(:,3),'.r')
0077 scatter3(0,0,0,'ob')
0078 scatter3(b(1),b(2),b(3),'xb')
0079 legend('object points','camera 1', 'camera 2')
0080 xlabel('X')
0081 ylabel('Y')
0082 zlabel('Z')