function [coeff, post] = fitplane(crd)

%
% [coeff, crdp] = fitplane(crd)
%
% Fits a plane of the form Ax + By + Cz = 1 to a given 3D point cloud
% If desired post processes data
%   projects coordinates (crd) on the plane
%   calculates absolute residuals (deviation from plane) and RMS
%
% Input:
%   crd:    n x 3 matrix, data coordinates
%           row denotes points, column denotes coordinates (X, Y, Z)
%
% Output:
%   coeff:  3 x 1 array
%           coefficients [A, B, C] to describe the plane
%   post:   structure
%       post.crdp:   n x 3 matrix, projected coordinates
%                    row denotes points, column denotes coordinates (X, Y, Z)
%       post.rsd:    residuals
%       post.rms:    root-mean-square error
%
% Dependencies:
%
%   None
%
% Author: 
%   Ahmet Erdemir, erdemira@ccf.org, August 1, 2008.
%

n = size(crd,1);
coeff = crd\ones(n, 1);

% ALTERNATIVELY
% x = crd(:,1);
% y = crd(:,2);
% z = crd(:,3);
% sx = sum(x);
% sy = sum(y);
% sz = sum(z);
% sxx = x'*x;
% syy = y'*y;
% szz = z'*z;
% sxy = x'*y;
% syz = y'*z;
% szx = z'*x;
% 
% A = [sxx sxy szx; sxy syy syz; szx syz szz];
% b = [sx sy sz]';
% coeff = A\b;

if nargout > 1
    s = (1. -coeff'*crd')/norm(coeff);
    post.crdp = crd + ((ones(3,1)*s).*(coeff*ones(1,n)))';
    dmy = crd - post.crdp;
    dmy = sum(dmy.*dmy, 2);
    post.rsd = sqrt(dmy);
    post.rms = sqrt(sum(dmy)/n);
end


