【數學模型】擬合平面
阿新 • • 發佈：2020-08-26
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 |
boolgFittingPlane(double*x,double*y,double*z,intn,double&a,double&b,double&c)
{
inti;
doublex1, 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))returnfalse;
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;
returntrue;
}
|
https://zhidao.baidu.com/question/293032018.html
2011-07-18 關注 解：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
其中第一步就有遺漏，而且後面還在用行列式來計算，不是太好。
--------------------------------------------------------------------------------------------
通過第一種方法的偏導，啟發，理解了第二種方法。但第二種方法程式解方程計算出來的結果有最大常數 × 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(); } } }