Estou ajustando um plano a um ponto 3D definido com o método do quadrado mínimo. Eu já tenho um algoritmo para fazer isso, mas quero modificá-lo para usar o quadrado mínimo ponderado. Ou seja, tenho um peso para cada ponto (quanto maior o peso, mais próximo o avião deve estar do ponto

O algoritmo atual (sem peso) fica assim:

Calcule a soma:

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

do que fazer as matrizes:

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

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

do que resolver Ax = B e os 3 componentes da solução são os coeficientes da superfície plana montada ...

Então, você pode me ajudar como modificar isso para usar pesos? Obrigado