% FITLINE3D - Fits a line to a set of 3D points
%
% Usage: [L] = fitline3d(XYZ)
%
% Where: XYZ - 3xNpts array of XYZ coordinates
% [x1 x2 x3 ... xN;
% y1 y2 y3 ... yN;
% z1 z2 z3 ... zN]
%
% Returns: L - 3x2 matrix consisting of the two endpoints of the line
% that fits the points. The line is centered about the
% mean of the points, and extends in the directions of the
% principal eigenvectors, with scale determined by the
% eigenvalues.
%
% Author: Felix Duvallet (CMU)
% August 2006
function L = fitline3d(XYZ)
% Since the covariance matrix should be 3x3 (not NxN), need
% to take the transpose of the points.
XYZ = XYZ';
% find mean of the points
mu = mean(XYZ, 1);
% covariance matrix
C = cov(XYZ);
% get the eigenvalues and eigenvectors
[V, D] = eig(C);
% largest eigenvector is in the last column
col = size(V, 2); %get the number of columns
% get the last eigenvector column and the last eigenvalue
eVec = V(:, col);
eVal = D(col, col);
% start point - center about mean and scale eVector by eValue
L(:, 1) = mu' - sqrt(eVal)*eVec;
% end point
L(:, 2) = mu' + sqrt(eVal)*eVec;