示例#1
0
文件: JZ.cs 项目: linbin-25/cucunku2
        public JZ Multiply(JZ other) //矩阵乘法
        {                            // 检查行列数是否符合要求
            if (numColumns != other.GetNumRows())
            {
                throw new Exception("矩阵的行/列数不匹配。");
            }

            JZ     result = new JZ(numRows, other.GetNumColumns());
            double value;

            for (int i = 0; i < result.GetNumRows(); ++i)
            {
                for (int j = 0; j < other.GetNumColumns(); ++j)
                {
                    value = 0.0;
                    for (int k = 0; k < numColumns; ++k)
                    {
                        value += GetElement(i, k) * other.GetElement(k, j);
                    }

                    result.SetElement(i, j, value);
                }
            }

            return(result);
        }
示例#2
0
文件: JZ.cs 项目: linbin-25/cucunku2
        public JZ Multiply(double val) //数乘矩阵
        {                              // 构造目标矩阵
            JZ result = new JZ(this);  // copy ourselves

            // 进行数乘
            for (int i = 0; i < numRows; ++i)
            {
                for (int j = 0; j < numColumns; ++j)
                {
                    result.SetElement(i, j, GetElement(i, j) * val);
                }
            }
            return(result);
        }
示例#3
0
文件: JZ.cs 项目: linbin-25/cucunku2
        public JZ Transpose() //转制
        {                     // 构造目标矩阵
            JZ Trans = new JZ(numColumns, numRows);

            // 转置各元素
            for (int i = 0; i < numRows; ++i)
            {
                for (int j = 0; j < numColumns; ++j)
                {
                    Trans.SetElement(j, i, GetElement(i, j));
                }
            }

            return(Trans);
        }
示例#4
0
文件: JZ.cs 项目: linbin-25/cucunku2
        public JZ Subtract(JZ other)//矩阵减法
        {
            if (numColumns != other.GetNumColumns() || numRows != other.GetNumRows())
            {
                throw new Exception("矩阵的行/列数不匹配。");
            }
            // 构造结果矩阵
            JZ result = new JZ(this);

            // 进行减法
            for (int i = 0; i < numRows; ++i)
            {
                for (int j = 0; j < numColumns; ++j)
                {
                    result.SetElement(i, j, GetElement(i, j) - other.GetElement(i, j));
                }
            }
            return(result);
        }
示例#5
0
文件: JZ.cs 项目: linbin-25/cucunku2
        public JZ Add(JZ other) //矩阵加法
        {                       // 检查行列数是否相等
            if (numColumns != other.GetNumColumns() || numRows != other.GetNumRows())
            {
                throw new Exception("矩阵的行/列数不匹配。");
            }
            // 构造结果矩阵
            JZ result = new JZ(this);

            // 进行加法
            for (int i = 0; i < numRows; ++i)
            {
                for (int j = 0; j < numColumns; ++j)
                {
                    result.SetElement(i, j, GetElement(i, j) + other.GetElement(i, j));
                }
            }
            return(result);
        }
示例#6
0
        private void button2_Click(object sender, EventArgs e)
        {
            double f = double.Parse(array[8][0]) * 0.001;//摄影机主距

            JZ xyf1 = new JZ(3, 6);
            JZ xyf2 = new JZ(3, 6);

            for (int i = 0; i < 6; i++)
            {
                xyf1[0, i] = double.Parse(array[i + 1][1]) / 1000;
                xyf1[1, i] = double.Parse(array[i + 1][2]) / 1000;
                xyf1[2, i] = -f;
                xyf2[0, i] = double.Parse(array[i + 1][3]) / 1000;
                xyf2[1, i] = double.Parse(array[i + 1][4]) / 1000;
                xyf2[2, i] = -f;
            }//导入像平面坐标

            double t0 = 0, m0 = 0, v0 = 0, f0 = 0, w0 = 0, k0 = 0, by = 0, bz = 0;//定义最初值

            JZ R1 = new JZ(3, 3);
            JZ R2 = new JZ(3, 3);

            //R1
            R1[0, 0] = Math.Cos(t0) * Math.Cos(v0) - Math.Sin(t0) * Math.Sin(m0) * Math.Sin(v0);
            R1[0, 1] = -Math.Cos(t0) * Math.Sin(v0) - Math.Sin(t0) * Math.Sin(m0) * Math.Cos(v0);
            R1[0, 2] = -Math.Sin(t0) * Math.Cos(m0);
            R1[1, 0] = Math.Cos(m0) * Math.Sin(v0);
            R1[1, 1] = Math.Cos(m0) * Math.Cos(v0);
            R1[1, 2] = -Math.Sin(m0);
            R1[2, 0] = Math.Sin(t0) * Math.Cos(v0) + Math.Cos(t0) * Math.Sin(m0) * Math.Sin(v0);
            R1[2, 1] = -Math.Sin(t0) * Math.Sin(v0) + Math.Cos(t0) * Math.Sin(m0) * Math.Cos(v0);
            R1[2, 2] = Math.Cos(t0) * Math.Cos(m0);//计算旋转矩阵各元素值

            //R2
            R2[0, 0] = Math.Cos(f0) * Math.Cos(k0) - Math.Sin(f0) * Math.Sin(w0) * Math.Sin(k0);
            R2[0, 1] = -Math.Cos(f0) * Math.Sin(k0) - Math.Sin(f0) * Math.Sin(w0) * Math.Cos(k0);
            R2[0, 2] = -Math.Sin(f0) * Math.Cos(w0);
            R2[1, 0] = Math.Cos(w0) * Math.Sin(k0);
            R2[1, 1] = Math.Cos(w0) * Math.Cos(k0);
            R2[1, 2] = -Math.Sin(w0);
            R2[2, 0] = Math.Sin(f0) * Math.Cos(k0) + Math.Cos(f0) * Math.Sin(w0) * Math.Sin(k0);
            R2[2, 1] = -Math.Sin(f0) * Math.Sin(k0) + Math.Cos(f0) * Math.Sin(w0) * Math.Cos(k0);
            R2[2, 2] = Math.Cos(f0) * Math.Cos(w0);//计算旋转矩阵各元素值

            JZ XYZ1 = new JZ(3, 6);
            JZ XYZ2 = new JZ(3, 6);

            XYZ1 = R1.Multiply(xyf1);
            XYZ2 = R2.Multiply(xyf2);
            double bx = xyf1[0, 0] - xyf2[0, 0];

            JZ N = new JZ(2, 6);
            JZ X = new JZ(5, 1);
            JZ Q = new JZ(6, 1);
            JZ A = new JZ(6, 5);

            do
            {
                R2[0, 0] = Math.Cos(f0) * Math.Cos(k0) - Math.Sin(f0) * Math.Sin(w0) * Math.Sin(k0);
                R2[0, 1] = -Math.Cos(f0) * Math.Sin(k0) - Math.Sin(f0) * Math.Sin(w0) * Math.Cos(k0);
                R2[0, 2] = -Math.Sin(f0) * Math.Cos(w0);
                R2[1, 0] = Math.Cos(w0) * Math.Sin(k0);
                R2[1, 1] = Math.Cos(w0) * Math.Cos(k0);
                R2[1, 2] = -Math.Sin(w0);
                R2[2, 0] = Math.Sin(f0) * Math.Cos(k0) + Math.Cos(f0) * Math.Sin(w0) * Math.Sin(k0);
                R2[2, 1] = -Math.Sin(f0) * Math.Sin(k0) + Math.Cos(f0) * Math.Sin(w0) * Math.Cos(k0);
                R2[2, 2] = Math.Cos(f0) * Math.Cos(w0);//计算旋转矩阵各元素值
                XYZ2     = R2.Multiply(xyf2);


                by = bx * m0;
                bz = bx * v0;
                for (int i = 0; i < 6; i++)
                {
                    //计算点投影系数
                    N[0, i] = (bx * XYZ2[2, i] - bz * XYZ2[0, i]) / (XYZ1[0, i] * XYZ2[2, i] - XYZ2[0, i] * XYZ1[2, i]);
                    N[1, i] = (bx * XYZ1[2, i] - bz * XYZ1[0, i]) / (XYZ1[0, i] * XYZ2[2, i] - XYZ2[0, i] * XYZ1[2, i]);

                    Q[i, 0] = N[0, i] * XYZ1[1, i] - N[1, i] * XYZ2[1, i] - by;

                    A[i, 0] = bx;
                    A[i, 1] = -bx * XYZ2[1, i] / XYZ2[2, i];
                    A[i, 2] = -N[1, i] * XYZ2[0, i] * XYZ2[1, i] / XYZ2[2, i];
                    A[i, 3] = -N[1, i] * (XYZ2[2, i] + XYZ2[1, i] * XYZ2[1, i] / XYZ2[2, i]);
                    A[i, 4] = XYZ2[0, i] * N[1, i];
                }

                JZ AT     = new JZ();
                JZ ATL    = new JZ();
                JZ ATA    = new JZ();
                JZ ATAn   = new JZ();
                JZ ATAnAT = new JZ();
                AT     = A.Transpose();
                ATA    = AT.Multiply(A);
                ATAn   = ATA.InvertGaussJordan();
                ATAnAT = ATAn.Multiply(AT);
                X      = ATAnAT.Multiply(Q);

                m0 = m0 + X[0, 0];
                v0 = v0 + X[1, 0];
                f0 = f0 + X[2, 0];
                w0 = w0 + X[3, 0];
                k0 = k0 + X[4, 0];
            } while (Math.Abs(X[0, 0]) >= 0.00003 || Math.Abs(X[1, 0]) >= 0.00003 || Math.Abs(X[2, 0]) >= 0.00003 || Math.Abs(X[3, 0]) >= 0.00003 || Math.Abs(X[4, 0]) >= 0.00003);

            double m;//求中误差

            double[] mk = new double[5]; double l;
            JZ       T = new JZ(); T = (A.Multiply(X) - Q).Transpose().Multiply(A.Multiply(X) - Q);

            l = T[0, 0];
            m = Math.Sqrt(l / (6 - 5));
            JZ P = new JZ(6, 6);

            P = A.Transpose().Multiply(A).InvertGaussJordan();
            for (int t = 0; t < 5; t++)
            {
                mk[t] = (Math.Sqrt(P[t, t])) * m;
            }



            //textBox2.Text += "m: " + m0.ToString() + "\r\n";
            //textBox2.Text += "v: " + v0.ToString() + "\r\n";
            //textBox2.Text += "f: " + f0.ToString() + "\r\n";
            //textBox2.Text += "w: " + w0.ToString() + "\r\n";
            //textBox2.Text += "k: " + k0.ToString() + "\r\n";

            //textBox2.Text += "m的精度: " + mk[0].ToString() + "\r\n";
            //textBox2.Text += "v的精度: " + mk[1].ToString() + "\r\n";
            //textBox2.Text += "f的精度: " + mk[2].ToString() + "\r\n";
            //textBox2.Text += "w的精度: " + mk[3].ToString() + "\r\n";
            //textBox2.Text += "k的精度: " + mk[4].ToString() + "\r\n";

            JZ XYZm = new JZ(3, 6);//计算模型点坐标

            for (int i = 0; i < 6; i++)
            {
                XYZm[0, i] = N[0, i] * XYZ1[0, i];
                XYZm[1, i] = 0.5 * (N[0, i] * XYZ1[1, i] + N[1, i] * XYZ2[1, i] + bx * m0);
                XYZm[2, i] = N[0, i] * XYZ1[2, i];
            }

            int mm   = 37000;        //像片比例尺分母
            JZ  XYZp = new JZ(3, 6); //计算模型点的摄影测量坐标

            for (int i = 0; i < 6; i++)
            {
                XYZp[0, i] = mm * N[0, i] * XYZ1[0, i];
                XYZp[1, i] = 0.5 * (N[0, i] * XYZ1[1, i] + N[1, i] * XYZ2[1, i] + bx * m0) * mm;
                XYZp[2, i] = mm * f + mm * N[0, i] * XYZ1[2, i];
            }

            JZ     XYZtp = new JZ(3, 6);//计算各点的地面摄影测量坐标
            JZ     R = new JZ(3, 3);
            JZ     dXYZ = new JZ(3, 6);
            double f00 = 0.0527; double w00 = 0.1426; double k00 = 0.2478;

            double numda = 1.00156;

            R[0, 0] = Math.Cos(f00) * Math.Cos(k00) - Math.Sin(f00) * Math.Sin(w00) * Math.Sin(k00);
            R[0, 1] = -Math.Cos(f00) * Math.Sin(k00) - Math.Sin(f00) * Math.Sin(w00) * Math.Cos(k00);
            R[0, 2] = -Math.Sin(f00) * Math.Cos(w00);
            R[1, 0] = Math.Cos(w00) * Math.Sin(k00);
            R[1, 1] = Math.Cos(w00) * Math.Cos(k00);
            R[1, 2] = -Math.Sin(w00);
            R[2, 0] = Math.Sin(f00) * Math.Cos(k00) + Math.Cos(f00) * Math.Sin(w00) * Math.Sin(k00);
            R[2, 1] = -Math.Sin(f00) * Math.Sin(k00) + Math.Cos(f00) * Math.Sin(w00) * Math.Cos(k00);
            R[2, 2] = Math.Cos(f00) * Math.Cos(w00);

            for (int i = 0; i < 6; i++)
            {
                dXYZ[0, i] = 6385.067;
                dXYZ[1, i] = 1954.325;
                dXYZ[2, i] = 724.215;
            }
            XYZtp = R.Multiply(numda).Multiply(XYZp).Add(dXYZ);

            textBox2.Text += "模型点坐标: " + "\r\n";
            for (int i = 0; i < 6; i++)
            {
                textBox2.Text += "(" + XYZm[0, i].ToString() + "," + XYZm[1, i].ToString() + "," + XYZm[2, i].ToString() + ")" + "\r\n";
            }
            textBox2.Text += "模型点的摄影测量坐标: " + "\r\n";
            for (int i = 0; i < 6; i++)
            {
                textBox2.Text += "(" + XYZp[0, i].ToString() + "," + XYZp[1, i].ToString() + "," + XYZp[2, i].ToString() + ")" + "\r\n";
            }
            textBox2.Text += "各点的地面摄影测量坐标: " + "\r\n";
            for (int i = 0; i < 6; i++)
            {
                textBox2.Text += "(" + XYZtp[0, i].ToString() + "," + XYZtp[1, i].ToString() + "," + XYZtp[2, i].ToString() + ")" + "\r\n";
            }
        }
示例#7
0
文件: JZ.cs 项目: linbin-25/cucunku2
 public JZ(JZ other)
 {
     numColumns = other.GetNumColumns();
     numRows    = other.GetNumRows();
     Init(numRows, numColumns);
 }
示例#8
0
文件: JZ.cs 项目: linbin-25/cucunku2
        public JZ InvertGaussJordan()  //矩阵的求逆
        {
            int    i, j, k, l, u, v;
            double d = 0.0, p = 0.0;

            int[] pnRow = new int[numColumns];
            int[] pnCol = new int[numColumns];
            {
                for (k = 0; k <= numColumns - 1; k++)
                {
                    d = 0.0;
                    for (i = k; i <= numColumns - 1; i++)
                    {
                        for (j = k; j <= numColumns - 1; j++)
                        {
                            l = i * numColumns + j; p = Math.Abs(elements[l]);
                            if (p > d)
                            {
                                d        = p;
                                pnRow[k] = i;
                                pnCol[k] = j;
                            }
                        }
                    }
                    if (d == 0.0)
                    {
                        throw new Exception("矩阵不可逆。");
                    }
                    if (pnRow[k] != k)
                    {
                        for (j = 0; j <= numColumns - 1; j++)
                        {
                            u           = k * numColumns + j;
                            v           = pnRow[k] * numColumns + j;
                            p           = elements[u];
                            elements[u] = elements[v];
                            elements[v] = p;
                        }
                    }
                    if (pnCol[k] != k)
                    {
                        for (i = 0; i <= numColumns - 1; i++)
                        {
                            u           = i * numColumns + k;
                            v           = i * numColumns + pnCol[k];
                            p           = elements[u];
                            elements[u] = elements[v];
                            elements[v] = p;
                        }
                    }
                    l           = k * numColumns + k;
                    elements[l] = 1.0 / elements[l];
                    for (j = 0; j <= numColumns - 1; j++)
                    {
                        if (j != k)
                        {
                            u           = k * numColumns + j;
                            elements[u] = elements[u] * elements[l];
                        }
                    }
                    for (i = 0; i <= numColumns - 1; i++)
                    {
                        if (i != k)
                        {
                            for (j = 0; j <= numColumns - 1; j++)
                            {
                                if (j != k)
                                {
                                    u           = i * numColumns + j;
                                    elements[u] = elements[u] - elements[i * numColumns + k] * elements[k * numColumns + j];
                                }
                            }
                        }
                    }
                    for (i = 0; i <= numColumns - 1; i++)
                    {
                        if (i != k)
                        {
                            u           = i * numColumns + k;
                            elements[u] = -elements[u] * elements[l];
                        }
                    }
                }
                for (k = numColumns - 1; k >= 0; k--)
                {
                    if (pnCol[k] != k)
                    {
                        for (j = 0; j <= numColumns - 1; j++)
                        {
                            u           = k * numColumns + j;
                            v           = pnCol[k] * numColumns + j;
                            p           = elements[u];
                            elements[u] = elements[v];
                            elements[v] = p;
                        }
                    }
                    if (pnRow[k] != k)
                    {
                        for (i = 0; i <= numColumns - 1; i++)
                        {
                            u           = i * numColumns + k;
                            v           = i * numColumns + pnRow[k];
                            p           = elements[u];
                            elements[u] = elements[v];
                            elements[v] = p;
                        }
                    }
                }
            }
            JZ result = new JZ(numRows, numColumns);

            for (int a = 0; a < numRows; ++a)
            {
                for (int b = 0; b < numColumns; ++b)
                {
                    result.SetElement(a, b, GetElement(a, b));
                }
            }
            return(result);
        }