/// <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);
}
