/// <summary> /// 通过新旧坐标表构造,至少两个点 /// </summary> /// <param name="pold">旧坐标表</param> /// <param name="pnew">新坐标表</param> /// <param name="mode">默认为0(迭代求解),其他时候(直接求解)</param> public CoordinateT(List<_2D_Point> pold,List<_2D_Point> pnew,int mode=0) { this.pold = pold; this.pnew = pnew; Matrix x; if(mode==0) { Matrix v=new Matrix(pnew.Count*2,1); do { Matrix B = GetB(), l = Getl(); x = (B.T() * B).Inverse() * B.T() * l; Update(x); v = v + B * x - l; } while (!Terminate(x)); xigema = Math.Sqrt((v.T() * v / (pnew.Count * 2 - 4))[0, 0]); } else { Matrix B = new Matrix(pnew.Count * 2, 4); Matrix l = new Matrix(pnew.Count * 2, 1); for (int i = 0; i < pnew.Count; i++) { B[2 * i, 0] = 1; B[2 * i, 1] = 0; B[2 * i, 2] = pold[i].X; B[2 * i, 3] = pold[i].Y; B[2 * i + 1, 0] = 0; B[2 * i + 1, 1] = 1; B[2 * i + 1, 2] = pold[i].Y; B[2 * i + 1, 3] = -pold[i].X; l[2*i, 0] = pnew[i].X; l[2 * i + 1,0] = pnew[i].Y; } x = (B.T() * B).Inverse() * B.T() * l; dx = x[0, 0]; dy = x[1, 0]; xita = Math.Atan(x[3, 0] / x[2, 0]); m = x[2, 0] / cos(xita) - 1; Matrix X=new Matrix(4,1); Matrix v = B * x - l; xigema = Math.Sqrt((v.T() * v / (pnew.Count * 2 - 4))[0, 0]); } }
/// <summary> /// 通过新旧坐标表构造,三个点以上 /// </summary> /// <param name="pold">旧坐标表</param> /// <param name="pnew">新坐标表</param> public CoordinateT3(List<_3D_Point> pold,List<_3D_Point> pnew) { this.pold = pold; this.pnew = pnew; Matrix x; Matrix v=new Matrix(pnew.Count*3,1); Matrix B = GetB(), l = Getl(); x = (B.T() * B).Inverse() * B.T() * l; Update(x); v = B * x - l; xigema = Math.Sqrt((v.T() * v / (pnew.Count * 3 - 4))[0, 0]); }
/// <summary> /// 严密的前方交会 /// </summary> public List<OData> ForwardForcus(List<OData> originOdList) { Matrix t, x; List<_3D_Point> Xx = new List<_3D_Point>(); //迭代求解加密点坐标(循环分块) do { Xx.Clear(); List<Matrix> Bs = new List<Matrix>(); List<Matrix> ls = new List<Matrix>(); for (int i = 0; i < outE.Count; i++) { Bs.Add(GetFB(outE[i], PassIList[i], originOdList)); ls.Add(GetFl(outE[i], PassIList[i], originOdList)); } Matrix B = new Matrix(Bs.Count * 2, 3); Matrix lm = new Matrix(Bs.Count * 2, 1); for (int m = 0; m < originOdList.Count; m++) { for (int i = 0; i < outE.Count; i++) { B[2 * i, 0] = Bs[i][2 * m, 0]; B[2 * i, 1] = Bs[i][2 * m, 1]; B[2 * i, 2] = Bs[i][2 * m, 2]; B[2 * i + 1, 0] = Bs[i][2 * m, 0]; B[2 * i + 1, 1] = Bs[i][2 * m + 1, 1]; B[2 * i + 1, 2] = Bs[i][2 * m + 1, 2]; lm[2 * i, 0] = ls[i][2 * m, 0]; lm[2 * i + 1, 0] = ls[i][2 * m + 1, 0]; } x = (B.T() * B).Inverse() * B.T() * lm; Xx.Add(new _3D_Point(x[0, 0], x[1, 0], x[2, 0])); } t = _3D_Point.ToColumnMatrix(Xx); for (int i = 0; i < originOdList.Count; i++) { originOdList[i].pos += Xx[i]; } } while (!IsTerminating(0.000001, t)); //StreamWriter sw = new StreamWriter("result.txt"); //List<double> rs = new List<double>(); //List<_3D_Point> ddxyz = new List<_3D_Point>(); //for (int i = 0; i < ps.Count; i++) //{ // _3D_Point dxyz = ps[i].pos - PassOList[i].pos; // ddxyz.Add(dxyz); // double r = _3D_Point.Get_Norm(dxyz, new _3D_Point()); // rs.Add(r); // sw.WriteLine(ps[i].Name + "," + ps[i].pos + "," + dxyz + "," + r); //} //double sum = 0; //rs.ForEach(r => sum += r * r); double a = Math.Sqrt(sum / rs.Count); //sw.WriteLine("dr=" + a + ",点位精度:" + a / 5000); //sw.Close(); PassOList = originOdList; return PassOList; }
/// <summary> /// 光束法 /// </summary> /// <param name="ie">内方位元素初值</param> /// <returns></returns> public List<OData> LightMethod(InElement ie) { BackForcus(ie); ForwardForcus(); AllOList = MCOList.Concat(PassOList).ToList(); AllIList =AllIList??new List<List<IData>>(); for (int i = 0; i < MCIList.Count;++i ) { AllIList.Add(MCIList[i].Concat(PassIList[i]).ToList()); } Matrix B = new Matrix(AllOList.Count * 2 * AllIList.Count, 7 * MCIList.Count + 3 * AllOList.Count), l = new Matrix(B.Row, 1), Bx = new Matrix(outE.Count, B.Column), w = new Matrix(Bx.Row, 1), P = Matrix.Eye(B.Column); #region 设置控制点的权值 for (int i = 0; i < MCOList.Count;++i ) { P[i, i] = 500; } #endregion double[] X = new double[7 * outE.Count+PassIList[0].Count*3]; for (int i = 0, j = 0; i < outE.Count; i++, j = j + 7) { X[j] = outE[i].Spos.X; X[j + 1] = outE[i].Spos.Y; X[j + 2] = outE[i].Spos.Z; X[j + 3] = outE[i].q0; X[j + 4] = outE[i].q1; X[j + 5] = outE[i].q2; X[j + 6] = outE[i].q3; } if (PassOList.Count == 0) { PassIList[0].ForEach(pi => PassOList.Add(new OData(pi.Name, new _3D_Point()))); } int start = 7 * outE.Count; for (int i = 0; i < PassIList[0].Count; ++i) { PassOList[i].pos.X = X[start + 3 * i]; PassOList[i].pos.Y = X[start + 3 * i + 1]; PassOList[i].pos.Z = X[start + 3 * i + 2]; } Matrix X0 = new Matrix(X.GetLength(0), 1, X); Matrix x; do { SetBl_light(B, l, Bx, w); var N1 = Matrix.ColumnCombine(B.T() * B, Bx.T()); var N2 = Matrix.ColumnCombine(Bx, Matrix.Zeros(Bx.Row, Bx.Row)); var N = Matrix.RowCombine(N1, N2); var W = Matrix.RowCombine(B.T() * l, w); var Y = N.Inverse() * W; x = Y.SubRMatrix(0, B.Column - 1); X0 = X0 + x; UpdateData_light(X0); } while (!IsTerminating(0.000001, x)); UpdateData_light(X0); return PassOList; }
/// <summary> /// 带有初值的后方交会 /// </summary> private void BackForcus0() { Matrix B = new Matrix(MCOList.Count * 2 * MCIList.Count, 9 + 7 * MCIList.Count), l = new Matrix(B.Row, 1), Bx = new Matrix(outE.Count, B.Column), w = new Matrix(Bx.Row, 1); int start = 7 * outE.Count; double[] X = new double[9 + 7 * outE.Count]; for (int i = 0, j = 0; i < outE.Count; i++, j = j + 7) { X[j] = outE[i].Spos.X; X[j + 1] = outE[i].Spos.Y; X[j + 2] = outE[i].Spos.Z; X[j + 3] = outE[i].q0; X[j + 4] = outE[i].q1; X[j + 5] = outE[i].q2; X[j + 6] = outE[i].q3; } X[start] = inE.p0.X; X[start + 1] = inE.p0.Y; X[start + 2] = inE.f; X[start + 3] = dParams.k1; X[start + 4] = dParams.k2; X[start + 5] = dParams.p1; X[start + 6] = dParams.p2; X[start + 7] = dParams.alph; X[start + 8] = dParams.beta; Matrix X0 = new Matrix(X.GetLength(0), 1, X); Matrix x; do { SetBl0(B, l, Bx, w); //B.OutPut("B"); //l.OutPut("l"); //Bx.OutPut("Bx"); //w.OutPut("w"); var N1 = Matrix.ColumnCombine(B.T() * B, Bx.T()); var N2 = Matrix.ColumnCombine(Bx, Matrix.Zeros(Bx.Row, Bx.Row)); var N = Matrix.RowCombine(N1, N2); var W = Matrix.RowCombine(B.T() * l, w); var Y = N.Inverse() * W; x = Y.SubRMatrix(0, B.Column - 1); X0 = X0 + x; UpdateData0(X0); } while (!IsTerminating(0.000001, x)); UpdateData0(X0); }
/// <summary> /// 已知内方位元素、畸变参数的后方交会(可求出外方位元素) /// 附有限制条件的间接平差 /// </summary> /// <param name="ie">内方位元素初值</param> /// <param name="dp">畸变参数</param> public void BackForcus(InElement ie, DParams dp) { for (int i = 0; i < MCIList.Count; ++i) { outE.Add(new OutElement4 { q0 = 1 }); } inE = ie; dParams = dp; Matrix B = new Matrix(MCOList.Count * 2 * MCIList.Count, 7 * MCIList.Count), l = new Matrix(B.Row, 1), Bx = new Matrix(outE.Count, B.Column), w = new Matrix(Bx.Row, 1); double[] X = new double[7 * outE.Count]; for (int i = 0, j = 0; i < outE.Count; i++, j = j + 7) { X[j] = outE[i].Spos.X; X[j + 1] = outE[i].Spos.Y; X[j + 2] = outE[i].Spos.Z; X[j + 3] = outE[i].q0; X[j + 4] = outE[i].q1; X[j + 5] = outE[i].q2; X[j + 6] = outE[i].q3; } Matrix X0 = new Matrix(X.GetLength(0), 1, X); Matrix x; do { SetBl(B, l, Bx, w); var N1 = Matrix.ColumnCombine(B.T() * B, Bx.T()); var N2 = Matrix.ColumnCombine(Bx, Matrix.Zeros(Bx.Row, Bx.Row)); var N = Matrix.RowCombine(N1, N2); var W = Matrix.RowCombine(B.T() * l, w); var Y = N.Inverse() * W; x = Y.SubRMatrix(0, B.Column - 1); X0 = X0 + x; UpdateData(X0); } while (!IsTerminating(0.000001, x)); UpdateData(X0); }
/// <summary> /// 已知内方位元素、外方位元素、畸变参数初值的后方交会,应先调用SetOriginValue /// </summary> public void BackForcus() { Matrix B = new Matrix(MCOList.Count * 2 * MCIList.Count, 6 * MCIList.Count+9), l = new Matrix(B.Row, 1); double[] X = new double[6 * outE.Count+9]; for (int i = 0, j = 0; i < outE.Count; i++, j = j + 6) { X[j] = outE[i].Spos.X; X[j + 1] = outE[i].Spos.Y; X[j + 2] = outE[i].Spos.Z; X[j + 3] = outE[i].phi; X[j + 4] = outE[i].omega; X[j + 5] = outE[i].kappa; } int start = outE.Count * 6; X[start] = inE.p0.X; X[start + 1] = inE.p0.Y; X[start + 2] = inE.f; X[start + 3] = dParams.k1; X[start + 4] = dParams.k2; X[start + 5] = dParams.p1; X[start + 6] = dParams.p2; X[start + 7] = dParams.alph; X[start + 8] = dParams.beta; Matrix X0 = new Matrix(X.GetLength(0), 1, X); Matrix x; do { SetBl(B, l); x = (B.T() * B).Inverse() * B.T() * l; X0 = X0 + x; UpdateData(X0); } while (!IsTerminating(0.000001, x)); UpdateData(X0); }
public void TTest1() { double[,] data = null; // TODO: инициализация подходящего значения Matrix target = new Matrix(data); // TODO: инициализация подходящего значения target.T(); Assert.Inconclusive("Невозможно проверить метод, не возвращающий значение."); }
//DLT算法 public void Adjustment() { int count=data.oCount; Matrix B=new Matrix(count * 2, 11); Matrix l=new Matrix(count * 2, 1); //求L系数初值 for (int j = 0; j < data.Count;j++) { for (int i = 0; i < data.oCount;i++) { double x = data.oIPoints[j][i].X, y = data.oIPoints[j][i].Y, X = data.oPoints[i].X, Y = data.oPoints[i].Y, Z = data.oPoints[i].Z; B[2 * i,0] = X; B[2 * i,1] = Y; B[2 * i,2] = Z; B[2 * i,3] = 1; B[2 * i,8] = -x*X; B[2 * i,9] = -x*Y; B[2 * i,10] = -x*Z; B[2 * i + 1,4] = X; B[2 * i + 1,5] = Y; B[2 * i + 1,6] = Z; B[2 * i + 1,7] = 1; B[2 * i + 1,8] = -y*X; B[2 * i + 1,9] = -y*Y; B[2 * i + 1,10] = -y*Z; l[2 * i,0] = x; l[2 * i + 1,0] = y; } int xCount = 12;//未知数个数 Matrix L0 = ((B.T()*B).Inverse()*(B.T()*l)); Matrix L=new Matrix(xCount, 1); for (int i = 0; i < xCount-1; i++) L[i,0] = L0[i,0]; Matrix M=new Matrix(count * 2, xCount); Matrix W=new Matrix(count * 2, 1); double f = 9999; //迭代求解L系数 while (abs(f - inE[j].fx) >= 0.01) { f = inE[j].fx; double x0 = (L[0,0] * L[8,0] + L[1,0] * L[9,0] + L[2,0] * L[10,0]) / (pow2(L[8,0]) + pow2(L[9,0]) + pow2(L[10,0])), y0 = (L[4,0] * L[8,0] + L[5,0] * L[9,0] + L[6,0] * L[10,0]) / (pow2(L[8,0]) + pow2(L[9,0]) + pow2(L[10,0])); for (int i = 0; i < data.oCount; i++) { double x = data.oIPoints[j][i].X, y = data.oIPoints[j][i].Y, X = data.oPoints[i].X, Y = data.oPoints[i].Y, Z = data.oPoints[i].Z; double A = X*L[8,0] +Y*L[9,0] +Z*L[10,0] + 1; double r_2 = (x - x0)*(x - x0) + (y - y0)*(y - y0); M[2 * i,0] = X / A; M[2 * i,1] = Y / A; M[2 * i,2] = Z / A; M[2 * i,3] = 1 / A; M[2 * i,8] = -X*x / A; M[2 * i,9] = -Y*x / A; M[2 * i,10] = -Z*x / A; M[2 * i,11] = -(x - x0)*r_2; M[2 * i + 1,4] = X / A; M[2 * i + 1,5] = Y / A; M[2 * i + 1,6] = Z / A; M[2 * i + 1,7] = 1 / A; M[2 * i + 1,8] = -X*y / A; M[2 * i + 1,9] = -Y*y / A; M[2 * i + 1,10] = -Z*y / A; M[2 * i + 1,11] = -(y - y0)*r_2; W[2 * i,0] = x / A; W[2 * i + 1,0] = y / A; } Matrix nL = (M.T()*M).Inverse()*M.T()*W; double dbeta, ds, fx, fy,Xs,Ys,Zs,a3,b3,c3,b1,b2; double gama3 = 1 / sqrt(pow2(nL[8,0]) + pow2(nL[9,0]) + pow2(nL[10,0])); x0 = (nL[0,0] * nL[8,0] + nL[1,0] * nL[9,0] + nL[2,0] * nL[10,0]) / (pow2(nL[8,0]) + pow2(nL[9,0]) + pow2(nL[10,0])); y0 = (nL[4,0] * nL[8,0] + nL[5,0] * nL[9,0] + nL[6,0] * nL[10,0]) / (pow2(nL[8,0]) + pow2(nL[9,0]) + pow2(nL[10,0])); double At = gama3*gama3*(pow2(nL[0,0]) + pow2(nL[1,0]) + pow2(nL[2,0])) - x0*x0, Bt = gama3*gama3*(pow2(nL[4,0]) + pow2(nL[5,0]) + pow2(nL[6,0])) - y0*y0, Ct = gama3*gama3*(nL[0,0] * nL[4,0] + nL[1,0] * nL[5,0] + nL[2,0] * nL[6,0]) - x0*y0; if (Ct >= 0) { dbeta = -asin(sqrt(Ct*Ct / At / Bt)); } else { dbeta = asin(sqrt(Ct*Ct / At / Bt)); } ds = sqrt(At / Bt) - 1; fx = sqrt((At*Bt - Ct*Ct) / Bt); fy = sqrt((At*Bt - Ct*Ct) / At); Matrix t=new Matrix(3, 3),b=new Matrix(3,1); t[0,0] = L[0,0]; t[0,1] = L[1,0]; t[0,2] = L[2,0]; t[1,0] = L[4,0]; t[1,1] = L[5,0]; t[1,2] = L[6,0]; t[2,0] = L[8,0]; t[2,1] = L[9,0]; t[2,2] = L[10,0]; b[0,0] = L[3,0]; b[1,0] = L[7,0]; b[2,0] = -1; Matrix xyz = t.Inverse()*b; Xs = xyz[0,0]; Ys = xyz[1,0]; Zs = xyz[2,0]; a3 = gama3*L[8,0]; b3 = gama3*L[9,0]; c3 = gama3*L[10,0]; b2 = (-gama3*L[5,0] + b3*y0)*(1 + ds)*cos(dbeta) / fx; b1 = (-gama3*L[1,0] + b2*fx*tan(dbeta) + b3*x0) / fx; outE[j].ps.X=Xs; outE[j].ps.Y=Ys; outE[j].ps.Z=Zs; outE[j].phi = atan(-a3 / c3); outE[j].omege = asin(-b3); outE[j].kappa = atan(b1 / b2); inE[j].p0.X=x0; inE[j].p0.Y=y0; inE[j].fx = fx; inE[j].fy = fy; other[j].dbeta = dbeta; other[j].ds = ds; other[j].k1 = nL[xCount - 1,0]; L = nL; } Ls.Add(L); } //像点改正 for (int i = 0; i < data.Count;i++) { for (int j = 0; j < data.pCount;j++) { double x = data.iPoints[i][j].X, y = data.iPoints[i][j].Y, x0 = inE[i].p0.X, y0 = inE[i].p0.Y, k1 = other[i].k1, r_2=(x-x0)*(x-x0)+(y-y0)*(y-y0); data.iPoints[i][j].X=(x+k1*(x-x0)*r_2); data.iPoints[i][j].Y=(y + k1*(y - y0)*r_2); } } //前方交会 for (int i = 0; i < data.pCount;i++) { Matrix Bc=new Matrix(2 * data.Count, 3); Matrix lc=new Matrix(2 * data.Count, 1); for (int j = 0; j < data.Count;j++) { double x = data.iPoints[j][i].X, y = data.iPoints[j][i].Y; Bc[2 * j,0] = Ls[j][0,0] - x*Ls[j][8,0]; Bc[2 * j,1] = Ls[j][1,0] - x*Ls[j][9,0]; Bc[2 * j,2] = Ls[j][2,0] - x*Ls[j][10,0]; Bc[2 * j + 1,0] = Ls[j][4,0] - y*Ls[j][8,0]; Bc[2 * j + 1,1] = Ls[j][5,0] - y*Ls[j][9,0]; Bc[2 * j + 1,2] = Ls[j][6,0] - y*Ls[j][10,0]; lc[2 * j,0] = Ls[j][3,0] - x; lc[2 * j + 1,0] = Ls[j][7,0] - y; } Matrix t = -(Bc.T()*Bc).Inverse()*Bc.T()*lc; data.allPoints.Add(new _3D_Point(t[0,0], t[1,0], t[2,0])); } }