Weighted least square - fit a plane to 3D point set

Question

I am fitting a plane to a 3D point set with the least square method. I already have algorithm to do that, but I want to modify it to use weighted least square. Meaning I have a weight for each point (the bigger weight, the closer the plane should be to the point).

The current algorithm (without weight) looks like this:

Compute the sum:

for(Point3D p3d : pointCloud) {
    pos = p3d.getPosition();
    fSumX += pos[0];
    fSumY += pos[1];
    fSumZ += pos[2];
    fSumXX += pos[0]*pos[0];
    fSumXY += pos[0]*pos[1];
    fSumXZ += pos[0]*pos[2];
    fSumYY += pos[1]*pos[1];
    fSumYZ += pos[1]*pos[2];
}

than make the matrices:

double[][] A = {
    {fSumXX, fSumXY, fSumX},
    {fSumXY, fSumYY, fSumY},
    {fSumX,  fSumY,  pointCloud.size()}
};

double[][] B =  {
    {fSumXZ},
    {fSumYZ},
    {fSumZ}
};

than solve Ax = B and the 3 components of the solution are the coefficients of the fitted plain...

So, can you please help me how to modify this to use weights? Thanks!

Answer 1

Multiply each term in each sum by the corresponding weight. For example:

fSumZ += weight * pos[2];
fSumXX += weight * pos[0]*pos[0];

Since pointCloude.size() is the sum of 1 for all points, it should be replaced with the sum of all weights.

Answer 2

Intuition

A point x on a plane defined by normal n and a point on the plane p obeys: n.(x - p) = 0. If a point y does not lie on the plane, n.(y -p) will not be equal to zero, so a useful way to define a cost is by |n.(y - p)|^2 . This is the squared distance of the point y from the plane.

With equal weights, you want to find an n that minimizes the total squared error when summing over the points:

f(n) = sum_i | n.(x_i - p) |^2

Now this assumes we know some point p that lies on the plane. We can easily compute one as the centroid, which is simply the component-wise mean of the points in the point cloud and will always lie in the least-squares plane.

Solution

Let's define a matrix M where each row is the ith point x_i minus the centroid c, we can re-write:

f(n) = | M n |^2

You should be able to convince yourself that this matrix multiplication version is the same as the sum on the previous equation.

You can then take singular value decomposition of M, and the n you want is then given by the right singular vector of M that corresponds to the smallest singular value.

To incorporate weights you simply need to define a weight w_i for each point. Calculate c as the weighted average of the points, and change sum_i | n.(x_i - c) |^2 to sum_i | w_i * n.(x_i - c) |^2, and the matrix M in a similar way. Then solve as before.