private double[,] GetV(double[,] X, double[,] Y, CoordTrans7Param dpp)
{
int rowNum = X.GetLength(0);
double[,] B, F, A, B2, B3, F2, V;
double[,] AT = MatrixTool.Init(6, 3);
A = GetA();
MatrixTool.AT(A, ref AT);
MatrixTool.MutliConst(ref AT, 1 / (1 + (1 + k) * (1 + k)));
F = GetF(X, Y);
B = GetB(X);
B2 = MatrixTool.Init(3, 7);
B3 = MatrixTool.Init(3, 1);
F2 = MatrixTool.Init(rowNum, 1);
for (int i = 0; i < rowNum / 3; i++)
{
MatrixTool.CopySub(B, i * 3, 0, 3, 7, ref B2, 0, 0);
MatrixTool.Multi(B2, dpp.values, ref B3);
MatrixTool.CopySub(B3, 0, 0, 3, 1, ref F2, i * 3, 0);
}
MatrixTool.Sub(F, F2, ref F2);
V = specialMulti(AT, F2);
return(V);
}
/// <summary>
/// 最小二乘误差矩阵
/// [1 0 -y x]
/// [0 1 x y]
/// </summary>
/// <param name="X"></param>
/// <returns></returns>
private double[,] GetB(double[,] X)
{
int rowNum = X.GetLength(0);
double[,] B = MatrixTool.Init(rowNum, 4);
//double[,] M = GetM();
double[,] mi = MatrixTool.Ident(2);
//double[,] MK;
//MK = specialMulti(M, X);
for (int i = 0; i < rowNum; i += 2)
{
MatrixTool.CopySub(mi, 0, 0, 2, 2, ref B, i, 0);
}
for (int i = 0; i < rowNum; i += 2)
{
B[i, 2] = X[i, 0];
B[i, 3] = -X[i + 1, 0];
B[i + 1, 2] = X[i + 1, 0];
B[i + 1, 3] = X[i, 0];
}
return(B);
}
private double[,] GetA()
{
double[,] M = GetM();
double[,] I2 = MatrixTool.Ident(3);
double[,] A = MatrixTool.Init(3, 6);
MatrixTool.MutliConst(ref I2, -1);
MatrixTool.MutliConst(ref M, (1 + k));
MatrixTool.CopySub(M, 0, 0, 3, 3, ref A, 0, 0);
MatrixTool.CopySub(I2, 0, 0, 3, 3, ref A, 0, 3);
return(A);
}
private double[,] specialSub(double[,] m, double[,] X)
{
int rowNumM = m.GetLength(0);
int colNumM = m.GetLength(1);
int rowNumX = X.GetLength(0);
int colNumX = X.GetLength(1);
int lines = rowNumX / rowNumM;
double[,] subX = MatrixTool.Init(rowNumM, colNumX);
double[,] res = MatrixTool.Init(rowNumX, colNumX);
for (int i = 0; i < rowNumX; i += rowNumM)
{
MatrixTool.CopySub(X, i, 0, rowNumM, colNumX, ref subX, 0, 0);
MatrixTool.Sub(m, subX, ref subX);
MatrixTool.CopySub(subX, 0, 0, rowNumM, colNumX, ref res, i, 0);
}
return(res);
}
private double[,] GetB(double[,] X)
{
int rowNum = X.GetLength(0);
double[,] B = MatrixTool.Init(rowNum, 7);
//double[,] M = GetM();
//double[,] Mdx = GetMdx();
//double[,] Mdy = GetMdy();
//double[,] Mdz = GetMdz();
double[,] mi = MatrixTool.Ident(3);
//double[,] MX, MY, MZ, MK;
//MK = specialMulti(M, X);
//MX = specialMulti(Mdx, X);
//MY = specialMulti(Mdy, X);
//MZ = specialMulti(Mdz, X);
for (int i = 0; i < rowNum; i += 3)
{
MatrixTool.CopySub(mi, 0, 0, 3, 3, ref B, i, 0);
B[i, 3] = 0;
B[i, 4] = -X[i + 2, 0];
B[i, 5] = X[i + 1, 0];
B[i, 6] = X[i, 0];
B[i + 1, 3] = X[i + 2, 0];
B[i + 1, 4] = 0;
B[i + 1, 5] = -X[i + 0, 0];
B[i + 1, 6] = X[i + 1, 0];
B[i + 2, 3] = -X[i + 1, 0];
B[i + 2, 4] = X[i + 0, 0];
B[i + 2, 5] = 0;
B[i + 2, 6] = X[i + 2, 0];
}
//MatrixTool.CopySub(MX, 0, 0, rowNum, 1, ref B, 0, 3);
//MatrixTool.CopySub(MY, 0, 0, rowNum, 1, ref B, 0, 4);
//MatrixTool.CopySub(MZ, 0, 0, rowNum, 1, ref B, 0, 5);
//MatrixTool.CopySub(MK, 0, 0, rowNum, 1, ref B, 0, 6);
return(B);
}
//private double[,] GetMdx()
//{
// double[,] mt = { { 0, 0, 0 }, { 0, -Math.Sin(rx), Math.Cos(rx) }, { 0, -Math.Cos(rx), -Math.Sin(rx) } };
// double[,] m = new double[3, 3];
// MatrixTool.Multi(GetMz(), GetMy(), ref m);
// MatrixTool.Multi(m, mt, ref m);
// return m;
//}
//private double[,] GetMdy()
//{
// double[,] mt = { { -Math.Sin(ry), 0, -Math.Cos(ry) }, { 0, 0, 0 }, { Math.Cos(ry), 0, -Math.Sin(ry) } };
// double[,] m = new double[3, 3];
// MatrixTool.Multi(GetMz(), mt, ref m);
// MatrixTool.Multi(m, GetMx(), ref m);
// return m;
//}
//private double[,] GetMdz()
//{
// double[,] mt = { { -Math.Sin(rz), Math.Cos(rz), 0 }, { -Math.Cos(rz), -Math.Sin(rz), 0 }, { 0, 0, 0 } };
// double[,] m = new double[3, 3];
// MatrixTool.Multi(mt, GetMy(), ref m);
// MatrixTool.Multi(m, GetMx(), ref m);
// return m;
//}
private double[,] specialMulti(double[,] m, double[,] X)
{
int rowNumM = m.GetLength(0);
int colNumM = m.GetLength(1);
int rowNumX = X.GetLength(0);
int colNumX = X.GetLength(1);
int lines = rowNumX / colNumM;
double[,] mt = MatrixTool.Init(rowNumM, colNumX);
double[,] subX = MatrixTool.Init(colNumM, colNumX);
double[,] res = MatrixTool.Init(rowNumM * lines, colNumX);
for (int i = 0; i < lines; i++)
{
MatrixTool.CopySub(X, i * colNumM, 0, colNumM, colNumX, ref subX, 0, 0);
MatrixTool.Multi(m, subX, ref mt);
MatrixTool.CopySub(mt, 0, 0, rowNumM, colNumX, ref res, i * rowNumM, 0);
}
return(res);
}
