基于罗格里德矩阵的坐标转换7参数求取

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;



namespace Spearman_count
{
    class Spearman 
    {
       public static double[,] Input(int n)//对输入的坐标，按行分，即每行存储一个点的坐标(x,y,z)信息;
        {            
            double[,] coordinate = new double[n, 3];
            string[] st = Console.ReadLine().Split(' ');
            for (int i = 0; i < n; ++i)
            {
                coordinate[i, 0] = Convert.ToDouble(st[0 + i * 3]);
                coordinate[i, 1] = Convert.ToDouble(st[1 + i * 3]);
                coordinate[i, 2] = Convert.ToDouble(st[2 + i * 3]);
            }
                return coordinate;
        }
       public static int Factorial(int x)//定义一个用于计算阶乘的方法；
        {
            int y=1;
            for (int i = x; i > 0; --i)
                y = y * i;
            return y;
        }
       public static int Com_number(int n, int m)//计算C(n,m)
        {

            int N = (Factorial(n) / (Factorial(n - m)) / Factorial(m));
            return N;
        }
       public static double[,] Combination_2(int C_n_m, int n, double[,] a)
        {
            double[,] com = new double[C_n_m, 6];int I = 0;int J = 0;          
                    for (int i_1 = 0; i_1 < n - 1; ++i_1)
                        for (int i_2 = 1 + i_1; i_2 < n ; ++i_2)
                            for (int j = 0; j < 3; ++j)
                                {
                                    com[I, J] = a[i_1, j]; ++J;
                                    com[I, J] = a[i_2, j]; ++J;
                                    if (J == 6) { J = 0; ++I; } 
                                }
            return com;
        }   
       public static double[,] Combination_3(int C_n_m,int n,double[,] a)//定义一个从n个点中抽取3个的组合方法；
        {
            double[,] com = new double[C_n_m, 9]; int I = 0; int J = 0;   
        
                    for (int i_1 = 0; i_1 < n - 2; ++i_1)
                        for (int i_2 = 1 + i_1; i_2 < n - 1; ++i_2)
                            for (int i_3 = 1 + i_2; i_3 < n; ++i_3)
                                for (int j = 0; j < 3; ++j)
                                {
                                    com[I, J] = a[i_1, j]; ++J;  com[I, J] = a[i_2, j]; ++J;
                                    com[I, J] = a[i_3, j]; ++J;
                                    if (J == 9) { J = 0; I = I + 1; }                                    
                                } 
            return com;
        }
       public static double[,] Combination_4(int C_n_m, int n, double[,] a)//定义一个从n个点中抽取4个的组合方法；
        {
            double[,] com = new double[C_n_m, 12]; int I = 0; int J = 0;
            for (int i_1 = 0; i_1 < n - 3; ++i_1)
                        for (int i_2 = 1 + i_1; i_2 < n - 2; ++i_2)
                            for (int i_3 = 1 + i_2; i_3 < n - 1; ++i_3)
                                for (int i_4 = 1 + i_3; i_4 < n; ++i_4 )
                                    for (int j = 0; j < 3; ++j)
                                    {
                                        com[I, J] = a[i_1, j]; ++J; com[I, J] = a[i_2, j]; ++J;
                                        com[I, J] = a[i_3, j]; ++J; com[I, J] = a[i_4, j]; ++J;
                                        if (J == 12) { J = 0; ++I; }
                                    }
            return com;
        }
       public static double[,] Combination_5(int C_n_m, int n, double[,] a)//定义一个从n个点中抽取5个的组合方法；
        {
            double[,] com = new double[C_n_m, 15]; int I = 0; int J = 0;
                    for (int i_1 = 0; i_1 < n - 4; ++i_1)
                        for (int i_2 = 1 + i_1; i_2 < n - 3; ++i_2)
                            for (int i_3 = 1 + i_2; i_3 < n - 2; ++i_3)
                                for (int i_4 = 1 + i_3; i_4 < n - 1; ++i_4)
                                    for (int i_5 = 1 + i_4; i_5 < n; ++i_5)
                                        for (int j = 0; j < 3; ++j)
                                        {
                                            com[I, J] = a[i_1, j]; ++J; com[I, J] = a[i_2, j]; ++J;
                                            com[I, J] = a[i_3, j]; ++J; com[I, J] = a[i_4, j]; ++J;
                                            com[I, J] = a[i_5, j]; ++J; if (J == 15) { J = 0; I = I + 1; }
                                        }
            return com;
        }
       public static double[,] Com_Array(int C_n_m, int n, int m, double[,] a)//定义一个生成组合数组的方法；
        {
            double[,] com_a = new double[C_n_m, m * 3]; 
            switch (m)
            {
                case 2:
                    com_a = Combination_2(C_n_m, n, a);
                    break;
                case 3:
                    com_a = Combination_3(C_n_m, n, a);
                    break;
                case 4:
                    com_a = Combination_4(C_n_m, n, a);
                    break;
                case 5:
                    com_a = Combination_5(C_n_m, n, a);
                    break;
            }
            return com_a;
        }
       public static double[,] R(int C_n_m, int m, double[,] a)//定义一个对组合数组安装每行进行求秩的方法，默认每个数都不相等，返回每行的秩；
        {
            double[,] R_a = new double[C_n_m, m * 3];//实现对数组内所有x、y、z元素排序后存储的空间；
            double[,] r = new double[C_n_m, m * 3];
            double[] b=new double[m * 3];
            for (int i = 0; i < C_n_m; ++i)//逐行进行排序；
            {
                for (int j = 0; j < m * 3; ++j)
                    b[j] = a[i, j];
                Array.Sort(b);
                Array.Reverse(b);    //实现倒序排列，即数值越大，秩越大；
                for (int k = 0; k < m * 3; ++k)
                    R_a[i,k] = b[k];
                Array.Clear(b, 0, 6);
            }
            for (int i = 0; i < C_n_m; ++i)
                for (int j = 0; j < m * 3; ++j)
                    for (int k = 0; k < m * 3; ++k)
                        if (a[i, j] == R_a[i, k])//定原数组a的元素，对排序好的数组R_a进行遍历；
                            r[i, j] = k + 1;
            return r;
        }
       public static double[] spearman(int C_n_m, int m, double[,] a, double[,] b)//求每个组合数组的spearman系数；
        {                                                                   //数组a、b分别为新、旧坐标秩数；每一行为一组公共点，列为新旧坐标值。
            double[,] Diffe_Sqr_R = new double[C_n_m, m * 3]; //每个元素的秩作差求平方；
            double[] sum = new double[C_n_m];                 //每组内的秩差平方求和；
            double[] r_s = new double[C_n_m];                 //每组的Spearman系数；
            for (int i = 0; i < C_n_m; ++i)
            {
                for (int j = 0; j < 3 * m; ++j)
                {
                    Diffe_Sqr_R[i, j] = Math.Pow((a[i, j] - b[i, j]), 2);
                    sum[i] = sum[i] + Diffe_Sqr_R[i, j];
                }
                r_s[i] = 1 - 6 * sum[i] / ((3 * m) * (Math.Pow(3 * m, 2) - 1));
            }
            return r_s;
        }
       public static double[] Sort(int C_n_m,double[] a)
       {
           double[] sort = new double[C_n_m];
           Array.Sort(a);
           for (int j = 0; j < C_n_m; ++j)
               sort[j] = a[j];
           return sort;
       }
       public static double[,] Print(int C_n_m,int n, int m, double[,] a, double[,] b)
        {
            double[,] R_N = new double[C_n_m, m * 3]; R_N = R(C_n_m, m, a);  //求新坐标中每种组合方法的秩；
            double[,] R_O = new double[C_n_m, m * 3]; R_O = R(C_n_m, m, b);
            double[] R_S = new double[C_n_m]; R_S = spearman(C_n_m, m, R_N, R_O);//求每一组的spearman系数；
            double[,] print = new double[C_n_m, 1 + 6 * m];
            int I=0,J=0;
            for (int i = 0; i < C_n_m; ++i)
            {
                for (int j = 0; j < 6 * m; ++j)
                {
                    if (j < 3 * m)
                    { print[I, J] = a[i, j]; ++J; }
                    else
                    { print[I, J] = b[i, j - (3 * m)]; ++J; }
                }
                print[I, J] = R_S[I]; ++I; J = 0;
            }            
           for (int i = 0; i < C_n_m; ++i)
           {
               Console.Write("{0}，", i + 1);
               for (int j = 0; j < 1 + 6 * m; ++j)
                   Console.Write("{0:f5}，", print[i, j]);
               Console.Write("\n");
           }
           Console.ReadLine();
           return print;
       }     
    }

    class parameter : Spearman
   {
       public static double[] M_para(int C_n_m, int m, double[,] a)//比例尺参数；//运行无误
       {
           double[,] I = new double[C_n_m, 6 * (m - 1)];
           double[,] m_i_j = new double[C_n_m, m - 1];
           double[] sum_m_i_j = new double[C_n_m]; sum_m_i_j[0] = 0;
           double[] M = new double[C_n_m];
           int k = 0;
           for (int i = 0; i < C_n_m; ++i)
           {
               for (int j = 0; j < 2 * 3 * m; ++j)
               {                         //例：m=3;
                   if (j < m - 1)        //k=0,1; I存储的是相邻x相减平方的值；                
                   { I[i, k] = Math.Pow(a[i, j] - a[i, j + 1], 2); ++k; }      //K=0,1
                   else if (j >= m && j < 2 * m - 1)  //k=3,4; I存储的是相邻Y相减平方的值；
                   { I[i, k] = Math.Pow(a[i, j] - a[i, j + 1], 2); ++k; }      //K=2,3
                   else if (j >= 2 * m && j < 3 * m - 1)  //k=6,7; Z
                   { I[i, k] = Math.Pow(a[i, j] - a[i, j + 1], 2); ++k; }      //K=4,5
                   else if (j >= 3 * m && j < 4 * m - 1)   //k=9,10; k
                   { I[i, k] = Math.Pow(a[i, j] - a[i, j + 1], 2); ++k; }      //K=6,7
                   else if (j >= 4 * m && j < 5 * m - 1)  //k=12,13;y
                   { I[i, k] = Math.Pow(a[i, j] - a[i, j + 1], 2); ++k; }      //K=8,9
                   else if (j >= 5 * m && j < 6 * m - 1)   //k=15,16;z
                   { I[i, k] = Math.Pow(a[i, j] - a[i, j + 1], 2); ++k; }      //K=10,11
                   else
                       continue;
               }
               k = 0;   //返回的数组I：(X1-X2)^2,(X2-X3)^2,(Y1-Y2)^2,(Y2-Y3)^2,(Z1-Z2)^2,(Z2-Z3)^2,
                        //             (x1-x2)^2,(x2-x3)^2,(y1-y2)^2,(y2-y3)^2,(z1-z2)^2,(z2-z3)^2;
           }
           for (int i = 0; i < C_n_m; ++i)
               for (int j = 0; j < m - 1; ++j)
               {
                   m_i_j[i, j] = Math.Sqrt((I[i, j] + I[i, j + (m - 1)] + I[i, j + (2 * (m - 1))])
                                         / (I[i, j + (3 * (m - 1))] + I[i, j + (3 * (m - 1)) + (m - 1)] + I[i, j + (3 * (m - 1)) + (2 * (m - 1))]));
                   //如m=4———————(I[0,0]+I[0,   3  )]+I[i,    6     ])/(I[i,     9    ))]+I[i,         12      ]+I[i,            15       ])
                   //m=3——3个点能做每个坐标能做2组差，I每行有2*(3+3);
                   sum_m_i_j[i] = sum_m_i_j[i] + m_i_j[i, j];
               }
           for (int i = 0; i < C_n_m; ++i)
               M[i] = (2 * sum_m_i_j[i] / (m * (m - 1)));           
           foreach(double i in M)
               Console.Write("{0:f5}，",i);
           Console.Write("\n");
           Console.ReadLine();
           return M;
       }
       public static double[, ,] Rota_rel(int C_n_m, int m,double[] M, double[,] a)//求公共点旋转参数相关矩阵,最后一列为坐标差；//运行无误
       {           
           double[, ,] R_rel = new double[C_n_m, 3 * (m - 1), 4];//构建一个用于求罗格里德矩阵中三个参数的3*(m-1)×3反对称矩阵,
           double[,] Sub_N = new double[C_n_m, 3 * (m - 1)];  //       最后一列将存储sub;
           double[,] Sub_O = new double[C_n_m, 3 * (m - 1)]; //已经乘以尺度参数M；
           double[,] sub = new double[C_n_m, 3 * (m - 1)];//新坐标与旧坐标×M之后的差；
           double[,] add = new double[C_n_m, 3 * (m - 1)];//辅助建立反对称矩阵； 
           int K = 0;
           for (int i = 0; i < C_n_m; ++i)  //将新旧坐标分开存储；               //新：X2-X1,X3-X1,Y2-Y1,Y3-Y1,Z2-Z1,Z3-Z1;
           {                                                                     //旧：Move×(x2-x1,x3-x1,y2-y1,y3-y1,z2-z1,z3-z1);
               for (int j = 1; j < 6 * m; ++j)
               {                                                   //li：m=3;
                   if (j < m)                                      //k=1,2
                   { Sub_N[i, K] = a[i, j] - a[i, 0]; ++K; }
                   else if (j > m && j < 2 * m)                    //k=4,5
                   { Sub_N[i, K] = a[i, j] - a[i, m]; ++K; }
                   else if (j > 2 * m && j < 3 * m)                //k=7,8
                   {
                       Sub_N[i, K] = a[i, j] - a[i, 2 * m]; ++K;
                       if (K == 3 * (m - 1)) K = 0;
                   }
                   else if (j > 3 * m && j < 4 * m)                //k=10,11
                   { Sub_O[i, K] = M[i] * (a[i, j] - a[i, 3 * m]); ++K; }
                   else if (j > 4 * m && j < 5 * m)                //k=13,14
                   { Sub_O[i, K] = M[i] * (a[i, j] - a[i, 4 * m]); ++K; }
                   else if (j > 5 * m && j < 6 * m)                //k=16,17
                   { Sub_O[i, K] = M[i] * (a[i, j] - a[i, 5 * m]); ++K; }
                   else
                       continue;
               } K = 0;
           }
           for (int i = 0; i < C_n_m; ++i)
               for (int j = 0; j < 3 * (m - 1); ++j)
               {
                   sub[i, j] = Sub_N[i, j] - Sub_O[i, j];//完成求新坐标与旧坐标×M之后的差以及和计算，U21-X21,U31-X31,V21-Y21,V31-Y31,W21-Z21,W31-Z31;
                   add[i, j] = Sub_N[i, j] + Sub_O[i, j];//和计算用于辅助建立反对称阵;行之间为组合方式,行内是一种组合方式中关于点的计算
               }                                         //add数组: U21+X21,U31+X31,V21+Y21,V31+Y31,W21+Z21,W31+Z31;
           int L = 0;
           for (int i = 0; i < C_n_m; ++i)
           {
               for (int j = 0; j < 3 * (m - 1); ++j) //m=3  ,  k<6
               {
                   for (int k = 0; k < 4; ++k)
                   {
                       if (j % 3 == 0)
                       {
                           switch (k)
                           {
                               case 0:
                                   R_rel[i, j, k] = 0;
                                   break;
                               case 1:
                                   R_rel[i, j, k] = 0 - add[i, K + 2 * (m - 1)];
                                   break;
                               case 2:
                                   R_rel[i, j, k] = 0 - add[i, K + (m - 1)];
                                   break;
                               case 3:
                                   R_rel[i, j, 3] = sub[i, L];
                                   break;
                           }
                       }
                       if (j % 3 == 1)
                       {
                           switch (k)
                           {
                               case 0:
                                   R_rel[i, j, k] = R_rel[i, j - 1, 1];
                                   break;
                               case 1:
                                   R_rel[i, j, k] = 0;
                                   break;
                               case 2:
                                   R_rel[i, j, k] = add[i, K]; ++K;
                                   break;
                               case 3:
                                   R_rel[i, j, 3] = sub[i, (m - 1) + L];
                                   break;
                           }
                       }
                       if (j % 3 == 2)
                       {
                           switch (k)
                           {
                               case 0:
                                   R_rel[i, j, k] = R_rel[i, j - 2, 2];
                                   break;
                               case 1:
                                   R_rel[i, j, k] = R_rel[i, j - 1, 2];
                                   break;
                               case 2:
                                   R_rel[i, j, k] = 0;
                                   break;
                               case 3:
                                   R_rel[i, j, 3] = sub[i, 2 * (m - 1) + L]; ++L;
                                   break;
                           }
                       }
                   }
               } K = 0; L = 0;
           }           
           for (int i = 0; i < C_n_m; ++i)
           {
               for (int j = 0; j < 3*(m-1); ++j)
               {
                   for (int k = 0; k < 4; ++k)
                   {
                       Console.Write("{0:f5}", R_rel[i, j, k]);
                       if (k != 3) Console.Write("，");
                   }
                   Console.Write("\n");
               }Console.Write("\n");               
           }Console.ReadLine();           
           return R_rel;
       }
//借助MATLAB，求出a,b,c      
       public static double[, ,] Rota_para(int C_n_m, double[,] loge)//9个旋转参数，运行无误
       {
           double[] H = new double[C_n_m];
           for (int i = 0; i < C_n_m; ++i)
               H[i] = 1 + Math.Pow(loge[i, 0], 2) + Math.Pow(loge[i, 1], 2) + Math.Pow(loge[i, 2], 2);
           double[, ,] R = new double[C_n_m, 3, 3]; //最终旋转矩阵；
           for (int i = 0; i < C_n_m; ++i)
           {
               R[i, 0, 0] = (1 + Math.Pow(loge[i, 0], 2) - Math.Pow(loge[i, 1], 2) - Math.Pow(loge[i, 2], 2)) / H[i];
               R[i, 0, 1] = 0 - 2 * (loge[i, 2] + loge[i, 0] * loge[i, 1]) / H[i];
               R[i, 0, 2] = 0 - 2 * (loge[i, 1] - loge[i, 0] * loge[i, 2]) / H[i];
               R[i, 1, 0] = 2 * (loge[i, 2] - loge[i, 0] * loge[i, 1]) / H[i];
               R[i, 1, 1] = (1 - Math.Pow(loge[i, 0], 2) + Math.Pow(loge[i, 1], 2) - Math.Pow(loge[i, 2], 2)) / H[i];
               R[i, 1, 2] = 0 - 2 * (loge[i, 0] + loge[i, 1] * loge[i, 2]) / H[i];
               R[i, 2, 0] = 2 * (loge[i, 1] + loge[i, 0] * loge[i, 2]) / H[i];
               R[i, 2, 1] = 2 * (loge[i, 0] - loge[i, 1] * loge[i, 2]) / H[i];
               R[i, 2, 2] = (1 - Math.Pow(loge[i, 0], 2) - Math.Pow(loge[i, 1], 2) + Math.Pow(loge[i, 2], 2)) / H[i];
           }
           for (int i = 0; i < C_n_m; ++i)
           {
               for (int j = 0; j < 3; ++j)
               {
                   for (int k = 0; k < 3; ++k)
                       Console.Write("{0,5}，", R[i, j, k]);
                   Console.Write("\n");
               } Console.Write("\n");
           } Console.ReadLine();
           return R;
       }
       public static double[,] Move_para(int C_n_m, int m, double[,] a,double[] M , double[, ,] R)//平移参数，运行无误；
       {                                                                                      //move的存储内容：m=3
           double[, ,] move = new double[C_n_m, 3, 3 * m];                                    //DX1 DX2 DX3  →相加 sum[,0]
           double[,] sum = new double[C_n_m, 3]; sum[0, 0] = sum[0, 1] = sum[0, 2] = 0;       //DY1 DY2 DY3  →相加 sum[,1]
           double[,] Move = new double[C_n_m, 3];  //返回最终的平移参数，即求平均后；         //DZ1 DZ2 DZ3  →相加 sum[,2]
           for (int i = 0; i < C_n_m; ++i)
           {
               for (int k = 0; k < m; ++k)
               {
                   move[i, 0, k] = a[i, k] - M[i] * (R[i, 0, 0] * a[i, k + 3 * m] + R[i, 0, 1] * a[i, k + 3 * m + m] + R[i, 0, 2] * a[i, k + 3 * m + 2 * m]);
                   sum[i, 0] = sum[i, 0] + move[i, 0, k];
                   move[i, 1, k] = a[i, m + k] - M[i] * (R[i, 1, 0] * a[i, k + 3 * m] + R[i, 1, 1] * a[i, k + 3 * m + m] + R[i, 1, 2] * a[i, k + 3 * m + 2 * m]);
                   sum[i, 1] = sum[i, 1] + move[i, 1, k];
                   move[i, 2, k] = a[i, 2 * m + k] - M[i] * (R[i, 2, 0] * a[i, k + 3 * m] + R[i, 2, 1] * a[i, k + 3 * m + m] + R[i, 2, 2] * a[i, k + 3 * m + 2 * m]);
                   sum[i, 2] = sum[i, 2] + move[i, 2, k];
               }
               Move[i, 0] = sum[i, 0] / m; Move[i, 1] = sum[i, 1] / m; Move[i, 2] = sum[i, 2] / m;
           }
           for (int i = 0; i < C_n_m; ++i)
           {
               for (int j = 0; j < 3; ++j)
                   Console.Write("{0,5}，", Move[i, j]);
               Console.Write("\n");
           } Console.ReadLine();
           return Move;
       }
       public static double[] Va_error(int C_n_m, int m, double[,] a, double[] M,double[, ,] R, double[,] Move)//求方差，运行无误；
       {
           double[, ,] TheoCo = new double[C_n_m, m, 3];
           double[, ,] error_V = new double[C_n_m, m, 3];
           double[] Va = new double[C_n_m];
           double[] sum = new double[C_n_m]; sum[0] = 0;
           for (int i = 0; i < C_n_m; ++i)
           {
               for (int j = 0; j < m; ++j)
               {
                   TheoCo[i, j, 0] = Move[i, 0] + M[i] * (R[i, 0, 0] * a[i, 3 * m + j] + R[i, 0, 1] * a[i, 4 * m + j] + R[i, 0, 2] * a[i, 5 * m + j]);
                   error_V[i, j, 0] = a[i, j] - TheoCo[i, j, 0];
                   TheoCo[i, j, 1] = Move[i, 1] + M[i] * (R[i, 1, 0] * a[i, 3 * m + j] + R[i, 1, 1] * a[i, 4 * m + j] + R[i, 1, 2] * a[i, 5 * m + j]);
                   error_V[i, j, 1] = a[i, m + j] - TheoCo[i, j, 1];
                   TheoCo[i, j, 2] = Move[i, 2] + M[i] * (R[i, 2, 0] * a[i, 3 * m + j] + R[i, 2, 1] * a[i, 4 * m + j] + R[i, 2, 2] * a[i, 5 * m + j]);
                   error_V[i, j, 2] = a[i, 2 * m + j] - TheoCo[i, j, 2];
                   sum[i] = sum[0] + (Math.Pow(error_V[i, j, 0], 2) + Math.Pow(error_V[i, j, 1], 2) + Math.Pow(error_V[i, j, 2], 2));
               }
               Va[i] = sum[i] / (3 * m - 7);
           }       
           foreach (int i in Va)
               Console.Write("{0,5}，", i);
           Console.ReadLine();
           return Va;
       }
       static void Main(string[] args)
       {
           Console.Write(" 请说明成对公共点的数量：\n");//应当大于3
           int n = Convert.ToInt32(Console.ReadLine());
           Console.Write(" 请说明要选择用于7参数计算的公共点数量（应当3~5对）：\n");
           int m = Convert.ToInt32(Console.ReadLine());
           int C_n_m = Com_number(n, m);
           Console.Write(" {0}中选取{1}个点的组合方法共有{2}种。\n", n, m, C_n_m); Console.Write("\n");
           Console.Write(" 请输入新坐标系下点的三维坐标，以空格符分开，如: X Y Z\n");
           double[,] N = new double[n, m * 3]; N = Input(n); Console.Write("\n");
           Console.Write(" 请输入对应原坐标系下点的三维坐标，以空格符分开，如: x y z\n");
           double[,] O = new double[n, m * 3]; O = Input(n);
           double[,] Com_N = new double[C_n_m, m * 3]; Com_N = Com_Array(C_n_m, n, m, N); //对新坐标点进行组合，行数为组合数量C(n,m),列数为公共点坐标总量；            
           double[,] Com_O = new double[C_n_m, m * 3]; Com_O = Com_Array(C_n_m, n, m, O);//每种组合方法构成一行；
           Console.Write("\n");
           double[,] N_O_Spear = new double[C_n_m, 6 * m + 1];//对组合后新旧坐标以及spearman系数组成一行；
           N_O_Spear = Print(C_n_m, n, m, Com_N, Com_O);
           double[] M = new double[C_n_m]; M = M_para(C_n_m, m, N_O_Spear);//尺度参数；
           double[, ,] R_rel = new double[C_n_m, 3 * (m - 1), 4];//用于计算罗格里德参数a,b,c的系数矩阵；
           R_rel = Rota_rel(C_n_m, m, M, N_O_Spear);           
           double[,] loge = new double[C_n_m, 3];//从MATLAB中获取3个参数； 

           double[, ,] R = new double[C_n_m, 3, 3];
           R = Rota_para(C_n_m, loge);//完成旋转矩阵的计算；
           double[,] Move = new double[C_n_m, 3];
           Move = Move_para(C_n_m, m, N_O_Spear, M, R);//完成平移参数计算；
           double[] Va = new double[C_n_m];
           Va = Va_error(C_n_m, m, N_O_Spear, M, R, Move);//求方差计算；

           Print(C_n_m, n, m, Com_N, Com_O);
           Console.ReadLine();

       }   
   }
}