Weighted Least Square - Set the plane to a 3D point

Question

I set the plane to a three-dimensional point with the smallest square method. I already have an algorithm for this, but I want to change it to use the weighted least square. Value I have a weight for each point (the greater the weight, the closer the plane should be to the point).

The current algorithm (without weight) is as follows:

Calculate the amount:

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];
}

how to make matrices:

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

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

than the solution Ax = B, and the 3 components of the solution are the coefficients of the applied plain ...

So can you please help me change this to use weight? Thank!

Answer 1

Intuition

x , n, p : n.(x - p) = 0. y , n.(y -p) , - |n.(y - p)|^2. y .

n, :

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

, p, . , .

M, ith x_i c, :

f(n) = | M n |^2

, .

M, n, , M, .

, w_i . c sum_i | n.(x_i - c) |^2 sum_i | w_i * n.(x_i - c) |^2, M . , .

Answer 2

. :

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

pointCloude.size() 1 , .

Answer 3

. . , . .