Exemplo n.º 1
0
            /// <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;
            }
Exemplo n.º 2
0
            /// <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;
            }
Exemplo n.º 3
0
            /// <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);
            }
Exemplo n.º 4
0
 /// <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;
 }