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