https://www.cnblogs.com/zhangli07/p/12013561.html
二、最小二乘面拟合
对空间中的一系列散点，寻求一个近似平面，与线性最小二乘一样，只是变换了拟合方程：
使用平面的一般方程：
Ax + By + CZ + D = 0
可以令平面方程为：
由最小二乘法知：
同样分别取 a0,a1,a2的偏导数：
即是：
换算为矩阵形式有：
可以直接通过克拉默法则求出a0,a1,a2的行列式表达式，有：
c++实现(gDaterm3() 为自定义的三阶行列式计算函数)：
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
|
bool gFittingPlane(double *x, double *y, double *z, int n, double &a, double &b, double &c){ int i; double x1, x2, y1, y2, z1, xz, yz, xy, r; x1 = x2 = y1 = y2 = z1 = xz = yz = xy = 0; for(i=0; i<n; i++) { x1 += x[i]; x2 += x[i]*x[i]; xz += x[i]*z[i]; y1 += y[i]; y2 += y[i]*y[i]; yz += y[i]*z[i]; z1 += z[i]; xy += x[i]*y[i]; } r = gDeterm3(x2, xy, x1, xy, y2, y1, x1, y1, n); if(IS_ZERO(r)) return false; a = gDeterm3(xz, xy, x1, yz, y2, y1, z1, y1, n) / r; b = gDeterm3(x2, xz, x1, xy, yz, y1, x1, z1, n) / r; c = gDeterm3(x2, xy, xz, xy, y2, yz, x1, y1, z1) / r; return true;} |
https://zhidao.baidu.com/question/293032018.html
解：2113
设有n个数据点Pi(xi, yi, zi)。
假设平面方程5261为：a*x+b*y+c*z+d=0，其中a、b、c、d为待定系数4102a、b、c不能同1653时为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，
解此方程组即可。具体如何解，可参考计算方法的书，上面有详细说明。
数值方法解线性方程组最好不要用行列式来算。
在如下网址有一篇介绍文档
http://wenku.baidu.com/view/c9d0713710661ed9ac51f305.html
其中第一步就有遗漏，而且后面还在用行列式来计算，不是太好。
设有n个数据点Pi(xi, yi, zi)。
假设平面方程5261为：a*x+b*y+c*z+d=0，其中a、b、c、d为待定系数4102a、b、c不能同1653时为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，
解此方程组即可。具体如何解，可参考计算方法的书，上面有详细说明。
数值方法解线性方程组最好不要用行列式来算。
在如下网址有一篇介绍文档
http://wenku.baidu.com/view/c9d0713710661ed9ac51f305.html
其中第一步就有遗漏，而且后面还在用行列式来计算，不是太好。
--------------------------------------------------------------------------------------------
通过第一种方法的偏导，启发，理解了第二种方法。但第二种方法程序解方程计算出来的结果有最大常数 × 10的负13次方的误差（C# double乘和除之后 造成误差放大）。第一种方法求出来的平面是错误的结果，但可根据第一种方法的矩阵求解思路，解第二种方法的方程。
分别设 d 不等于零时，取d=1024；
d不等于零时，试取 a = 1024， 或 b =1024 或 c = 1024。只要求得 a b c d 不同时等于零即可。
-------------------------------------------------------------------------------------------------
验证平面方法，取3点，计算出平面，把a b c d代入用于拟合平面的方程，看是否成立，若成立，则可通过解方程拟合平面。
经测试，第一种方法代入a，b，c，d后方程组不成立，故是错误的方程组。
第二种方法代入a，b，c，d后方程组成立，可解方程拟合平面。
using System; using System.Collections.Generic; using System.Linq; using System.Text; using FunctionalProgramming; using FunctionalProgramming.Extension; using MathPrograming.Calculate; namespace MathPrograming.MathSpace { /// <summary> /// 平面ax+by+cz+d=0 /// </summary> public class MathPlane { //public: public double A { get; set; } public double B { get; set; } public double C { get; set; } public double D { get; set; } public double GetDistanceToPoint(double x, double y, double z) { return Math.Abs(A * x + B * y + C * z + D) / Math.Sqrt(A * A + B * B + C * C); } /// <summary> /// 获取平面的法向量 /// </summary> /// <returns></returns> public FuncNode<MathSpatialVector> GetNormalVector() { return new MathSpatialVector { X = this.A, Y = this.B, Z = this.C }; } //pubilc static: /// <summary> /// 拟合平面 最小二乘法 偏导 方程求解(存在很小的误差) /// </summary> /// <param unknown_name="points"></param> /// <returns></returns> public static FuncNode<MathPlane> FittingPlane(MathSpatialPoint[] points) { // 最小二乘法 偏导 方程求解 // a∑x2 + b∑xy + c∑xz + d∑x = 0; // a∑xy + b∑y2 + c∑yz + d∑y = 0; // a∑xz + b∑yz + c∑z2 + d∑z = 0; // a∑x + b∑y + c∑z + d∑1 = 0; double x = 0, x2 = 0, xy = 0, xz = 0, y = 0, y2 = 0, yz = 0, z = 0, z2 = 0, n = points.Length; foreach (MathSpatialPoint point1 in points) { x += point1.X; x2 += point1.X * point1.X; xy += point1.X * point1.Y; xz += point1.X * point1.Z; y += point1.Y; y2 += point1.Y * point1.Y; yz += point1.Y * point1.Z; z += point1.Z; z2 += point1.Z * point1.Z; } #region // 消元法，假设 d为零和不为零的情况求解 2020-8-25 16:30:48 // 编写写四元一次方程组算法 解代码 2020-8-20 17:19:45 var eqs = new List<LinearEquation>(); return LinearEquation.Create(new ExpressionItem[] { new Unknown("a", x2), new Unknown("b", xy), new Unknown("c", xz), new Unknown("d", x), }, new ExpressionItem[] { new Number(0) }) .Bind(eq1 => { eqs.Add(eq1); return LinearEquation.Create(new ExpressionItem[] { new Unknown("a", xy), new Unknown("b", y2), new Unknown("c", yz), new Unknown("d", y), }, new ExpressionItem[] { new Number(0) }); }).Bind(eq1 => { eqs.Add(eq1); return LinearEquation.Create(new ExpressionItem[] { new Unknown("a", xz), new Unknown("b", yz), new Unknown("c", z2), new Unknown("d", z), }, new ExpressionItem[] { new Number(0) }); }).Bind(eq1 => { eqs.Add(eq1); return LinearEquation.Create(new ExpressionItem[] { new Unknown("a", x), new Unknown("b", y), new Unknown("c", z), new Unknown("d", n), }, new ExpressionItem[] { new Number(0) }); }) .Bind(eq1 => { eqs.Add(eq1); return SolvePlane(eqs.ToArray()); // # 测试得，4个组合的结果算出来的平面误差很小 //根据4个方程，拟合可能的平面。 2020-8-26 9:23:44 //return Combination.Of(eqs.ToArray(), 3) // .Bind<MathPlane[]>(combination_arr => // { // if (combination_arr.Length == 0) { return "拟合平面失败,获取平面方程的组合失败。".AsFuncNodeError(); } // List<MathPlane> plane_s = new List<MathPlane>(); // for (int i = 0; i < combination_arr.Length; i++) // { // SolvePlane(combination_arr[i]).ForEach(plane_s.Add); // } // if (plane_s.Count == 0) { return "拟合平面失败。".AsFuncNodeError(); } // return plane_s.ToArray(); // }); }); #endregion } /// <summary> /// 拟合平面 最小二乘法 偏导 矩阵求解 /// </summary> /// <param unknown_name="points"></param> /// <returns></returns> /// todo #z-extend 试用矩阵（克拉默）法则求平面； 2020-8-26 10:13:36 //public static FuncNode<MathPlane> FittingPlane2(MathSpatialPoint[] points) //{ // // 最小二乘法 偏导 方程求解 // // a∑x2 + b∑xy + c∑xz + d∑x = 0; // // a∑xy + b∑y2 + c∑yz + d∑y = 0; // // a∑xz + b∑yz + c∑z2 + d∑z = 0; // // a∑x + b∑y + c∑z + d∑1 = 0; // double x = 0, x2 = 0, xy = 0, xz = 0, y = 0, y2 = 0, yz = 0, z = 0, z2 = 0, n = points.Length; // foreach (MathSpatialPoint point1 in points) // { // x += point1.X; // x2 += point1.X * point1.X; // xy += point1.X * point1.Y; // xz += point1.X * point1.Z; // y += point1.Y; // y2 += point1.Y * point1.Y; // yz += point1.Y * point1.Z; // z += point1.Z; // z2 += point1.Z * point1.Z; // } // #region // var eqs = new List<LinearEquation>(); // //假设 d != 0; 取 d = 1024； // double r = //} /// <summary> /// 已知3点坐标，求平面ax+by+cz+d=0; /// </summary> /// <param unknown_name="p1"></param> /// <param unknown_name="p2"></param> /// <param unknown_name="p3"></param> /// <returns></returns> public static FuncNode<MathPlane> GetPanel(MathSpatialPoint p1, MathSpatialPoint p2, MathSpatialPoint p3) { MathPlane plane = new MathPlane(); plane.A = ((p2.Y - p1.Y) * (p3.Z - p1.Z) - (p2.Z - p1.Z) * (p3.Y - p1.Y)); plane.B = ((p2.Z - p1.Z) * (p3.X - p1.X) - (p2.X - p1.X) * (p3.Z - p1.Z)); plane.C = ((p2.X - p1.X) * (p3.Y - p1.Y) - (p2.Y - p1.Y) * (p3.X - p1.X)); plane.D = (0 - (plane.A * p1.X + plane.B * p1.Y + plane.C * p1.Z)); return plane; } //private static: private static FuncNode<MathPlane> SolvePlane(LinearEquation[] eq_arr) { double a = 0, b = 0, c = 0, d = 0; Dictionary<string, double> dict_vars = new Dictionary<string, double>(); //用于方程的未知数赋默认值 2020-8-25 9:00:57 //假设D 不等于零 ，令A = a/d， B = b/d， C = c/d， D = 1； dict_vars.Add("d", 1024); return LinearEquationEvaluator.Solve(eq_arr, dict_vars) //解方程组 2020-8-24 10:33:50 .Bind(dic1 => { a = dic1["a"]; b = dic1["b"]; c = dic1["c"]; d = dic1["d"]; if (a == 0 && b == 0 && c == 0 && d == 0) { //假设D 等于零 ，令A = a， B = b， C = c， D = 0； dict_vars.Clear(); dict_vars.Add("d", 0); //若d为零，故设 a 不为零。 2020-8-26 8:43:33 dict_vars.Add("a", 1024); return LinearEquationEvaluator.Solve(eq_arr.ToArray(), dict_vars); } return dic1; }).Bind(dic1 => { a = dic1["a"]; b = dic1["b"]; c = dic1["c"]; d = dic1["d"]; if (a == 0 && b == 0 && c == 0 && d == 0) { dict_vars.Clear(); dict_vars.Add("d", 0); //若d为零，故设 b 不为零。 2020-8-26 8:43:33 dict_vars.Add("b", 1024); return LinearEquationEvaluator.Solve(eq_arr.ToArray(), dict_vars); } return dic1; }).Bind(dic1 => { a = dic1["a"]; b = dic1["b"]; c = dic1["c"]; d = dic1["d"]; if (a == 0 && b == 0 && c == 0 && d == 0) { dict_vars.Clear(); dict_vars.Add("d", 0); //若d为零，故设 c 不为零。 2020-8-26 8:43:33 dict_vars.Add("c", 1024); return LinearEquationEvaluator.Solve(eq_arr.ToArray(), dict_vars); } return dic1; }) .Match(dic1 => { a = dic1["a"]; b = dic1["b"]; c = dic1["c"]; d = dic1["d"]; if (a == 0 && b == 0 && c == 0 && d == 0) { return "拟合平面失败, ax + by + cz +d = 0 中 a, b, c, d 不能同时为零".AsFuncNodeError(); } return new MathPlane { A = a, B = b, C = c, D = d, }; }, err => err.ConvertTo<MathPlane>()); } } }
测试代码：
using System; using System.Collections.Generic; using System.Linq; using System.Text; using MathPrograming.Calculate; using MathPrograming.MathSpace; namespace test { class Program { static void Main(string[] args) { MathSpatialPoint v1 = new MathSpatialPoint { X = 2, Y = 31, Z = 1 }; MathSpatialPoint v2 = new MathSpatialPoint { X = 7, Y = 12, Z = 1 }; MathSpatialPoint v3 = new MathSpatialPoint { X = 71, Y = 1, Z = 82 }; var points = new MathSpatialPoint[] { v1, v2, v3, }; double x = 0, x2 = 0, xy = 0, xz = 0, y = 0, y2 = 0, yz = 0, z = 0, z2 = 0, n = points.Length; foreach (MathSpatialPoint point1 in points) { x += point1.X; x2 += point1.X * point1.X; xy += point1.X * point1.Y; xz += point1.X * point1.Z; y += point1.Y; y2 += point1.Y * point1.Y; yz += point1.Y * point1.Z; z += point1.Z; z2 += point1.Z * point1.Z; } Action<MathPlane> show = plane1 => { Console.WriteLine("{3}*{0} + {4}*{1} + {5}*{2} + {6} = 0 {7}", v1.X, v1.Y, v1.Z, plane1.A, plane1.B, plane1.C, plane1.D, ((v1.X * plane1.A + v1.Y * plane1.B + v1.Z * plane1.C + plane1.D) == 0).ToString()); Console.WriteLine("{3}*{0} + {4}*{1} + {5}*{2} + {6} = 0 {7}", v2.X, v2.Y, v2.Z, plane1.A, plane1.B, plane1.C, plane1.D, ((v2.X * plane1.A + v2.Y * plane1.B + v2.Z * plane1.C + plane1.D) == 0).ToString()); Console.WriteLine("{3}*{0} + {4}*{1} + {5}*{2} + {6} = 0 {7}", v3.X, v3.Y, v3.Z, plane1.A, plane1.B, plane1.C, plane1.D, ((v3.X * plane1.A + v3.Y * plane1.B + v3.Z * plane1.C + plane1.D) == 0).ToString()); Console.WriteLine("a = {0}, b = {1}, c = {2}, d = {3}", plane1.A, plane1.B, plane1.C, plane1.D); Action<double, double, double, double, double, double, double, double> write_line = (_a, _b, _c, _d, _x, _y, _z, _n) => { Console.WriteLine("{4}*{0} + {5}*{1} + {6}*{2} + {7}*{3} = 0 {8}", _x, _y, _z, _n, _a, _b, _c, _d, ((_x * _a + _y * _b + _z * _c + _n * _d) == 0).ToString()); }; write_line(plane1.A, plane1.B, plane1.C, plane1.D, x2, xy, xz, x); write_line(plane1.A, plane1.B, plane1.C, plane1.D, xy, y2, yz, y); write_line(plane1.A, plane1.B, plane1.C, plane1.D, xz, yz, z2, z); write_line(plane1.A, plane1.B, plane1.C, plane1.D, x, y, z, n); }; //3 点构成平面 MathPlane.GetPanel(v1, v2, v3).ForEach(show); #if DEBUG #endif //多点拟合平面 MathPlane.FittingPlane(points).Match(res => { Console.WriteLine(); Console.WriteLine(); show(res); }, err => Console.WriteLine(err)); Console.ReadKey(); } } }