Fast plane fitting to many points

Question

I\'m looking to fit a plane to a set of ~ 6-10k 3D points. I\'m looking to do this as fast as possible, and accuracy is not the highest concern (frankly the plane can be off

Answer 1

Use the standard plane equation Ax + By + Cz + D = 0, and write the equation as a matrix multiplication. P is your unknown 4x1 [A;B;C;D]

g = [x y z 1];  % represent a point as an augmented row vector
g*P = 0;        % this point is on the plane

Now expand this to all your actual points, an Nx4 matrix G. The result is no longer exactly 0, it's the error you're trying to minimize.

G*P = E;   % E is a Nx1 vector

So what you want is the closest vector to the null-space of G, which can be found from the SVD. Let's test:

% Generate some test data
A = 2;
B = 3;
C = 2.5;
D = -1;

G = 10*rand(100, 2);  % x and y test points
% compute z from plane, add noise (zero-mean!)
G(:,3) = -(A*G(:,1) + B*G(:,2) + D) / C + 0.1*randn(100,1);

G(:,4) = ones(100,1);   % augment your matrix

[u s v] = svd(G, 0);
P = v(:,4);             % Last column is your plane equation

OK, remember that P can vary by a scalar. So just to show that we match:

scalar = 2*P./P(1);
P./scalar

ans = 2.0000 3.0038 2.5037 -0.9997