public static Matrix operator +(Matrix A, Matrix B)
{
return(MatrixComputation.Add(A.elements, B.elements));
}
internal static void CalcMain(List <ResectionPoint> PhotoPoint, List <ResectionPoint> GroundPoint, double m, double f, double Limit, int ReTimeLimit, ref double[,] Result, ref int ReTime, out double[,] RM, out double[,] Q)
{
ResectionPoint CenterPoint = new ResectionPoint(); //摄影中心点
//逐点计算Xs、Ys、Zs
foreach (ResectionPoint Point in GroundPoint)
{
Result[0, 0] += Point.X;
Result[1, 0] += Point.Y;
Result[2, 0] += Point.Z;
}
Result[0, 0] /= GroundPoint.Count;
Result[1, 0] /= GroundPoint.Count;
Result[2, 0] = m * f + Result[2, 0] / GroundPoint.Count;
//根据点的个数创建矩阵
double[,] A = new double[2 * PhotoPoint.Count, 6]; //A矩阵，每个点2*6
double[,] L = new double[2 * PhotoPoint.Count, 1]; //L矩阵，每个点2*1
double[,] RotationAngles = new double[3, 1]; //角元素矩阵，3*1
double[,] X = { { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, { 0 } }; //改正数矩阵，6*1
do
{
//检查迭代次数
if (ReTime > ReTimeLimit)
{
throw new Exception("迭代次数超出限制！");
}
//每次迭代开始对本次计算的外方位元素重新赋值
CenterPoint.X = Result[0, 0];
CenterPoint.Y = Result[1, 0];
CenterPoint.Z = Result[2, 0];
Array.Copy(Result, 3, RotationAngles, 0, 3);
//通过三个角元素建立旋转矩阵
RM = GetRotationMatrix(RotationAngles);
//逐点计算并构造A，L
int CurrentRow = 0; //当前行计数器
for (int i = 0; i < PhotoPoint.Count; i++)
{
ResectionPoint AvgPoint = GetAvg(GroundPoint[i], CenterPoint, RM); //合并重复项，简化变量
//逐点获取A、L
double[,] ATmp = GetA(AvgPoint, RotationAngles, RM, f);
double[,] LTmp = GetL(PhotoPoint[i], AvgPoint, f);
//拷贝暂存数组
Array.Copy(ATmp, 0, A, A.GetLength(1) * CurrentRow, ATmp.GetLength(0) * ATmp.GetLength(1));
Array.Copy(LTmp, 0, L, L.GetLength(1) * CurrentRow, LTmp.GetLength(0) * LTmp.GetLength(1));
CurrentRow += 2; //单个点赋值完毕，当前行数+2
}
X = GetX(A, L); //计算改正量
Result = MatrixComputation.Add(Result, X); //外方位元素矩阵加改正量
ReTime++; //迭代次数+1
} while (Math.Abs(X[3, 0]) > Limit || Math.Abs(X[4, 0]) > Limit || Math.Abs(X[5, 0]) > Limit); //当改正量大于阈值时继续迭代
Q = GetQ(A, X, L, GroundPoint.Count); //计算完毕后的误差分析
}
