/// <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;
}
