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!
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.
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.
Start from redefining the least-square error calculation. The formula tries to minimize the sum of squares of errors. Multiply the squared error by a function of two points which decreases with their distance. Then try to minimize the weighted sum of squared errors and derive the coefficients from that.