1/13/2016

Study of line fitting in 3D and example source code (matlab)

This article is the method about line fitting in 3d points.
If we have these 3d points, how to find best 3d line?



The main concept is to find principal component of 3d points.
There are several method to find principal component in some data.
RANSAC(Random Sample Consensus), PCA(Principal Component Analysis), SVD(Singular Value Decomposition), Probabilistic approach...

I will approach 3 ways to solve this problem.
Those are SVD, SVD II(Cross product), PCA.
And I will use matlab for example code, because matlab is very simple and useful.


1. SVD method

SVD(singular value decomposition) factorize a input matrix into 3 parts.


If M is input matrix and mxn matrix, SVM factorize U,sigma,V.
U is unitary matrix of mxm, 
Sigma is rectangular diagonal matrix of mxn,
and V is unitary matrix of nxn

In here(to get principal axis of 3d points), V matrix is important.
The biggest major component is the first column in V. Or the column of the largest value in sigma, that column is main component in the V.

See example.

[0.7139 0.0008 0.7003] is matched with 8.9673.
[-0.1302 -0.9824 0.1338] is matched with 5.1319.
[-0.6880 0.1867 0.7012] is matched with 3.3285.

In this case, The biggest principal axis is [0.7139, 0.0008 0.7003].
In line fitting problem, the vector is that line direction and vector. very simple.
Or we can get cross product by [-0.1302 -0.9824 0.1338] X [-0.6880 0.1867 0.7012].

More detail, see example source code in matlab.

2. PCA
PCA is also same concept with SVD. To get main axis, that is to get principal component in some matrix.

In matlab, we can get pca processing result very simple.
like that
[coeff, score, roots] = princomp(X);

And first column is the biggest principal vector.
dirVect = coeff(:,1)

Sorry, I can not explain well in English...
But If you see source code, you can understand more correct.

About line equation in 3d and intorduction, refer to this page->

%make example data
X = mvnrnd(rand(1,3)*10, [1 .2 .7; .2 1 0; .7 0 1],50);

%data drawing
figure(1);
plot3(X(:,1),X(:,2),X(:,3),'r+');
grid on;
hold on;


%Get mean value in data and subtraction data
x = X(:,1);
y = X(:,2);
z = X(:,3);
P=[mean(x),mean(y),mean(z)]; %mean
mX = [x-P(1),y-P(2),z-P(3)]; %subtraction data, X - mean

%set figure axis - that is setted by data
maxlim = max( X(:,:,:) );
minlim = min( X(:,:,:) );
axis([minlim(1) maxlim(1) minlim(2) maxlim(2) minlim(3) maxlim(3)]);
axis square


%SVD
[U, S, V] = svd(mX,0);
%V(:,1) is line direction or main vector
t=[-1000:1000]; %t is range.
SS=V(:,1)*t;
%line equation
A=P(1)+SS(1,:);
B=P(2)+SS(2,:);
C=P(3)+SS(3,:);
%drawing
plot3(A,B,C, 'k-');
...




%%cross product
%SVD
[U, S, V] = svd(mX,0);
direction=cross(V(:,end),V(:,end-1))
%direction(1)
%direction=direction./direction(1)
A2=P(1)+direction(1)*t;
B2=P(2)+direction(2)*t;
C2=P(3)+direction(3)*t;
plot3(A2,B2,C2, 'b-');
...


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% PCA
[coeff,score,roots] = princomp(X);
dirVect = coeff(:,1)

A3=P(1)+dirVect(1)*t;
B3=P(2)+dirVect(2)*t;
C3=P(3)+dirVect(3)*t;
plot3(A3,B3,C3, 'b-');
...

..

And the method to check error is to get distance sum between 3d point of data and point on fitted line that normal direction crossed.

See code.


[n m] =size(X)
x0 = P(1);
y0 = P(2);
z0 = P(3);
a = V(1,1)
b = V(2,1)
c = V(3,1)

terror=0;
for i=1:n
    x = X(i,1);
    y = X(i,2);
    z = X(i,3);
    
    %get corss point on the line and that is of normal direction of 3d point
    t = -(a*x0 - a*x + b*y0 - b*y + c*z0 - c*z) / (a^2 + b^2 + c^2);    
    lx = x0 + t*a ;
    ly = y0 + t*b ;
    lz = z0 + t*c ;

    plot3(lx,ly,lz,'r+'); %%point on the line - A
    plot3(x,y,z,'k+'); %%point of the data - B
    plot3([x lx],[y ly],[z lz],'k-'); %line A to B
    
    d1 = ( (lx-x)^2+(ly-y)^2+(lz-z)^2 ); %uclidian distance between A and B
    terror= d1 + terror;
    
end

disp('total error =')
disp(terror/n )