public JZ Multiply(JZ other) //矩阵乘法
{ // 检查行列数是否符合要求
if (numColumns != other.GetNumRows())
{
throw new Exception("矩阵的行/列数不匹配。");
}
JZ result = new JZ(numRows, other.GetNumColumns());
double value;
for (int i = 0; i < result.GetNumRows(); ++i)
{
for (int j = 0; j < other.GetNumColumns(); ++j)
{
value = 0.0;
for (int k = 0; k < numColumns; ++k)
{
value += GetElement(i, k) * other.GetElement(k, j);
}
result.SetElement(i, j, value);
}
}
return(result);
}
public JZ Multiply(double val) //数乘矩阵
{ // 构造目标矩阵
JZ result = new JZ(this); // copy ourselves
// 进行数乘
for (int i = 0; i < numRows; ++i)
{
for (int j = 0; j < numColumns; ++j)
{
result.SetElement(i, j, GetElement(i, j) * val);
}
}
return(result);
}
public JZ Transpose() //转制
{ // 构造目标矩阵
JZ Trans = new JZ(numColumns, numRows);
// 转置各元素
for (int i = 0; i < numRows; ++i)
{
for (int j = 0; j < numColumns; ++j)
{
Trans.SetElement(j, i, GetElement(i, j));
}
}
return(Trans);
}
public JZ Subtract(JZ other)//矩阵减法
{
if (numColumns != other.GetNumColumns() || numRows != other.GetNumRows())
{
throw new Exception("矩阵的行/列数不匹配。");
}
// 构造结果矩阵
JZ result = new JZ(this);
// 进行减法
for (int i = 0; i < numRows; ++i)
{
for (int j = 0; j < numColumns; ++j)
{
result.SetElement(i, j, GetElement(i, j) - other.GetElement(i, j));
}
}
return(result);
}
public JZ Add(JZ other) //矩阵加法
{ // 检查行列数是否相等
if (numColumns != other.GetNumColumns() || numRows != other.GetNumRows())
{
throw new Exception("矩阵的行/列数不匹配。");
}
// 构造结果矩阵
JZ result = new JZ(this);
// 进行加法
for (int i = 0; i < numRows; ++i)
{
for (int j = 0; j < numColumns; ++j)
{
result.SetElement(i, j, GetElement(i, j) + other.GetElement(i, j));
}
}
return(result);
}
private void button2_Click(object sender, EventArgs e)
{
double f = double.Parse(array[8][0]) * 0.001;//摄影机主距
JZ xyf1 = new JZ(3, 6);
JZ xyf2 = new JZ(3, 6);
for (int i = 0; i < 6; i++)
{
xyf1[0, i] = double.Parse(array[i + 1][1]) / 1000;
xyf1[1, i] = double.Parse(array[i + 1][2]) / 1000;
xyf1[2, i] = -f;
xyf2[0, i] = double.Parse(array[i + 1][3]) / 1000;
xyf2[1, i] = double.Parse(array[i + 1][4]) / 1000;
xyf2[2, i] = -f;
}//导入像平面坐标
double t0 = 0, m0 = 0, v0 = 0, f0 = 0, w0 = 0, k0 = 0, by = 0, bz = 0;//定义最初值
JZ R1 = new JZ(3, 3);
JZ R2 = new JZ(3, 3);
//R1
R1[0, 0] = Math.Cos(t0) * Math.Cos(v0) - Math.Sin(t0) * Math.Sin(m0) * Math.Sin(v0);
R1[0, 1] = -Math.Cos(t0) * Math.Sin(v0) - Math.Sin(t0) * Math.Sin(m0) * Math.Cos(v0);
R1[0, 2] = -Math.Sin(t0) * Math.Cos(m0);
R1[1, 0] = Math.Cos(m0) * Math.Sin(v0);
R1[1, 1] = Math.Cos(m0) * Math.Cos(v0);
R1[1, 2] = -Math.Sin(m0);
R1[2, 0] = Math.Sin(t0) * Math.Cos(v0) + Math.Cos(t0) * Math.Sin(m0) * Math.Sin(v0);
R1[2, 1] = -Math.Sin(t0) * Math.Sin(v0) + Math.Cos(t0) * Math.Sin(m0) * Math.Cos(v0);
R1[2, 2] = Math.Cos(t0) * Math.Cos(m0);//计算旋转矩阵各元素值
//R2
R2[0, 0] = Math.Cos(f0) * Math.Cos(k0) - Math.Sin(f0) * Math.Sin(w0) * Math.Sin(k0);
R2[0, 1] = -Math.Cos(f0) * Math.Sin(k0) - Math.Sin(f0) * Math.Sin(w0) * Math.Cos(k0);
R2[0, 2] = -Math.Sin(f0) * Math.Cos(w0);
R2[1, 0] = Math.Cos(w0) * Math.Sin(k0);
R2[1, 1] = Math.Cos(w0) * Math.Cos(k0);
R2[1, 2] = -Math.Sin(w0);
R2[2, 0] = Math.Sin(f0) * Math.Cos(k0) + Math.Cos(f0) * Math.Sin(w0) * Math.Sin(k0);
R2[2, 1] = -Math.Sin(f0) * Math.Sin(k0) + Math.Cos(f0) * Math.Sin(w0) * Math.Cos(k0);
R2[2, 2] = Math.Cos(f0) * Math.Cos(w0);//计算旋转矩阵各元素值
JZ XYZ1 = new JZ(3, 6);
JZ XYZ2 = new JZ(3, 6);
XYZ1 = R1.Multiply(xyf1);
XYZ2 = R2.Multiply(xyf2);
double bx = xyf1[0, 0] - xyf2[0, 0];
JZ N = new JZ(2, 6);
JZ X = new JZ(5, 1);
JZ Q = new JZ(6, 1);
JZ A = new JZ(6, 5);
do
{
R2[0, 0] = Math.Cos(f0) * Math.Cos(k0) - Math.Sin(f0) * Math.Sin(w0) * Math.Sin(k0);
R2[0, 1] = -Math.Cos(f0) * Math.Sin(k0) - Math.Sin(f0) * Math.Sin(w0) * Math.Cos(k0);
R2[0, 2] = -Math.Sin(f0) * Math.Cos(w0);
R2[1, 0] = Math.Cos(w0) * Math.Sin(k0);
R2[1, 1] = Math.Cos(w0) * Math.Cos(k0);
R2[1, 2] = -Math.Sin(w0);
R2[2, 0] = Math.Sin(f0) * Math.Cos(k0) + Math.Cos(f0) * Math.Sin(w0) * Math.Sin(k0);
R2[2, 1] = -Math.Sin(f0) * Math.Sin(k0) + Math.Cos(f0) * Math.Sin(w0) * Math.Cos(k0);
R2[2, 2] = Math.Cos(f0) * Math.Cos(w0);//计算旋转矩阵各元素值
XYZ2 = R2.Multiply(xyf2);
by = bx * m0;
bz = bx * v0;
for (int i = 0; i < 6; i++)
{
//计算点投影系数
N[0, i] = (bx * XYZ2[2, i] - bz * XYZ2[0, i]) / (XYZ1[0, i] * XYZ2[2, i] - XYZ2[0, i] * XYZ1[2, i]);
N[1, i] = (bx * XYZ1[2, i] - bz * XYZ1[0, i]) / (XYZ1[0, i] * XYZ2[2, i] - XYZ2[0, i] * XYZ1[2, i]);
Q[i, 0] = N[0, i] * XYZ1[1, i] - N[1, i] * XYZ2[1, i] - by;
A[i, 0] = bx;
A[i, 1] = -bx * XYZ2[1, i] / XYZ2[2, i];
A[i, 2] = -N[1, i] * XYZ2[0, i] * XYZ2[1, i] / XYZ2[2, i];
A[i, 3] = -N[1, i] * (XYZ2[2, i] + XYZ2[1, i] * XYZ2[1, i] / XYZ2[2, i]);
A[i, 4] = XYZ2[0, i] * N[1, i];
}
JZ AT = new JZ();
JZ ATL = new JZ();
JZ ATA = new JZ();
JZ ATAn = new JZ();
JZ ATAnAT = new JZ();
AT = A.Transpose();
ATA = AT.Multiply(A);
ATAn = ATA.InvertGaussJordan();
ATAnAT = ATAn.Multiply(AT);
X = ATAnAT.Multiply(Q);
m0 = m0 + X[0, 0];
v0 = v0 + X[1, 0];
f0 = f0 + X[2, 0];
w0 = w0 + X[3, 0];
k0 = k0 + X[4, 0];
} while (Math.Abs(X[0, 0]) >= 0.00003 || Math.Abs(X[1, 0]) >= 0.00003 || Math.Abs(X[2, 0]) >= 0.00003 || Math.Abs(X[3, 0]) >= 0.00003 || Math.Abs(X[4, 0]) >= 0.00003);
double m;//求中误差
double[] mk = new double[5]; double l;
JZ T = new JZ(); T = (A.Multiply(X) - Q).Transpose().Multiply(A.Multiply(X) - Q);
l = T[0, 0];
m = Math.Sqrt(l / (6 - 5));
JZ P = new JZ(6, 6);
P = A.Transpose().Multiply(A).InvertGaussJordan();
for (int t = 0; t < 5; t++)
{
mk[t] = (Math.Sqrt(P[t, t])) * m;
}
//textBox2.Text += "m: " + m0.ToString() + "\r\n";
//textBox2.Text += "v: " + v0.ToString() + "\r\n";
//textBox2.Text += "f: " + f0.ToString() + "\r\n";
//textBox2.Text += "w: " + w0.ToString() + "\r\n";
//textBox2.Text += "k: " + k0.ToString() + "\r\n";
//textBox2.Text += "m的精度: " + mk[0].ToString() + "\r\n";
//textBox2.Text += "v的精度: " + mk[1].ToString() + "\r\n";
//textBox2.Text += "f的精度: " + mk[2].ToString() + "\r\n";
//textBox2.Text += "w的精度: " + mk[3].ToString() + "\r\n";
//textBox2.Text += "k的精度: " + mk[4].ToString() + "\r\n";
JZ XYZm = new JZ(3, 6);//计算模型点坐标
for (int i = 0; i < 6; i++)
{
XYZm[0, i] = N[0, i] * XYZ1[0, i];
XYZm[1, i] = 0.5 * (N[0, i] * XYZ1[1, i] + N[1, i] * XYZ2[1, i] + bx * m0);
XYZm[2, i] = N[0, i] * XYZ1[2, i];
}
int mm = 37000; //像片比例尺分母
JZ XYZp = new JZ(3, 6); //计算模型点的摄影测量坐标
for (int i = 0; i < 6; i++)
{
XYZp[0, i] = mm * N[0, i] * XYZ1[0, i];
XYZp[1, i] = 0.5 * (N[0, i] * XYZ1[1, i] + N[1, i] * XYZ2[1, i] + bx * m0) * mm;
XYZp[2, i] = mm * f + mm * N[0, i] * XYZ1[2, i];
}
JZ XYZtp = new JZ(3, 6);//计算各点的地面摄影测量坐标
JZ R = new JZ(3, 3);
JZ dXYZ = new JZ(3, 6);
double f00 = 0.0527; double w00 = 0.1426; double k00 = 0.2478;
double numda = 1.00156;
R[0, 0] = Math.Cos(f00) * Math.Cos(k00) - Math.Sin(f00) * Math.Sin(w00) * Math.Sin(k00);
R[0, 1] = -Math.Cos(f00) * Math.Sin(k00) - Math.Sin(f00) * Math.Sin(w00) * Math.Cos(k00);
R[0, 2] = -Math.Sin(f00) * Math.Cos(w00);
R[1, 0] = Math.Cos(w00) * Math.Sin(k00);
R[1, 1] = Math.Cos(w00) * Math.Cos(k00);
R[1, 2] = -Math.Sin(w00);
R[2, 0] = Math.Sin(f00) * Math.Cos(k00) + Math.Cos(f00) * Math.Sin(w00) * Math.Sin(k00);
R[2, 1] = -Math.Sin(f00) * Math.Sin(k00) + Math.Cos(f00) * Math.Sin(w00) * Math.Cos(k00);
R[2, 2] = Math.Cos(f00) * Math.Cos(w00);
for (int i = 0; i < 6; i++)
{
dXYZ[0, i] = 6385.067;
dXYZ[1, i] = 1954.325;
dXYZ[2, i] = 724.215;
}
XYZtp = R.Multiply(numda).Multiply(XYZp).Add(dXYZ);
textBox2.Text += "模型点坐标: " + "\r\n";
for (int i = 0; i < 6; i++)
{
textBox2.Text += "(" + XYZm[0, i].ToString() + "," + XYZm[1, i].ToString() + "," + XYZm[2, i].ToString() + ")" + "\r\n";
}
textBox2.Text += "模型点的摄影测量坐标: " + "\r\n";
for (int i = 0; i < 6; i++)
{
textBox2.Text += "(" + XYZp[0, i].ToString() + "," + XYZp[1, i].ToString() + "," + XYZp[2, i].ToString() + ")" + "\r\n";
}
textBox2.Text += "各点的地面摄影测量坐标: " + "\r\n";
for (int i = 0; i < 6; i++)
{
textBox2.Text += "(" + XYZtp[0, i].ToString() + "," + XYZtp[1, i].ToString() + "," + XYZtp[2, i].ToString() + ")" + "\r\n";
}
}
public JZ(JZ other)
{
numColumns = other.GetNumColumns();
numRows = other.GetNumRows();
Init(numRows, numColumns);
}
public JZ InvertGaussJordan() //矩阵的求逆
{
int i, j, k, l, u, v;
double d = 0.0, p = 0.0;
int[] pnRow = new int[numColumns];
int[] pnCol = new int[numColumns];
{
for (k = 0; k <= numColumns - 1; k++)
{
d = 0.0;
for (i = k; i <= numColumns - 1; i++)
{
for (j = k; j <= numColumns - 1; j++)
{
l = i * numColumns + j; p = Math.Abs(elements[l]);
if (p > d)
{
d = p;
pnRow[k] = i;
pnCol[k] = j;
}
}
}
if (d == 0.0)
{
throw new Exception("矩阵不可逆。");
}
if (pnRow[k] != k)
{
for (j = 0; j <= numColumns - 1; j++)
{
u = k * numColumns + j;
v = pnRow[k] * numColumns + j;
p = elements[u];
elements[u] = elements[v];
elements[v] = p;
}
}
if (pnCol[k] != k)
{
for (i = 0; i <= numColumns - 1; i++)
{
u = i * numColumns + k;
v = i * numColumns + pnCol[k];
p = elements[u];
elements[u] = elements[v];
elements[v] = p;
}
}
l = k * numColumns + k;
elements[l] = 1.0 / elements[l];
for (j = 0; j <= numColumns - 1; j++)
{
if (j != k)
{
u = k * numColumns + j;
elements[u] = elements[u] * elements[l];
}
}
for (i = 0; i <= numColumns - 1; i++)
{
if (i != k)
{
for (j = 0; j <= numColumns - 1; j++)
{
if (j != k)
{
u = i * numColumns + j;
elements[u] = elements[u] - elements[i * numColumns + k] * elements[k * numColumns + j];
}
}
}
}
for (i = 0; i <= numColumns - 1; i++)
{
if (i != k)
{
u = i * numColumns + k;
elements[u] = -elements[u] * elements[l];
}
}
}
for (k = numColumns - 1; k >= 0; k--)
{
if (pnCol[k] != k)
{
for (j = 0; j <= numColumns - 1; j++)
{
u = k * numColumns + j;
v = pnCol[k] * numColumns + j;
p = elements[u];
elements[u] = elements[v];
elements[v] = p;
}
}
if (pnRow[k] != k)
{
for (i = 0; i <= numColumns - 1; i++)
{
u = i * numColumns + k;
v = i * numColumns + pnRow[k];
p = elements[u];
elements[u] = elements[v];
elements[v] = p;
}
}
}
}
JZ result = new JZ(numRows, numColumns);
for (int a = 0; a < numRows; ++a)
{
for (int b = 0; b < numColumns; ++b)
{
result.SetElement(a, b, GetElement(a, b));
}
}
return(result);
}
