最小二乘法平面方程拟合计算, 点云法向量估算

转自：https://blog.csdn.net/z444_579/article/details/50039771

设有n个数据点Pi(xi,yi,zi).

假设平面方程为：a*x+b*y+c*z+d=0,其中a、b、c、d为待定系数a、b、c不能同时为0.
显然,a*x+b*y+c*z+d=0与
k*a*x+k*b*y+k*c*z+k*d=0（k≠0）
表示同一个平面.故,如d不为0,可通过把方程两边同除以d,把常数项化为1；但d=0时,情况稍微复杂一点.
现在说明大致思路,为讨论方便,开始时暂不假设d=1或0.
设拟合平面的方程为∏：a*x+b*y+c*z+d=0.
数据点Pi到平面a*x+b*y+c*z+d=0的距离设为di,
则di^2=(a*xi+b*yi+c*zi+d)^2/(a^2+b^2+c^2),
令L=∑di^2 （i=1,...,n）,为目标函数,现欲使L最小.
L可以看成是关于(a,b,c,d)的函数（(xi,yi,zi)均已知）,
L取最小值的一个必要（非充分）条件是：
∂L/∂a=0,∂L/∂b=0,∂L/∂c=0,∂L/∂d=0,
∂L/∂a=∑2*xi*(a*xi+b*yi+c*zi+d)/(a^2+b^2+c^2) （i=1,...,n）
=A1*a+B1*b+C1*c+D1*d,
其中,
A1=2/(a^2+b^2+c^2)*(∑xi^2)（i=1,...,n）,
B1=2/(a^2+b^2+c^2)*(∑xi*yi)（i=1,...,n）,
C1=2/(a^2+b^2+c^2)*(∑xi*zi)（i=1,...,n）,
D1=2/(a^2+b^2+c^2)*(∑xi)（i=1,...,n）,
同理,
∂L/∂b=A2*a+B2*b+C2*c+D2*d,
∂L/∂c=A3*a+B3*b+C3*c+D3*d,
其中,
A2=2/(a^2+b^2+c^2)*(∑yi*xi)（i=1,...,n）,
B2=2/(a^2+b^2+c^2)*(∑yi^2)（i=1,...,n）,
C2=2/(a^2+b^2+c^2)*(∑yi*zi)（i=1,...,n）,
D2=2/(a^2+b^2+c^2)*(∑yi)（i=1,...,n）,
A3=2/(a^2+b^2+c^2)*(∑zi*xi)（i=1,...,n）,
B3=2/(a^2+b^2+c^2)*(∑zi*yi)（i=1,...,n）,
C3=2/(a^2+b^2+c^2)*(∑zi^2)（i=1,...,n）,
D3=2/(a^2+b^2+c^2)*(∑zi)（i=1,...,n）,
∂L/∂d=∑2*(a*xi+b*yi+c*zi+d)/(a^2+b^2+c^2) （i=1,...,n）
=D1*a+D2*b+D3*c+D4*d,
其中,D4=2n/(a^2+b^2+c^2).
于是有方程组：
A1*a+B1*b+C1*c+D1*d=0,
A2*a+B2*b+C2*c+D2*d=0,
A3*a+B3*b+C3*c+D3*d=0,
D1*a+D2*b+D3*c+D4*d=0,

解此方程组即可.具体如何解,可参考计算方法的书,上面有详细说明.

千万注意：上述矩阵的秩rank<=3, 会的人立刻明白怎么结算了，不会的留言吧.