//计算外方位元素方法，量测相机解算，6参数
public void ImageCal6(double ee, CPoint[] p)
{
//读取
int i, j, k;
int num = p.GetLength(0);
double a1, a2, a3, b1, b2, b3, c1, c2, c3; //R矩阵
double xh, yh, zh; //像空间坐标
fiu = 0; omg = 0; kaf = 0; //外方位元素
double sx = 0, sy = 0, sz = 0; //求和
for (i = 0; i < p.GetLength(0); i++)
{
sx = sx + p[i].xt; sy = sy + p[i].yt; sz = sz + p[i].zt;
}
xs = sx / p.GetLength(0); ys = sy / p.GetLength(0); zs = sz / p.GetLength(0) + m * f / 1000; //设置初始值
double dmax = 1;
//迭代
CMat mat = new CMat();
double[,] b = null;
double[] x = null;
double[] l = null;
do
{
a1 = Math.Cos(fiu) * Math.Cos(kaf) - Math.Sin(fiu) * Math.Sin(omg) * Math.Sin(kaf);
a2 = -Math.Cos(fiu) * Math.Sin(kaf) - Math.Sin(fiu) * Math.Sin(omg) * Math.Cos(kaf);
a3 = -Math.Sin(fiu) * Math.Cos(omg);
b1 = Math.Cos(omg) * Math.Sin(kaf);
b2 = Math.Cos(omg) * Math.Cos(kaf);
b3 = -Math.Sin(omg);
c1 = Math.Sin(fiu) * Math.Cos(kaf) + Math.Cos(fiu) * Math.Sin(omg) * Math.Sin(kaf);
c2 = -Math.Sin(fiu) * Math.Sin(kaf) + Math.Cos(fiu) * Math.Sin(omg) * Math.Cos(kaf);
c3 = Math.Cos(fiu) * Math.Cos(omg);
CMat mat1 = new CMat();
x = mat1.MatZero(6);
double[,] n = mat1.MatZero(6, 6);
double[] w = mat1.MatZero(6);
for (k = 0; k < p.GetLength(0); k++)
{
b = mat1.MatZero(2, 6);
l = mat1.MatZero(2);
double[,] n1 = mat1.MatZero(6, 6);
double[] w1 = mat1.MatZero(6);
xh = a1 * (p[k].xt - xs) + b1 * (p[k].yt - ys) + c1 * (p[k].zt - zs);
yh = a2 * (p[k].xt - xs) + b2 * (p[k].yt - ys) + c2 * (p[k].zt - zs);
zh = a3 * (p[k].xt - xs) + b3 * (p[k].yt - ys) + c3 * (p[k].zt - zs);
double xp0 = -f * (xh / zh);
double yp0 = -f * (yh / zh);
l[0] = p[k].xp - x0 - xp0;
l[1] = p[k].yp - y0 - yp0;
b[0, 0] = (a1 * f + a3 * (p[k].xp - x0)) / zh;
b[0, 1] = (b1 * f + b3 * (p[k].xp - x0)) / zh;
b[0, 2] = (c1 * f + c3 * (p[k].xp - x0)) / zh;
b[1, 0] = (a2 * f + a3 * (p[k].yp - y0)) / zh;
b[1, 1] = (b2 * f + b3 * (p[k].yp - y0)) / zh;
b[1, 2] = (c2 * f + c3 * (p[k].yp - y0)) / zh;
b[0, 3] = (p[k].yp - y0) * Math.Sin(omg) - ((p[k].xp - x0) / f * ((p[k].xp - x0) * Math.Cos(kaf) - (p[k].yp - y0) * Math.Sin(kaf)) + f * Math.Cos(kaf)) * Math.Cos(omg);
b[0, 4] = -f *Math.Sin(kaf) - (p[k].xp - x0) / f * ((p[k].xp - x0) * Math.Sin(kaf) + (p[k].yp - y0) * Math.Cos(kaf));
b[0, 5] = (p[k].yp - y0);
b[1, 3] = -(p[k].xp - x0) * Math.Sin(omg) - ((p[k].yp - y0) / f * ((p[k].xp - x0) * Math.Cos(kaf) - (p[k].yp - y0) * Math.Sin(kaf)) - f * Math.Sin(kaf)) * Math.Cos(omg);
b[1, 4] = -f *Math.Cos(kaf) - (p[k].yp - y0) / f * ((p[k].xp - x0) * Math.Sin(kaf) + (p[k].yp - y0) * Math.Cos(kaf));
b[1, 5] = -(p[k].xp - x0);
n1 = mat1.MatMulti(mat1.MatTrans(b), b);
w1 = mat1.MatMulti(mat1.MatTrans(b), l);
n = mat1.MatAddTo(n1, n, 0, 0);
w = mat1.MatAddTo(w1, w, 0);
}
double[,] q = mat1.MatInver(n);
x = mat1.MatMulti(q, w);
dmax = mat1.MatMax(x);
xs = xs + x[0]; ys = ys + x[1]; zs = zs + x[2];
fiu = fiu + x[3]; omg = omg + x[4]; kaf = kaf + x[5];
} while (dmax >= ee);
////精度评定
double[] V = mat.MatZero(2);
double[,] Qii = mat.MatZero(6, 6);
V = mat.MatMulti(b, x);
double VTV = V[0] * V[0] + V[1] * V[1];
m0 = Math.Sqrt(VTV / (2 * p.GetLength(0) - 2));
Qii = mat.MatMulti(mat.MatTrans(b), b);
mxs = m0 * Math.Sqrt(Qii[0, 0]);
mys = m0 * Math.Sqrt(Qii[1, 1]);
mzs = m0 * Math.Sqrt(Qii[2, 2]);
mfiu = m0 * Math.Sqrt(Qii[3, 3]);
momg = m0 * Math.Sqrt(Qii[4, 4]);
mkaf = m0 * Math.Sqrt(Qii[5, 5]);
}
//单点精确计算
public void cal1(CImage image1, CImage image2, CPoints cps)
{
CMat mat = new CMat();
//定义平差计算变量
int i, j;
double[,] b = mat.MatZero(4, 3);
double[] l = mat.MatZero(4);
double[,] n = mat.MatZero(3, 3);
double[] w = mat.MatZero(3);
double[,] q = mat.MatZero(3, 3);
double[] x = mat.MatZero(3);
double a1, a2, a3, b1, b2, b3, c1, c2, c3;
double d1, d2, d3, e1, e2, e3, f1, f2, f3;
double xh, yh, zh;
double xp0, yp0;
//左像点计算
a1 = Math.Cos(image1.fiu) * Math.Cos(image1.kaf) - Math.Sin(image1.fiu) * Math.Sin(image1.omg) * Math.Sin(image1.kaf);
a2 = -Math.Cos(image1.fiu) * Math.Sin(image1.kaf) - Math.Sin(image1.fiu) * Math.Sin(image1.omg) * Math.Cos(image1.kaf);
a3 = -Math.Sin(image1.fiu) * Math.Cos(image1.omg);
b1 = Math.Cos(image1.omg) * Math.Sin(image1.kaf);
b2 = Math.Cos(image1.omg) * Math.Cos(image1.kaf);
b3 = -Math.Sin(image1.omg);
c1 = Math.Sin(image1.fiu) * Math.Cos(image1.kaf) + Math.Cos(image1.fiu) * Math.Sin(image1.omg) * Math.Sin(image1.kaf);
c2 = -Math.Sin(image1.fiu) * Math.Sin(image1.kaf) + Math.Cos(image1.fiu) * Math.Sin(image1.omg) * Math.Cos(image1.kaf);
c3 = Math.Cos(image1.fiu) * Math.Cos(image1.omg);
xh = a1 * (cps.xt - image1.xs) + b1 * (cps.yt - image1.ys) + c1 * (cps.zt - image1.zs);
yh = a2 * (cps.xt - image1.xs) + b2 * (cps.yt - image1.ys) + c2 * (cps.zt - image1.zs);
zh = a3 * (cps.xt - image1.xs) + b3 * (cps.yt - image1.ys) + c3 * (cps.zt - image1.zs);
xp0 = -image1.f * (xh / zh);
yp0 = -image1.f * (yh / zh);
l[0] = cps.x1 - image1.x0 - xp0;
l[1] = cps.y1 - image1.y0 - yp0;
b[0, 0] = -(a1 * image1.f + a3 * (cps.x1 - image1.x0)) / zh;
b[0, 1] = -(b1 * image1.f + b3 * (cps.x1 - image1.x0)) / zh;
b[0, 2] = -(c1 * image1.f + c3 * (cps.x1 - image1.x0)) / zh;
b[1, 0] = -(a2 * image1.f + a3 * (cps.y1 - image1.y0)) / zh;
b[1, 1] = -(b2 * image1.f + b3 * (cps.y1 - image1.y0)) / zh;
b[1, 2] = -(c2 * image1.f + c3 * (cps.y1 - image1.y0)) / zh;
//右像点计算
d1 = Math.Cos(image2.fiu) * Math.Cos(image2.kaf) - Math.Sin(image2.fiu) * Math.Sin(image2.omg) * Math.Sin(image2.kaf);
d2 = -Math.Cos(image2.fiu) * Math.Sin(image2.kaf) - Math.Sin(image2.fiu) * Math.Sin(image2.omg) * Math.Cos(image2.kaf);
d3 = -Math.Sin(image2.fiu) * Math.Cos(image2.omg);
e1 = Math.Cos(image2.omg) * Math.Sin(image2.kaf);
e2 = Math.Cos(image2.omg) * Math.Cos(image2.kaf);
e3 = -Math.Sin(image2.omg);
f1 = Math.Sin(image2.fiu) * Math.Cos(image2.kaf) + Math.Cos(image2.fiu) * Math.Sin(image2.omg) * Math.Sin(image2.kaf);
f2 = -Math.Sin(image2.fiu) * Math.Sin(image2.kaf) + Math.Cos(image2.fiu) * Math.Sin(image2.omg) * Math.Cos(image2.kaf);
f3 = Math.Cos(image2.fiu) * Math.Cos(image2.omg);
xh = d1 * (cps.xt - image2.xs) + e1 * (cps.yt - image2.ys) + f1 * (cps.zt - image2.zs);
yh = d2 * (cps.xt - image2.xs) + e2 * (cps.yt - image2.ys) + f2 * (cps.zt - image2.zs);
zh = d3 * (cps.xt - image2.xs) + e3 * (cps.yt - image2.ys) + f3 * (cps.zt - image2.zs);
xp0 = -image2.f * (xh / zh);
yp0 = -image2.f * (yh / zh);
l[2] = cps.x2 - image2.x0 - xp0;
l[3] = cps.y2 - image2.y0 - yp0;
b[2, 0] = -(d1 * image2.f + d3 * (cps.x2 - image2.x0)) / zh;
b[2, 1] = -(e1 * image2.f + e3 * (cps.x2 - image2.x0)) / zh;
b[2, 2] = -(f1 * image2.f + f3 * (cps.x2 - image2.x0)) / zh;
b[3, 0] = -(d2 * image2.f + d3 * (cps.y2 - image2.y0)) / zh;
b[3, 1] = -(e2 * image2.f + e3 * (cps.y2 - image2.y0)) / zh;
b[3, 2] = -(f2 * image2.f + f3 * (cps.y2 - image2.y0)) / zh;
//建立法方程
n = mat.MatMulti(mat.MatTrans(b), b);
w = mat.MatMulti(mat.MatTrans(b), l);
// //求逆
q = mat.MatInver(n);
//求未知数
x = mat.MatMulti(q, w);
cps.xt = cps.xt + x[0];
cps.yt = cps.yt + x[1];
cps.zt = cps.zt + x[2];
//误差
double[] v = new double[4];
double vv, m0;
for (i = 0; i <= 3; i++)
{
v[i] = -l[i];
}
vv = 0;
double[] bx = mat.MatMulti(b, x);
for (i = 0; i <= 3; i++)
{
for (j = 0; j <= 2; j++)
{
v[i] = v[i] + bx[i];
}
}
for (i = 0; i <= 3; i++)
{
vv = vv + v[i] * v[i];
}
//
m0 = Math.Sqrt(vv);
cps.mx = m0 * Math.Sqrt(q[0, 0]);
cps.my = m0 * Math.Sqrt(q[1, 1]);
cps.mz = m0 * Math.Sqrt(q[2, 2]);
}
//连续像对法
private void button_cal3_Click(object sender, EventArgs e)
{
//
int num = list.Count;
CMat mat = new CMat();
points = new CPoints[num];
string line;
string[] splits;
int[] id = new int[num];
double[] xp1 = new double[num];
double[] yp1 = new double[num];
double[] xp2 = new double[num];
double[] yp2 = new double[num];
double[] xm = new double[num];
double[] ym = new double[num];
double[] zm = new double[num];
int i, j, k;
for (i = 0; i <= num - 1; i++)
{
line = Convert.ToString(list[i]);
splits = line.Split(',');
id[i] = Convert.ToInt16(splits[0]);
xp1[i] = Convert.ToDouble(splits[1]);
yp1[i] = Convert.ToDouble(splits[2]);
xp2[i] = Convert.ToDouble(splits[3]);
yp2[i] = Convert.ToDouble(splits[4]);
}
xs1 = ys1 = zs1 = fiu1 = omg1 = kaf1 = 0;
a1 = Math.Cos(fiu1) * Math.Cos(kaf1) - Math.Sin(fiu1) * Math.Sin(omg1) * Math.Sin(kaf1);
a2 = -Math.Cos(fiu1) * Math.Sin(kaf1) - Math.Sin(fiu1) * Math.Sin(omg1) * Math.Cos(kaf1);
a3 = -Math.Sin(fiu1) * Math.Cos(omg1);
b1 = Math.Cos(omg1) * Math.Sin(kaf1);
b2 = Math.Cos(omg1) * Math.Cos(kaf1);
b3 = -Math.Sin(omg1);
c1 = Math.Sin(fiu1) * Math.Cos(kaf1) + Math.Cos(fiu1) * Math.Sin(omg1) * Math.Sin(kaf1);
c2 = -Math.Sin(fiu1) * Math.Sin(kaf1) + Math.Cos(fiu1) * Math.Sin(omg1) * Math.Cos(kaf1);
c3 = Math.Cos(fiu1) * Math.Cos(omg1);
xs2 = bu;
ys2 = zs2 = fiu2 = omg2 = kaf2 = 0;
u = v = 0;
double[] b = new double[5];
double l;
double[,] n = new double[5, 5];
double[] w = new double[5];
double[,] q = new double[5, 5];
double[] x = new double[5];
double x1, y1, x2, y2;
loop1:
for (i = 0; i <= 4; i++) //法方程式系数和常数项置零
{
for (j = 0; j <= 4; j++)
{
n[i, j] = 0;
}
w[i] = 0;
x[i] = 0;
}
d1 = Math.Cos(fiu2) * Math.Cos(kaf2) - Math.Sin(fiu2) * Math.Sin(omg2) * Math.Sin(kaf2);
d2 = -Math.Cos(fiu2) * Math.Sin(kaf2) - Math.Sin(fiu2) * Math.Sin(omg2) * Math.Cos(kaf2);
d3 = -Math.Sin(fiu2) * Math.Cos(omg2);
e1 = Math.Cos(omg2) * Math.Sin(kaf2);
e2 = Math.Cos(omg2) * Math.Cos(kaf2);
e3 = -Math.Sin(omg2);
f1 = Math.Sin(fiu2) * Math.Cos(kaf2) + Math.Cos(fiu2) * Math.Sin(omg2) * Math.Sin(kaf2);
f2 = -Math.Sin(fiu2) * Math.Sin(kaf2) + Math.Cos(fiu2) * Math.Sin(omg2) * Math.Cos(kaf2);
f3 = Math.Cos(fiu2) * Math.Cos(omg2);
bv = bu * u;
bw = bu * v;
// 逐点建立误差法方程式
for (k = 0; k <= 5; k++)
{
for (i = 0; i <= 4; i++) //各点的误差方程式系数和常数项置零
{
b[i] = 0;
}
l = 0;
x1 = xp1[k];
y1 = yp1[k];
x2 = xp2[k];
y2 = yp2[k];
u1 = a1 * (x1 - x0) + a2 * (y1 - y0) - a3 * f;
v1 = b1 * (x1 - x0) + b2 * (y1 - y0) - b3 * f;
w1 = c1 * (x1 - x0) + c2 * (y1 - y0) - c3 * f;
u2 = d1 * (x2 - x0) + d2 * (y2 - y0) - d3 * f;
v2 = e1 * (x2 - x0) + e2 * (y2 - y0) - e3 * f;
w2 = f1 * (x2 - x0) + f2 * (y2 - y0) - f3 * f;
n1 = (bu * w2 - bw * u2) / (u1 * w2 - u2 * w1);
n2 = (bu * w1 - bw * u1) / (u1 * w2 - u2 * w1);
b[0] = bu;
b[1] = -v2 / w2 * bu;
b[2] = -u2 * v2 / w2 * n2;
b[3] = -(w2 + v2 * v2 / w2) * n2;
b[4] = u2 * n2;
l = n1 * v1 - n2 * v2 - bv;
//逐点建立法方程式
for (i = 0; i <= 4; i++)
{
for (j = 0; j <= 4; j++)
{
n[i, j] = n[i, j] + b[i] * b[j];
}
}
for (i = 0; i <= 4; i++)
{
w[i] = w[i] + b[i] * l;
}
}
// //求逆
q = inv(5, n);
//求未知数
for (i = 0; i <= 4; i++)
{
for (j = 0; j <= 4; j++)
{
x[i] = x[i] + q[i, j] * w[j];
}
}
u = u + x[0];
v = v + x[1];
fiu2 = fiu2 + x[2];
omg2 = omg2 + x[3];
kaf2 = kaf2 + x[4];
double max = Math.Abs(x[0]);
for (i = 1; i <= 4; i++)
{
if (Math.Abs(x[i]) >= max)
{
max = Math.Abs(x[i]);
}
}
if (max >= Convert.ToDouble(0.000001))
{
goto loop1;
}
//模型点坐标计算
d1 = Math.Cos(fiu2) * Math.Cos(kaf2) - Math.Sin(fiu2) * Math.Sin(omg2) * Math.Sin(kaf2);
d2 = -Math.Cos(fiu2) * Math.Sin(kaf2) - Math.Sin(fiu2) * Math.Sin(omg2) * Math.Cos(kaf2);
d3 = -Math.Sin(fiu2) * Math.Cos(omg2);
e1 = Math.Cos(omg2) * Math.Sin(kaf2);
e2 = Math.Cos(omg2) * Math.Cos(kaf2);
e3 = -Math.Sin(omg2);
f1 = Math.Sin(fiu2) * Math.Cos(kaf2) + Math.Cos(fiu2) * Math.Sin(omg2) * Math.Sin(kaf2);
f2 = -Math.Sin(fiu2) * Math.Sin(kaf2) + Math.Cos(fiu2) * Math.Sin(omg2) * Math.Cos(kaf2);
f3 = Math.Cos(fiu2) * Math.Cos(omg2);
bv = bu * u;
bw = bu * v;
xs2 = xs1 + bu;
ys2 = ys1 + bv;
zs2 = zs1 + bw;
for (k = 0; k <= num - 1; k++)
{
x1 = xp1[k];
y1 = yp1[k];
x2 = xp2[k];
y2 = yp2[k];
u1 = a1 * (x1 - x0) + a2 * (y1 - y0) - a3 * f;
v1 = b1 * (x1 - x0) + b2 * (y1 - y0) - b3 * f;
w1 = c1 * (x1 - x0) + c2 * (y1 - y0) - c3 * f;
u2 = d1 * (x2 - x0) + d2 * (y2 - y0) - d3 * f;
v2 = e1 * (x2 - x0) + e2 * (y2 - y0) - e3 * f;
w2 = f1 * (x2 - x0) + f2 * (y2 - y0) - f3 * f;
n1 = (bu * w2 - bw * u2) / (u1 * w2 - u2 * w1);
n2 = (bu * w1 - bw * u1) / (u1 * w2 - u2 * w1);
xm[k] = xs1 + n1 * u1;
ym[k] = ((ys1 + n1 * v1) + (ys2 + n2 * v2)) / 2;
zm[k] = zs1 + n1 * w1;
xm[k] = xm[k] * m / 1000;
ym[k] = ym[k] * m / 1000;
zm[k] = zm[k] * m / 1000;
}
bu = bu * m / 1000;
bv = bv * m / 1000;
bw = bw * m / 1000;
xs2 = xs2 * m / 1000;
ys2 = ys2 * m / 1000;
zs2 = zs2 * m / 1000;
string path = Application.StartupPath + "\\连续像对法相对定向模型点坐标文件.txt";
StreamWriter sw = File.CreateText(path);
sw.WriteLine(xs1.ToString() + "," + ys1.ToString() + "," + zs1.ToString() + "," + fiu1.ToString() + "," + omg1.ToString() + "," + kaf1.ToString());
sw.WriteLine(xs2.ToString() + "," + ys2.ToString() + "," + zs2.ToString() + "," + fiu2.ToString() + "," + omg2.ToString() + "," + kaf2.ToString());
for (i = 0; i <= num - 1; i++)
{
sw.WriteLine(id[i] + "," + xm[i].ToString() + "," + ym[i].ToString() + "," + zm[i].ToString());
}
sw.Close();
MessageBox.Show("连续像对法相对定向计算结束，数据已保存！", "数据保存进程提示", MessageBoxButtons.OK);
}
