//单点精确计算
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]);
}
//计算外方位元素方法，量测相机解算，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]);
}
