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>
/// X矩阵如下所示
///
///
/// </summary>
/// <param name="X"></param>
/// <param name="Y"></param>
/// <returns></returns>
public double CalculateTrans7Param(double[,] X, double[,] Y)
{
int PtNum = X.GetLength(0) / 3;
double[,] B;
double[,] F;
double[,] BT = MatrixTool.Init(7, 3 * PtNum);
double[,] BTB = MatrixTool.Init(7, 7);
double[,] BTF = MatrixTool.Init(7, 1);
//init pararm
CoordTrans7Param dpp = new CoordTrans7Param();
Set4Param(0, 0, 0, 0);
this.SetRotationParam(0, 0, 0);
//debug
//this.TransCoord(X[0,0],X[1,0],X[2,0],out x2,out y2,out z2);
int round = 0;
while (round++ < 20)
{
F = GetF(X, Y);
B = GetB(X);
MatrixTool.AT(B, ref BT);
MatrixTool.Multi(BT, B, ref BTB);
MatrixTool.Inv(BTB);
MatrixTool.Multi(BT, F, ref BTF);
MatrixTool.Multi(BTB, BTF, ref dpp.values);
if (dpp.isSmall())
{
break;
}
else
{
MatrixTool.Add(this.values, dpp.values, ref this.values);
}
}
Console.WriteLine(round);
//this.TransCoord(X[0,0],X[1,0],X[2,0],out x2,out y2,out z2);
//double[,] V = GetV(X, Y, dpp);
double vMax = -1;
//for (int i = 0; i < V.GetLength(0); i++)
//{
// if (Math.Abs(V[i, 0]) > vMax)
// vMax = Math.Abs(V[i, 0]);
//}
return(vMax);
}
private void button1_Click(object sender, EventArgs e)
{
double x1, y1, x2, y2;
x1 = 119;
x2 = 120;
y1 = y2 = 35;
Console.WriteLine("x:" + CoordinateConverter.getUTMDistance(x1, y1, x2, y2));
Console.WriteLine("y:" + CoordinateConverter.getUTMDistance(x1, y1, x1, y2 + 1));
string fileName = @"D:\sevenParamsTest.xlsx";
//if (chainList.Count > 0)
// chainList.Clear();
//LoadDataTableFromExcel(fileName, "Sheet1");
int ptNum = 10;
double[,] X = MatrixTool.Init(3 * ptNum, 1);
double[,] Y = MatrixTool.Init(3 * ptNum, 1);
double[,] Y84 = MatrixTool.Init(3 * ptNum, 1);
int pN = 10;
double[] XX = new double[pN * 3];
double[] YYRead = new double[pN * 3];
double[] YY = new double[pN * 3];
double[] YY84 = new double[pN * 3];
int i = 0;
int j = 0;
int k = 0;
foreach (DataRow dr in ds.Tables["Sheet1"].Rows)
{
if (i % 20 == 0 && j < ptNum)
{
X[j * 3 + 1, 0] = Convert.ToDouble(dr["North"]);
X[j * 3 + 0, 0] = Convert.ToDouble(dr["East"]);
X[j * 3 + 2, 0] = Convert.ToDouble(dr["Altitude"]);
Y84[j * 3 + 0, 0] = Convert.ToDouble(dr["Longitude"]);
Y84[j * 3 + 1, 0] = Convert.ToDouble(dr["Latitude"]);
Y84[j * 3 + 2, 0] = Convert.ToDouble(dr["Altitude"]);
j++;
}
if (i % 20 == 10 && k < ptNum)
{
XX[k * 3 + 1] = Convert.ToDouble(dr["North"]);
XX[k * 3 + 0] = Convert.ToDouble(dr["East"]);
XX[k * 3 + 2] = Convert.ToDouble(dr["Altitude"]);
YYRead[k * 3 + 0] = Convert.ToDouble(dr["Longitude"]);
YYRead[k * 3 + 1] = Convert.ToDouble(dr["Latitude"]);
YYRead[k * 3 + 2] = Convert.ToDouble(dr["Altitude"]);
k++;
}
i++;
if (j == ptNum && k == ptNum)
{
break;
}
}
for (i = 0; i < ptNum; i++)
{
CoordinateConverter.LatLonToUTMXY(Y84[3 * i + 1, 0], Y84[3 * i + 0, 0], out Y[3 * i + 0, 0], out Y[3 * i + 1, 0]);
}
CoordTrans7Param ct = new CoordTrans7Param();
ct.CalculateTrans7Param(X, Y);
for (i = 0; i < pN; i++)
{
ct.TransCoord(XX[i * 3 + 0], XX[i * 3 + 1], XX[i * 3 + 2], out YY[i * 3 + 0], out YY[i * 3 + 1], out YY[i * 3 + 2]);
CoordinateConverter.UTMXYToLatLon(YY[i * 3 + 0], YY[i * 3 + 1], out YY84[i * 3 + 1], out YY84[i * 3 + 0]);
Console.WriteLine("准确的 x {0}, y {1}, z{2}, 计算的 x {3} , y {4} , z {5}", YYRead[i * 3 + 0], YYRead[i * 3 + 1], YYRead[i * 3 + 2], YY84[i * 3 + 0], YY84[i * 3 + 1], YY[i * 3 + 2]);
}
}
