/// <summary> /// 转换为四元数形式??? /// </summary> /// <returns></returns> public OutElement4 ToOutElem4() { OutElement4 oe = new OutElement4(); oe.Spos = Spos.Clone(); double cosp = Math.Cos(GetRad(phi) / 2), sinp = Math.Sin(GetRad(phi) / 2), coso = Math.Cos(GetRad(omega) / 2), sino = Math.Sin(GetRad(omega) / 2), cosk = Math.Cos(GetRad(kappa) / 2), sink = Math.Sin(GetRad(kappa) / 2); oe.q2 = sinp * coso * cosk + cosp * sino * sink; oe.q1 = cosp * sino * cosk - sinp * coso * sink; oe.q3 = cosp * coso * sink + sinp * sino * cosk; oe.q0 = cosp * coso * cosk - sinp * sino * sink; return(oe); }
/// <summary> /// 转换为四元数形式??? /// </summary> /// <returns></returns> public OutElement4 ToOutElem4() { OutElement4 oe = new OutElement4(); oe.Spos = Spos.Clone(); double cosp = Math.Cos(GetRad(phi) / 2), sinp = Math.Sin(GetRad(phi) / 2), coso = Math.Cos(GetRad(omega) / 2), sino = Math.Sin(GetRad(omega) / 2), cosk = Math.Cos(GetRad(kappa) / 2), sink = Math.Sin(GetRad(kappa) / 2); oe.q2 = sinp * coso * cosk + cosp * sino * sink; oe.q1 = cosp * sino * cosk - sinp * coso * sink; oe.q3= cosp * coso * sink + sinp * sino * cosk; oe.q0 = cosp * coso * cosk - sinp * sino * sink; return oe; }
/// <summary> /// 获取严密前方一张相片的系数矩阵 /// </summary> /// <param name="oe"></param> /// <param name="id"></param> /// <param name="od"></param> /// <returns></returns> private Matrix GetFB(OutElement4 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 = GetR(oe); 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; }
/// <summary> /// 计算前方交会时的lxy /// </summary> /// <param name="oe">外方元素</param> /// <param name="od">物方坐标</param> /// <param name="id">像方坐标</param> /// <returns>l矩阵</returns> private Matrix GetFl(OutElement4 oe, List<ImageData> id, List<ObjectData> od) { double[] l = new double[od.Count * 2]; int i = 0, k = 0; Matrix R = GetR(oe); 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); }
private Matrix GetR(OutElement4 oe) { double q0 = oe.q0, q1 = oe.q1, q2 = oe.q2, q3 = oe.q3; Matrix R = new Matrix(3, 3); R[0, 0] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3; R[0, 1] = 2 * (q1 * q2 - q0 * q3); R[0, 2] = 2 * (q1 * q3 + q0 * q2); R[1, 0] = 2 * (q1 * q2 + q0 * q3); R[1, 1] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3; R[1, 2] = 2 * (q2 * q3 - q0 * q1); R[2, 0] = 2 * (q1 * q3 - q0 * q2); R[2, 1] = 2 * (q2 * q3 + q0 * q1); R[2, 2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; return R; }