demo_direct_plane_fit

% demo plane fitting direct method

 generate points in [-1,+1]x[-0.5,+0.5] in arbitry plane
 add noise

 estimate plane parameters of centroid representation algebraically

 check estimation

 Wolfgang Förstner
 wfoerstn@uni-bonn.de

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

0001 %% demo plane fitting direct method
0002 %
0003 % generate points in [-1,+1]x[-0.5,+0.5] in arbitry plane
0004 % add noise
0005 %
0006 % estimate plane parameters of centroid representation algebraically
0007 %
0008 % check estimation
0009 %
0010 % Wolfgang Förstner
0011 % wfoerstn@uni-bonn.de
0012 %
0013 % last changes: Susanne Wenzel 06/18
0014 % wenzel@igg.uni-bonn.de
0015 
0016 % clear all
0017 close all
0018 clearvars
0019 
0020 addpath(genpath('../../General-Functions'))
0021 
0022 disp('--------------------------------------------')
0023 disp('Check direct estimation of plane from points')
0024 disp('--------------------------------------------')
0025 
0026 % number of samples N_samples
0027 N_samples = 10000;
0028 disp(['Number of samples                      : ', num2str(N_samples)])
0029 
0030 % standard deviation of points
0031 sigma_x = 1e-3;
0032 disp(['MInimal standard deviation of points   : ', num2str(sigma_x)])
0033 
0034 % density
0035 pointSpacing = 0.1;
0036 disp(['Point spacing in [-1,+1]x[-0.5,+0.5]   : ', num2str(pointSpacing)])
0037 numPoints = length(-1:pointSpacing:1)*length(-0.5:pointSpacing:0.5);
0038 disp(['Number of points                       : ', num2str(numPoints)])
0039 
0040 %% random seed
0041 %init_rand      = 1;
0042 init_rand      = 0;
0043 init_rand_seed(init_rand);
0044 
0045 %% additional control variables
0046 
0047 % Parameterize an initial plane (z=1 plane) == true plane
0048 iniPlane = [0, 0, 1, -1]'; % z=1 plane
0049 
0050 % uncertainty
0051 % random variances between 0.0001 and 0.001
0052 
0053 SIGMA = sigma_x * randi(10, [1, numPoints]);
0054 %SIGMA = sigma_x * randInt(10, [1, numPoints]);
0055 w     = 1./SIGMA.^2;
0056 
0057 % alpha for checks
0058 alpha = 0.001;
0059 
0060 %% generate regularly spaced points on the initial plane == true points
0061 [m1, m2] = meshgrid(-1:pointSpacing:1, -0.5:pointSpacing:0.5);
0062 numPoints = numel(m1(:));
0063 iniPoints = [m1(:), m2(:), ones(numPoints,1)]'; % points of z=1 plane
0064 
0065 %% make up a random transformation == true transformation
0066 randRot = pi/4 * (rand(1,3) - 0.5); % random rotation within [-90 90] degrees
0067 randR = calc_Rot_r([randRot(1), randRot(2), randRot(3)]);
0068 randT = 20 * (rand(3,1) - 0.5); % random translation within range [-10 10]
0069 
0070 %% transform initial Plane == true plane
0071 plane_nd = [randR, randT; 0 0 0 1]' * iniPlane; 
0072 plane_nd = plane_nd * sign(-plane_nd(4)); % d is always positive
0073 
0074 %% Sample data and store results
0075 Omega_s     = zeros(1,N_samples);
0076 d_prime_s   = zeros(1,N_samples);
0077 normal_XY_s = zeros(2,N_samples);
0078 var_q_s     = zeros(1,N_samples);
0079 var_phi_s   = zeros(1,N_samples);
0080 var_psi_s   = zeros(1,N_samples);
0081 d_primes_s  = zeros(1,N_samples);
0082 
0083 start = cputime;
0084 for n_samples = 1:N_samples
0085     %transform points on the initial plane and add noise
0086     points = randR'*(iniPoints - repmat(randT, 1, numPoints)) + ...
0087         repmat(SIGMA, 3, 1) .* randn(3, numPoints);
0088     
0089     % fit a plane using direct method
0090     [Xo, Q, var_q, var_phi, var_psi, est_sigma_0_2] = ...
0091         sugr_direct_fit_plane_centroid_form_to_points(points, SIGMA.^2);
0092     
0093     Omega_s(n_samples)       = est_sigma_0_2 * (numPoints-3);
0094     d_primes_s(n_samples)    = -plane_nd(4) - plane_nd(1:3)' * Xo;
0095     normal_XY_s(:,n_samples) = Q(:,1:2)' * plane_nd(1:3);
0096     var_q_s(n_samples)       = var_q;
0097     var_phi_s(n_samples)     = var_phi;
0098     var_psi_s(n_samples)     = var_psi;
0099     
0100 end
0101 disp(['CPU time for ', num2str(N_samples),' samples             : ',num2str(cputime-start)]);
0102 %% check
0103 Red  = numPoints-3;
0104 
0105 %% Check estimation
0106 
0107 % collect parameters
0108 est_par      = [d_primes_s;normal_XY_s];
0109 % collect variances
0110 CovM_true    = diag([var_q,var_phi,var_psi]);
0111 %
0112 check_estimation_result(Red,zeros(3,1),CovM_true,Omega_s/Red,est_par',1-alpha,' algebraic plane fit');
0113 
0114 
0115 
0116 
0117 
0118