/// <summary> /// 获取严密前方一张相片的系数矩阵 /// </summary> /// <param name="oe"></param> /// <param name="id"></param> /// <param name="od"></param> /// <returns></returns> private Matrix GetFB(OutElement oe, List <ImageData> id, List <ObjectData> od) { Matrix B = new Matrix(2 * od.Count, 3); double f = inE.f, x0 = inE.p0.X, y0 = inE.p0.Y; Matrix R = oe.GetR(); double a1 = R[0, 0], a2 = R[0, 1], a3 = R[0, 2], b1 = R[1, 0], b2 = R[1, 1], b3 = R[1, 2], c1 = R[2, 0], c2 = R[2, 1], c3 = R[2, 2]; int j = 0; for (int i = 0; i < od.Count; i++) { double Xr = a1 * (od[i].pos.X - oe.Spos.X) + b1 * (od[i].pos.Y - oe.Spos.Y) + c1 * (od[i].pos.Z - oe.Spos.Z), Yr = a2 * (od[i].pos.X - oe.Spos.X) + b2 * (od[i].pos.Y - oe.Spos.Y) + c2 * (od[i].pos.Z - oe.Spos.Z), Zr = a3 * (od[i].pos.X - oe.Spos.X) + b3 * (od[i].pos.Y - oe.Spos.Y) + c3 * (od[i].pos.Z - oe.Spos.Z), x = id[i].pos.X - x0 - Get_dx(id[i]), y = id[i].pos.Y - y0 - Get_dy(id[i]); B[j, 0] = -(a1 * f + a3 * x) / Zr; B[j, 1] = -(b1 * f + b3 * x) / Zr; B[j, 2] = -(c1 * f + c3 * x) / Zr; B[j + 1, 0] = -(a2 * f + a3 * y) / Zr; B[j + 1, 1] = -(b2 * f + b3 * y) / Zr; B[j + 1, 2] = -(c2 * f + c3 * y) / Zr; j = j + 2; } return(B); }
private Matrix GetR(OutElement outElement) { DMath sin = Math.Sin, cos = Math.Cos; double[,] R = new double[3, 3]; R[0, 0] = cos(outElement.phi) * cos(outElement.kappa) - sin(outElement.phi) * sin(outElement.omega) * sin(outElement.kappa); R[0, 1] = -cos(outElement.phi) * sin(outElement.kappa) - sin(outElement.phi) * sin(outElement.omega) * cos(outElement.kappa); R[0, 2] = -sin(outElement.phi) * cos(outElement.omega); R[1, 0] = cos(outElement.omega) * sin(outElement.kappa); R[1, 1] = cos(outElement.omega) * cos(outElement.kappa); R[1, 2] = -sin(outElement.omega); R[2, 0] = sin(outElement.phi) * cos(outElement.kappa) + cos(outElement.phi) * sin(outElement.omega) * sin(outElement.kappa); R[2, 1] = -sin(outElement.phi) * sin(outElement.kappa) + cos(outElement.phi) * sin(outElement.omega) * cos(outElement.kappa); R[2, 2] = cos(outElement.phi) * cos(outElement.omega); return(new Matrix(R, 3, 3)); }
/// <summary> /// 计算前方交会时的lxy /// </summary> /// <param name="oe">外方元素</param> /// <param name="od">物方坐标</param> /// <param name="id">像方坐标</param> /// <returns>l矩阵</returns> private Matrix GetFl(OutElement oe, List <ImageData> id, List <ObjectData> od) { double[] l = new double[od.Count * 2]; int i = 0, k = 0; Matrix R = oe.GetR(); double a1 = R[0, 0], a2 = R[0, 1], a3 = R[0, 2], b1 = R[1, 0], b2 = R[1, 1], b3 = R[1, 2], c1 = R[2, 0], c2 = R[2, 1], c3 = R[2, 2], Xs = oe.Spos.X, Ys = oe.Spos.Y, Zs = oe.Spos.Z; foreach (var item in od) { double x = id[k].pos.X - Get_dx(id[k]) - inE.p0.X, y = id[k].pos.Y - Get_dy(id[k]) - inE.p0.Y; l[i] = x + inE.f * (a1 * (item.pos.X - Xs) + b1 * (item.pos.Y - Ys) + c1 * (item.pos.Z - Zs)) / (a3 * (item.pos.X - Xs) + b3 * (item.pos.Y - Ys) + c3 * (item.pos.Z - Zs)); l[i + 1] = y + inE.f * (a2 * (item.pos.X - Xs) + b2 * (item.pos.Y - Ys) + c2 * (item.pos.Z - Zs)) / (a3 * (item.pos.X - Xs) + b3 * (item.pos.Y - Ys) + c3 * (item.pos.Z - Zs)); i = i + 2; k++; } return(new Matrix(l.GetLength(0), 1, l)); }
/// <summary> /// 计算前方交会时的lxy /// </summary> /// <param name="oe">外方元素</param> /// <param name="od">物方坐标</param> /// <param name="id">像方坐标</param> /// <returns>l矩阵</returns> private Matrix GetFl(OutElement oe, List<ImageData> id, List<ObjectData> od) { double[] l = new double[od.Count * 2]; int i = 0, k = 0; Matrix R =oe.GetR(); double a1 = R[0, 0], a2 = R[0, 1], a3 = R[0, 2], b1 = R[1, 0], b2 = R[1, 1], b3 = R[1, 2], c1 = R[2, 0], c2 = R[2, 1], c3 = R[2, 2], Xs = oe.Spos.X, Ys = oe.Spos.Y, Zs = oe.Spos.Z; foreach (var item in od) { double x = id[k].pos.X - Get_dx(id[k]) - inE.p0.X, y = id[k].pos.Y - Get_dy(id[k]) - inE.p0.Y; l[i] = x + inE.f * (a1 * (item.pos.X - Xs) + b1 * (item.pos.Y - Ys) + c1 * (item.pos.Z - Zs)) / (a3 * (item.pos.X - Xs) + b3 * (item.pos.Y - Ys) + c3 * (item.pos.Z - Zs)); l[i + 1] = y + inE.f * (a2 * (item.pos.X - Xs) + b2 * (item.pos.Y - Ys) + c2 * (item.pos.Z - Zs)) / (a3 * (item.pos.X - Xs) + b3 * (item.pos.Y - Ys) + c3 * (item.pos.Z - Zs)); i = i + 2; k++; } return new Matrix(l.GetLength(0), 1, l); }
/// <summary> /// 获取严密前方一张相片的系数矩阵 /// </summary> /// <param name="oe"></param> /// <param name="id"></param> /// <param name="od"></param> /// <returns></returns> private Matrix GetFB(OutElement oe, List<ImageData> id, List<ObjectData> od) { Matrix B = new Matrix(2 * od.Count, 3); double f = inE.f, x0 = inE.p0.X, y0 = inE.p0.Y; Matrix R = oe.GetR(); double a1 = R[0, 0], a2 = R[0, 1], a3 = R[0, 2], b1 = R[1, 0], b2 = R[1, 1], b3 = R[1, 2], c1 = R[2, 0], c2 = R[2, 1], c3 = R[2, 2]; int j = 0; for (int i = 0; i < od.Count; i++) { double Xr = a1 * (od[i].pos.X - oe.Spos.X) + b1 * (od[i].pos.Y - oe.Spos.Y) + c1 * (od[i].pos.Z - oe.Spos.Z), Yr = a2 * (od[i].pos.X - oe.Spos.X) + b2 * (od[i].pos.Y - oe.Spos.Y) + c2 * (od[i].pos.Z - oe.Spos.Z), Zr = a3 * (od[i].pos.X - oe.Spos.X) + b3 * (od[i].pos.Y - oe.Spos.Y) + c3 * (od[i].pos.Z - oe.Spos.Z), x = id[i].pos.X - x0 - Get_dx(id[i]), y = id[i].pos.Y - y0 - Get_dy(id[i]); B[j, 0] = -(a1 * f + a3 * x) / Zr; B[j, 1] = -(b1 * f + b3 * x) / Zr; B[j, 2] = -(c1 * f + c3 * x) / Zr; B[j + 1, 0] = -(a2 * f + a3 * y) / Zr; B[j + 1, 1] = -(b2 * f + b3 * y) / Zr; B[j + 1, 2] = -(c2 * f + c3 * y) / Zr; j = j + 2; } return B; }
private Matrix GetR(OutElement outElement) { DMath sin = Math.Sin, cos = Math.Cos; double[,] R = new double[3, 3]; R[0, 0] = cos(outElement.phi) * cos(outElement.kappa) - sin(outElement.phi) * sin(outElement.omega) * sin(outElement.kappa); R[0, 1] = -cos(outElement.phi) * sin(outElement.kappa) - sin(outElement.phi) * sin(outElement.omega) * cos(outElement.kappa); R[0, 2] = -sin(outElement.phi) * cos(outElement.omega); R[1, 0] = cos(outElement.omega) * sin(outElement.kappa); R[1, 1] = cos(outElement.omega) * cos(outElement.kappa); R[1, 2] = -sin(outElement.omega); R[2, 0] = sin(outElement.phi) * cos(outElement.kappa) + cos(outElement.phi) * sin(outElement.omega) * sin(outElement.kappa); R[2, 1] = -sin(outElement.phi) * sin(outElement.kappa) + cos(outElement.phi) * sin(outElement.omega) * cos(outElement.kappa); R[2, 2] = cos(outElement.phi) * cos(outElement.omega); return new Matrix(R, 3, 3); }