//计算外方位元素方法,量测相机解算,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); }