/// <summary> /// 光束法 /// </summary> /// <param name="ie">内方位元素初值</param> /// <param name="oes">外方位元素初值</param> /// <param name="dp">畸变系数初值</param> /// <returns></returns> public List <OData> LightMethod(InElement ie, List <OutElement> oes, DParams dp) { SetOriginValue(ie, oes, dp); BackForcus(); ForwardForcus(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, 6 * MCIList.Count), C = new Matrix(B.Row, 3 * AllOList.Count), l = new Matrix(B.Row, 1), P = Matrix.Eye(B.Row); #region 设置控制点的权值 for (int i = 0; i < MCOList.Count; ++i) { for (int j = 0; j < outE.Count; ++j) { P[2 * i + j * AllOList.Count * 2, 2 * i + j * AllOList.Count * 2] = 200; P[2 * i + 1 + j * AllOList.Count * 2, 2 * i + 1 + j * AllOList.Count * 2] = 200; } } #endregion double[] X = new double[6 * outE.Count]; 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; } if (PassOList.Count == 0) { return(null); } //int start = 6 * 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_light1(B, C, l); //var N11 = B.T() * B; //var N12 = B.T() * C; //var N21 = C.T() * B; //var arN22 = (C.T() * C).Inverse(); //var M1 = B.T() * l; //var M2 = C.T() * l; //x = (N11 - N12 * arN22 * N21).Inverse() * (M1 - N12 * arN22 * M2); var AB = Matrix.ColumnCombine(B, C); AB.OutPut("AB"); var y = (AB.T() * AB).Inverse() * AB.T() * l; x = y.SubRMatrix(0, outE.Count * 6 - 1); X0 = X0 + x; UpdateData_light(X0); ForwardForcus(PassOList); } while (!IsTerminating(0.000001, x)); UpdateData_light(X0); //System.Windows.Forms.MessageBox.Show(X0.ToString()); return(PassOList); }
/// <summary> /// 设置初值 /// </summary> /// <param name="ie">内方位元素初值</param> /// <param name="oeList">外方元素初值</param> /// <param name="dp">畸变参数</param> public void SetOriginValue(InElement ie, List <OutElement> oeList, DParams dp) { inE = ie; dParams = dp; outE = oeList; }
/// <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> /// <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> /// 光束法 /// </summary> /// <param name="ie">内方位元素初值</param> /// <param name="oes">外方位元素初值</param> /// <param name="dp">畸变系数初值</param> /// <returns></returns> public List<OData> LightMethod(InElement ie,List<OutElement> oes,DParams dp) { SetOriginValue(ie, oes, dp); BackForcus(); ForwardForcus(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, 6 * MCIList.Count), C = new Matrix(B.Row, 3 * AllOList.Count), l = new Matrix(B.Row, 1), P = Matrix.Eye(B.Row); #region 设置控制点的权值 for (int i = 0; i < MCOList.Count;++i ) { for (int j = 0; j < outE.Count;++j ) { P[2 * i + j * AllOList.Count*2, 2 * i + j * AllOList.Count*2] = 200; P[2 * i + 1 + j * AllOList.Count*2, 2 * i + 1 + j * AllOList.Count*2] = 200; } } #endregion double[] X = new double[6 * outE.Count]; 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; } if (PassOList.Count == 0) { return null; } //int start = 6 * 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_light1(B, C, l); //var N11 = B.T() * B; //var N12 = B.T() * C; //var N21 = C.T() * B; //var arN22 = (C.T() * C).Inverse(); //var M1 = B.T() * l; //var M2 = C.T() * l; //x = (N11 - N12 * arN22 * N21).Inverse() * (M1 - N12 * arN22 * M2); var AB = Matrix.ColumnCombine(B, C); AB.OutPut("AB"); var y = (AB.T() * AB).Inverse() * AB.T() * l; x = y.SubRMatrix(0, outE.Count * 6 - 1); X0 = X0 + x; UpdateData_light(X0); ForwardForcus(PassOList); } while (!IsTerminating(0.000001, x)); UpdateData_light(X0); //System.Windows.Forms.MessageBox.Show(X0.ToString()); return PassOList; }
/// <summary> /// 设置初值 /// </summary> /// <param name="ie">内方位元素初值</param> /// <param name="oeList">外方元素初值</param> /// <param name="dp">畸变参数</param> public void SetOriginValue(InElement ie, List<OutElement> oeList, DParams dp) { inE = ie; dParams = dp; outE = oeList; }