/// <summary>
/// 构建基线网平差观测方程的信息,得到法方程的系数ATPA和常数ATPL
/// </summary>
private void AdjustMatrixBuilder()
{
int row = BaselineInfo.Count * 3;
int column = AllPointsApproXyz.Count * 3;
CoeffOfParam = new ArrayMatrix(row, column);
Observation = new ArrayMatrix(row, 1);
InverseOfWeight = new ArrayMatrix(row, row);
//BeginPoint = new int[row];
//EndPoint = new int[row];
int i = 0;
foreach (var item in BaselineInfo)
{
string[] name = item.Key.Split('-');
string refName = name[0].ToUpper();
string rovName = name[1].ToUpper();
int refIndex = AllPointName.IndexOf(refName);
int rovIndex = AllPointName.IndexOf(rovName);
//系数项
//BeginPoint[i] = refIndex;
//EndPoint[i] = rovIndex;
CoeffOfParam[3 * i + 0, 3 * refIndex + 0] = -1; CoeffOfParam[3 * i + 0, 3 * rovIndex + 0] = 1;
CoeffOfParam[3 * i + 1, 3 * refIndex + 1] = -1; CoeffOfParam[3 * i + 1, 3 * rovIndex + 1] = 1;
CoeffOfParam[3 * i + 2, 3 * refIndex + 2] = -1; CoeffOfParam[3 * i + 2, 3 * rovIndex + 2] = 1;
//常数项
//ObsL[3 * i + 0] = key.Value.X;
//ObsL[3 * i + 1] = key.Value.Y;
//ObsL[3 * i + 2] = key.Value.Z;
Observation[3 * i + 0, 0] = item.Value.X;
Observation[3 * i + 1, 0] = item.Value.Y;
Observation[3 * i + 2, 0] = item.Value.Z;
if (item.Value.X >= 0.06)
{
item.Value.X = 0.06 / 2;
}
if (item.Value.Y >= 0.06)
{
item.Value.Y = 0.06 / 2;
}
if (item.Value.Z >= 0.06)
{
item.Value.Z = 0.06 / 2;
}
//权逆阵
InverseOfWeight[3 * i + 0, 3 * i + 0] = BaselineInverseOfWegihtInfo[item.Key].Array[0][0]; InverseOfWeight[3 * i + 0, 3 * i + 1] = BaselineInverseOfWegihtInfo[item.Key].Array[1][0]; InverseOfWeight[3 * i + 0, 3 * i + 2] = BaselineInverseOfWegihtInfo[item.Key].Array[2][0];
InverseOfWeight[3 * i + 1, 3 * i + 0] = BaselineInverseOfWegihtInfo[item.Key].Array[1][0]; InverseOfWeight[3 * i + 1, 3 * i + 1] = BaselineInverseOfWegihtInfo[item.Key].Array[1][1]; InverseOfWeight[3 * i + 1, 3 * i + 2] = BaselineInverseOfWegihtInfo[item.Key].Array[2][1];
InverseOfWeight[3 * i + 2, 3 * i + 0] = BaselineInverseOfWegihtInfo[item.Key].Array[2][0]; InverseOfWeight[3 * i + 2, 3 * i + 1] = BaselineInverseOfWegihtInfo[item.Key].Array[2][1]; InverseOfWeight[3 * i + 2, 3 * i + 2] = BaselineInverseOfWegihtInfo[item.Key].Array[2][2];
//
i++;
}
IMatrix Weight = InverseOfWeight.GetInverse();
IMatrix Tmp = CoeffOfParam.Transposition;//.Multiply(Weight);
this.ATPA = Tmp.Multiply(CoeffOfParam);
this.ATPL = Tmp.Multiply(Observation);
////提取存放下三角
//int ni = InverseOfWeight.RowCount;
//InverseWeightOfObsL = new double[(ni + 1) * ni / 2];
//int sk = 0;
//for (int s1 = 0; s1 < ni; s1++)
//{
// for (int s2 = 0; s2 <= s1; s2++)
// {
// InverseWeightOfObsL[sk] = InverseOfWeight[s1, s2];
// sk++;
// }
//}
}
private void button_Calculate_Click(object sender, EventArgs e)
{
//读取基线信息
Int();
//根据基线信息建立观测方程和法方程,此时是条件数不足的，无法解
AdjustMatrixBuilder();
#region 根据所选网平差方法，对法方程增加不同的约束条件
string outName = "平差文件";
if (this.radioButton_fixedConstraintAdj.Checked) //固定点平差
{
outName = "固定点法平差文件";
foreach (var item in KonwnPointsXyz)
{
string name = item.Key.ToUpper();
XYZ Xyz = item.Value;
int index = AllPointName.IndexOf(name);
if (!AllPointsApproXyz.ContainsKey(name))
{
//
}
this.ATPA[3 * index + 0, 3 * index + 0] += 1.0e30;
this.ATPA[3 * index + 1, 3 * index + 1] += 1.0e30;
this.ATPA[3 * index + 2, 3 * index + 2] += 1.0e30;
}
}
else if (this.radioButton_freeNetAdj.Checked) //自由网约束平差
{
outName = "自由网松约束法平差文件";
double constrainValue = 1.0 / this.AllPointName.Count;
for (int i = 0; i < ATPA.RowCount; i++)
{
ATPA[i, i] += constrainValue;
}
}
else if (this.radioButton_minConstraintAdj.Checked) //最小约束条件平差 见 Bernese 5.2 Doc P221. Gamit 叫相似变换？？？
{
outName = "最小约束条件法平差文件";
int count = AllPointName.Count;
ArrayMatrix B = B = new ArrayMatrix(count * 3, 7);
ArrayMatrix h = new ArrayMatrix(count * 3, 1);
ArrayMatrix Ph = new ArrayMatrix(count * 3, count * 3);
int k = 0;
foreach (var item in KonwnPointsXyz)
{
string staName = item.Key;
XYZ xyz = item.Value; //真值
int index = AllPointName.IndexOf(staName);
B[3 * index + 0, 0] = 1; B[3 * index + 0, 1] = 0; B[3 * index + 0, 2] = 0; B[3 * index + 0, 3] = 0; B[3 * index + 0, 4] = -xyz.Z; B[3 * index + 0, 5] = xyz.Y; B[3 * index + 0, 6] = xyz.X;
B[3 * index + 1, 0] = 0; B[3 * index + 1, 1] = 1; B[3 * index + 1, 2] = 0; B[3 * index + 1, 3] = xyz.Z; B[3 * index + 1, 4] = 0; B[3 * index + 1, 5] = -xyz.X; B[3 * index + 1, 6] = xyz.Y;
B[3 * index + 2, 0] = 0; B[3 * index + 2, 1] = 0; B[3 * index + 2, 2] = 1; B[3 * index + 2, 3] = -xyz.Y; B[3 * index + 2, 4] = xyz.X; B[3 * index + 2, 5] = 0; B[3 * index + 2, 6] = xyz.Z;
h[3 * index + 0, 0] = xyz.X - AllPointsApproXyz[staName].X;
h[3 * index + 1, 0] = xyz.Y - AllPointsApproXyz[staName].Y;
h[3 * index + 2, 0] = xyz.Z - AllPointsApproXyz[staName].Z;
Ph[3 * index + 0, 3 * index + 0] = 10e10;
Ph[3 * index + 1, 3 * index + 1] = 10e10;
Ph[3 * index + 2, 3 * index + 2] = 10e10;
k++;
}
IMatrix tmp = (B.Transposition).Multiply(B); //7*7
IMatrix H = (B.Transposition); //tmp.GetInverse().Multiply(B.Transposition); //7*3n
// IMatrix H = tmp.GetInverse().Multiply(B.Transposition); //7*3n
IMatrix lb = H.Multiply(h); //7*1
IMatrix Nb = H.Transposition.Multiply(H); //3n*3n
IMatrix Ub = H.Transposition.Multiply(lb); //3n*1
this.ATPA = this.ATPA.Plus(Nb);
this.ATPL = this.ATPL.Plus(Ub);
}
else if (this.radioButton_Robust.Checked)
{
}
else if (this.radioButton_bayesRobust.Checked)
{
}
#endregion
// Geo.Algorithm.IMatrix QX = (N.GetInverse());
Geo.Algorithm.IMatrix QX = (ATPA).GetInverse();
Geo.Algorithm.IMatrix X = QX.Multiply(ATPL);
//Geo.Algorithm.IMatrix X = QX.Multiply(U);
//输出结果
#region
string savePath = "D:\\" + outName + ".txt";
FileInfo aFile = new FileInfo(savePath);
StreamWriter sw = aFile.CreateText();
System.Globalization.NumberFormatInfo GN = new System.Globalization.CultureInfo("zh-CN", false).NumberFormat;
GN.NumberDecimalDigits = 6;//小数点6位
StringBuilder sb = new StringBuilder();
sb.AppendLine("计算完毕！结果保存在：" + savePath);
sw.WriteLine("基准点坐标信息：");
foreach (var item in KnownPointName)
{
sw.Write(item); sw.Write("\t"); sw.Write(KonwnPointsXyz[item].ToSnxString()); sw.Write("\n");
}
sw.WriteLine("所有测站坐标的估计值：");
for (int i = 0; i < this.AllPointName.Count; i++)
{
string name = AllPointName[i];
sw.Write(name);
sw.Write("\t");
sw.Write((AllPointsApproXyz[name].X + X[3 * i + 0, 0]).ToString("0.0000", GN)); //("N", GN));//
sw.Write("\t");
sw.Write((AllPointsApproXyz[name].Y + X[3 * i + 1, 0]).ToString("0.0000", GN)); //("N", GN));//
sw.Write("\t");
sw.Write((AllPointsApproXyz[name].Z + X[3 * i + 2, 0]).ToString("0.0000", GN)); //("N", GN));//
sw.Write("\n");
}
for (int i = 0; i < QX.RowCount; i++)
{
for (int j = 0; j <= i; j++)
{
sw.Write(QX[i, j].ToString("N", GN));
sw.Write("\t");
}
sw.Write("\n");
}
sw.Close();
#endregion
this.textBox_info.Text = sb.ToString();
}
