% Fits a plane in centroid form to a set of 3D points [Xo, Q, var_q, var_phi, var_psi] = sugr_direct_fit_plane_centroid_form_to_points(X, CovX) * X = 3 x n matrix, n 3D points that lie on a plane * CovX = 1 x n or 3 x n or 9 x n matrix, covariance matrices of the points. Each column of CovX is either the variance of a point, or the diagonal/all elements of the covariance matrix of a point. * Xo = 3 x 1 vector, centroid of the fitted plane * Q = 3 x 3 matrix, rotation matrix of the fitted plane. N = Q(:,:,3); * var_q = 1 x 1 scalar, theoretical variance of fitted plane along the normal * var_phi = 1 x 1 scalar, theoretical variance of X component of the normal * var_psi = 1 x 1 scalar, theoretical variance of Y component of the normal * est_sigma_0_2 = 1 x 1 scalar, estimated variance factor Wolfgang Förstner wfoerstn@uni-bonn.de change log 29/08/2016 wf: added est_sigma_0_2 as output 26/08/2016 first version 28/08/2016 CovX can now be a 1 x n, 3 x n or 9 x n matrix
0001 %% Fits a plane in centroid form to a set of 3D points 0002 % 0003 % [Xo, Q, var_q, var_phi, var_psi] = sugr_direct_fit_plane_centroid_form_to_points(X, CovX) 0004 % 0005 % * X = 3 x n matrix, n 3D points that lie on a plane 0006 % * CovX = 1 x n or 3 x n or 9 x n matrix, covariance matrices of the points. 0007 % Each column of CovX is either the variance of a point, or the diagonal/all elements of the covariance matrix of a point. 0008 % 0009 % * Xo = 3 x 1 vector, centroid of the fitted plane 0010 % * Q = 3 x 3 matrix, rotation matrix of the fitted plane. N = Q(:,:,3); 0011 % * var_q = 1 x 1 scalar, theoretical variance of fitted plane along the normal 0012 % * var_phi = 1 x 1 scalar, theoretical variance of X component of the normal 0013 % * var_psi = 1 x 1 scalar, theoretical variance of Y component of the normal 0014 % * est_sigma_0_2 = 1 x 1 scalar, estimated variance factor 0015 % 0016 % Wolfgang Förstner 0017 % wfoerstn@uni-bonn.de 0018 % 0019 % change log 0020 % 29/08/2016 wf: added est_sigma_0_2 as output 0021 % 26/08/2016 first version 0022 % 28/08/2016 CovX can now be a 1 x n, 3 x n or 9 x n matrix 0023 0024 function [Xo, Q, var_q, var_phi, var_psi, est_sigma_0_2] = sugr_direct_fit_plane_centroid_form_to_points(X, CovX) 0025 0026 %% check input 0027 0028 % check that we dont have too few arguments 0029 if nargin < 1, error('Not enough input arguments. '), end 0030 0031 % or too many ... 0032 if nargin > 2, error('Too many input arguments'), end 0033 0034 % otherwise we have at least points; check if they are 3D 0035 [dim, numPoints] = size(X); 0036 if dim ~= 3, error('Input should be a 3 x n matrix containing 3D points. '), end 0037 0038 % check if we have enough points 0039 if numPoints < 3, error('At least three points are needed for plane fitting. '), end 0040 0041 % if variances are not given use default value of 1 for all points 0042 if nargin == 1 0043 warning('A default variance of 1 will be used for all points. ') 0044 CovX = repmat(ones(3,1), 1, numPoints); 0045 end 0046 0047 % check the form of (co)variances if given 0048 if nargin == 2 0049 0050 [dim2, np2] = size(CovX); 0051 0052 % check that variances are given for all points 0053 if np2 ~= numPoints, error('Number of variances does not match the number of points. '), end 0054 0055 % if for each point only one variance value is given then all is fine 0056 if dim2 == 1 0057 CovX = repmat(CovX, 3, 1); 0058 0059 % if for each point three variances are given then issue a warning but we can proceed 0060 elseif dim2 == 3 0061 warning('For anisotropic covariance matrices the result may not be optimal. '), 0062 0063 % if for each point all 9 elements of a covariance matrix are given issue a warning and discard the off-diagonal elements 0064 elseif dim2 == 9 0065 warning('Off-diagonal elements of covariance matrices will be ignored. '), 0066 %CovX = CovX([1 5 9], :); 0067 CovX = repmat((CovX(1, :)+CovX(5, :)+CovX(9, :))/3,3,1); 0068 0069 % hopefully one should never end up here! 0070 else 0071 error('Input variances/covariances should be in the form of a 1 x n, 3 x n or 9 x n matrix. '), 0072 end 0073 end 0074 0075 %% the weights 0076 w = 1 ./ CovX; 0077 0078 %% the weighted centroid 0079 Xo = sum(w .* X, 2) ./ sum(w, 2); 0080 0081 %% the moment matrix 0082 M = w .* (X - repmat(Xo, 1, numPoints)) * (X - repmat(Xo, 1, numPoints))' ; 0083 0084 %% the rotation matrix Q 0085 [Q, G] = eig(M); 0086 0087 % sort eigenvalues in descending order 0088 [srtval, srtid] = sort(diag(G), 'descend'); 0089 G = diag(srtval); 0090 0091 % sort eigenvectors correspondingly 0092 Q = Q(:, srtid); 0093 0094 % if Q is a reflection convert it to a rotation 0095 Q = Q*diag([1,1,det(Q)]); 0096 0097 %% the variances of plane parameters 0098 w_bar = mean(w(:)); 0099 0100 % redundancy 0101 Red = numPoints - 3; 0102 0103 % estimated Omega 0104 Omega = G(3,3); 0105 0106 if Red > 0 0107 est_sigma_0_2 = Omega/Red; 0108 else 0109 est_sigma_0_2 = 1; 0110 end 0111 0112 % theoretical variance along normal 0113 var_q = 1 / numPoints / w_bar ; 0114 0115 % theoretcial variance of x component of the normal 0116 var_phi = 1 / G(1, 1) ; 0117 0118 % theoretical variance of y component of the normal 0119 var_psi = 1 / G(2, 2) ; 0120 0121