/// <summary> /// 采用另外一种方式进行数学计算, 最小二乘法做一定的约束条件,约束方程为 J = L2*(A*aParam + B*bParam)^2) /// 这种方式可以小范围的调整角度 /// </summary> /// <param name="xArray"></param> /// <param name="yArray"></param> /// <param name="aParam"></param> /// <param name="bParam"></param> /// <param name="L2"></param> /// <returns></returns> public static bool leastSquareAdaptLine2(double[] xArray, double[] yArray, double aParam, double bParam, out cylineParam lineValue, double L2 = 100) { if (L2 <= 0) { return(leastSquareAdaptLine(new List <double>(xArray), new List <double>(yArray), out lineValue)); } lineValue = new cylineParam(); Trace.Assert(xArray.Length == yArray.Length); if (xArray.Length < 1) { return(false); } Trace.Assert(xArray.Length == yArray.Length); cyPointBaseClass class1 = new cyPointBaseClass(); int nums = xArray.Length; double xx = 0, yy = 0; double xy = 0; double x = 0, y = 0; for (int i = 0; i < xArray.Length; i++) { xx += xArray[i] * xArray[i]; yy += yArray[i] * yArray[i]; xy += xArray[i] * yArray[i]; y += yArray[i]; x += xArray[i]; } /* [ xx + n*L2*bParam, x ] [xy - n*L2*aParam] [n, -x] * T = [ ] , B = [ ], T* =[ ] * [ x, n ] [ y ] [-x xx + n*L2*bParam] */ double t11 = nums, t12 = -x; double t21 = t12, t22 = (xx + nums * L2 * bParam); double b1 = xy - nums * L2 * aParam; double b2 = y; double div = nums * (xx + nums * L2 * bParam) - x * x; double a = t11 * b1 + t12 * b2; double b = t21 * b1 + t22 * b2; if (Math.Abs(a) <= 1e-6 && Math.Abs(-div) <= 1e-6) { return(true); } double dValue = Math.Sqrt(a * a + div * div); lineValue.A = a / dValue; lineValue.B = -div / dValue; lineValue.C = -b / dValue; return(true); }
public static bool leastSquareAdaptLineWeight(List <double> xArray, List <double> yArray, List <double> weight, out cylineParam lineValue) { Trace.Assert(xArray.Count == yArray.Count); cyPointBaseClass class1 = new cyPointBaseClass(); lineValue = new cylineParam() { A = 0, B = 0, C = 0 }; int nums = xArray.Count; List <double> matrix = new List <double>() { 0, 0, 0, 0 }; List <double> bias = new List <double>() { 0, 0 }; double xx = 0, xy = 0, yy = 0, x = 0, y = 0; for (int i = 0; i < xArray.Count; i++) { xx += xArray[i] * xArray[i] * weight[i]; yy += yArray[i] * yArray[i] * weight[i]; xy += xArray[i] * yArray[i] * weight[i]; x += xArray[i] * weight[i]; y += yArray[i] * weight[i]; } matrix[0] = xx; matrix[1] = xy; matrix[2] = matrix[1]; matrix[3] = yy; bias[0] = x; bias[1] = y; List <double> result; List <double> invMatrix; cyUCSMeasure.MatrixInv2X2(matrix, out invMatrix); cyUCSMeasure.Matrix_Mult2X1(invMatrix, bias, out result); lineValue.A = result[0]; lineValue.B = result[1]; //double c = (result[0]*x + result[1]*y) /weight.Sum() ; lineValue = new cylineParam() { A = result[0], B = result[1], C = 1 }; lineValue.OnNormalize(); return(true); }
/// <summary> /// 根据点坐标拟合一条直线 /// </summary> public static bool leastSquareAdaptLine(List <double> xArray, List <double> yArray, out cylineParam lineValue) { Trace.Assert(xArray.Count == yArray.Count); cyPointBaseClass class1 = new cyPointBaseClass(); lineValue = new cylineParam() { A = 0, B = 0, C = 0 }; int nums = xArray.Count; List <double> matrix = new List <double>() { 0, 0, 0, 0 }; List <double> bias = new List <double>() { 0, 0 }; double xx = 0, xy = 0, yy = 0, x = 0, y = 0; for (int i = 0; i < xArray.Count; i++) { xx += xArray[i] * xArray[i]; yy += yArray[i] * yArray[i]; x += xArray[i]; xy += xArray[i] * yArray[i]; y += yArray[i]; } double dVAlue1 = xx - (x * x) / nums; double dValue2 = yy - (y * y) / nums; bool bFunX = true; // y方向的方差更小, 那证明, 应该采用 x= a*y +b; if (dVAlue1 < dValue2) { bFunX = false; } if (bFunX == true) { //y = ax+b; matrix[0] = xx; matrix[1] = x; matrix[2] = matrix[1]; matrix[3] = nums; bias[0] = xy; bias[1] = y; } else { //ay +b = x matrix[0] = yy; matrix[1] = y; matrix[2] = matrix[1]; matrix[3] = nums; bias[0] = xy; bias[1] = x; } List <double> Invmatrix = null; MatrixInv2X2(matrix, out Invmatrix); // AAd*y = a*x +c List <double> reusltMatrix = new List <double>(); Matrix_Mult2X1(Invmatrix, bias, out reusltMatrix); double a = reusltMatrix[0]; double c = reusltMatrix[1]; //三次去掉最大的偏差点 List <double> listDist = new List <double>(); List <double> moveIndex = new List <double>(); List <double> moveXArray = new List <double>(); List <double> moveYArray = new List <double>(); int nums2 = xArray.Count(); for (int t = 0; t < 3; t++) { // 求平均距离, 和最大的偏置项,在置信度[-99.6, 99.6]内的值保留,其他的去掉 double sValue = 0; double uValue = 0; if (xArray.Count() < nums2 * 2 / 3 || xArray.Count() <= 2) { break; } for (int j = 0; j < xArray.Count; j++) { double pValue = 0; pValue = (bFunX == true) ? (xArray[j] * a + c - yArray[j]) / Math.Sqrt(a * a + 1) : (yArray[j] * a + c - xArray[j]) / Math.Sqrt(a * a + 1); listDist.Add(pValue); uValue += pValue; sValue += pValue * pValue; } uValue /= xArray.Count; sValue = Math.Sqrt((sValue - (uValue * uValue) * xArray.Count) / (xArray.Count - 1)); double distMin = uValue - 1.96 * sValue; double distMax = uValue + 1.96 * sValue; nums = xArray.Count; for (int j = 0; j < xArray.Count; j++) { if (listDist[j] > distMin && listDist[j] < distMax) { continue; } matrix[0] -= ((bFunX == true)? (xArray[j] * xArray[j]):(yArray[j] * yArray[j])); matrix[1] -= ((bFunX == true) ? xArray[j]:yArray[j]); bias[0] -= xArray[j] * yArray[j]; bias[1] -= ((bFunX == true) ? yArray[j]:xArray[j]); xArray.RemoveAt(j); yArray.RemoveAt(j); listDist.RemoveAt(j); //double xValue = xArray[j]; //double yValue = yArray[j]; //moveXArray.Add(xValue); //moveYArray.Add(yValue); j--; } matrix[3] = xArray.Count; matrix[2] = matrix[1]; MatrixInv2X2(matrix, out Invmatrix); Matrix_Mult2X1(Invmatrix, bias, out reusltMatrix); a = reusltMatrix[0]; c = reusltMatrix[1]; listDist.Clear(); if (xArray.Count == nums) { break; } } double div = Math.Sqrt(a * a + 1); if (bFunX == true) { // y=ax+c lineValue.A = a / div; lineValue.B = -1 / div; lineValue.C = -c / div; } else { //x = ay+c lineValue.A = 1 / div; lineValue.B = -a / div; lineValue.C = c / div; } return(true); }
/// <summary> /// 根据点坐标拟合一条直线 /// </summary> public static bool leastSquareAdaptCirCle(double[] xArray, double[] yArray, out cyCircleParam circleValue) { Trace.Assert(xArray.Length == yArray.Length); cyPointBaseClass class1 = new cyPointBaseClass(); circleValue = new cyCircleParam() { xCenter = 0, yCenter = 0, radius = 0 }; //最小二乘法矩阵: // [ xx, xy, x ] [a] [-Ni_Xi] // [ xy, yy, y ]*[b] = [-Ni_Yi]; |A*AT| = |A|^n // [ x, y, nums] [c] [ -Ni ] int nums = xArray.Length; List <double> matrix = new List <double>(9) { 0, 0, 0, 0, 0, 0, 0, 0, 0 }; List <double> ResltMatr = new List <double>(3) { 0, 0, 0 }; for (int i = 0; i < xArray.Length; i++) { double tmpXX = xArray[i] * xArray[i]; double tmpYY = yArray[i] * yArray[i]; double tmpNi = tmpXX + tmpYY; ResltMatr[0] -= tmpNi * xArray[i]; ResltMatr[1] -= tmpNi * yArray[i]; ResltMatr[2] -= tmpNi; matrix[0] += tmpXX; matrix[1] += xArray[i] * yArray[i]; matrix[2] += xArray[i]; matrix[4] += tmpYY; matrix[5] += yArray[i]; } matrix[3] = matrix[1]; matrix[6] = matrix[2]; matrix[7] = matrix[5]; matrix[8] = nums; //List<double> matrix = new List<double> { xx, xy, x, xy, yy, y, x, y, nums }; List <double> INV_matrix; cyUCSMeasure.MatrixInv3X3(matrix, out INV_matrix); if (INV_matrix.Count != 9) { return(false); } List <double> circleData; cyUCSMeasure.Matrix_Mult3X1(INV_matrix, ResltMatr, out circleData); circleValue.xCenter = -circleData[0] / 2; circleValue.yCenter = -circleData[1] / 2; double tValue = circleData[0] * circleData[0] / 4 + circleData[1] * circleData[1] / 4 - circleData[2]; if (tValue < 1e-6) { return(false); } circleValue.radius = Math.Sqrt(tValue); return(true); }