//单点精确计算 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]); }