Algorithm 加权最小二乘-将平面拟合到三维点集

我正在用最小二乘法将平面拟合到三维点集。我已经有了这样做的算法，但我想修改它，使用加权最小二乘法。这意味着每个点都有一个权重（权重越大，平面应该离点越近）

当前算法（无权重）如下所示：

计算总和：

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

然后制作矩阵：

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

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

然后求解Ax=B，解的3个分量是拟合平面的系数

---

那么，你能帮助我如何修改这个来使用权重吗？谢谢

将每个总和中的每个项乘以相应的权重。例如：

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

---

由于

pointCloude.size（）

是所有点的

1

之和，因此应将其替换为所有权重之和。

从重新定义最小平方误差计算开始。该公式试图使误差平方和最小化。将平方误差乘以两个点的函数，该函数随距离减小。然后尝试最小化平方误差的加权和，并从中导出系数。

直觉

由法线

n

定义的平面上的点

x

，平面上的点

p

服从：

n.（x-p）=0

。如果点

y

不在平面上，

n.（y-p）

将不等于零，因此定义成本的有用方法是通过

|n.（y-p）^2

。这是点

y

与平面的平方距离

在权重相等的情况下，您希望找到一个

n

，在对各点求和时使总平方误差最小化：

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

现在假设我们知道平面上的某个点。我们可以很容易地计算出一个质心，它只是点云中点的分量平均值，并且始终位于最小二乘平面上

解决方案

让我们定义一个矩阵

M

，其中每行是

第i个
点
x_i
减去质心
c
，我们可以重新写入：

f(n) = | M n |^2

您应该能够说服自己，这个矩阵乘法版本与前面等式中的和相同

然后可以取
M
，然后通过与最小奇异值对应的
M
的右奇异向量给出所需的
n

要合并权重，只需为每个点定义一个权重
w_i
。计算
c
作为点的加权平均值，并将
sum|i|n.（x|i-c）^2
更改为
sum|i|w|i*n.（x|i-c）^2
，矩阵
M
。然后像以前一样求解。
我认为用每个项乘以权重应该足够了，但我不确定。。。我试试看，然后回来。Thank.FYI——如果你可以有很多点（>20）和/或坐标有很大的偏移量，永远不要用你现在的方式（通过计算原始位置的平方和）计算统计数据——它对数值误差的敏感性很差。至少，您可以先减去X/Y/Z坐标的平均值，然后进行处理，最后再将偏移量加回来。还有其他特定于算法的方法可以做到这一点，但我不太明白你的算法是如何使用最小二乘法的，所以我只能这样说。你所说的偏移量是什么意思？（很抱歉，在此上下文中不理解）。快速示例：点p1=（100011000210003），点p2=（100051000610007），点p3=（10009100041008）。这些平均值为（100051000410006）。因此，将点坐标偏移（平移）这个量的相反值，得到p1'=（-4，-2，-3），p2'=（0,2,1），p3'=（4,0,2）。然后做你的数学运算，然后加回偏移量。