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();
}
}
}
基于罗格里德矩阵的坐标转换7参数求取
最新推荐文章于 2024-12-29 19:28:23 发布
本文详细探讨了罗格里德矩阵在坐标转换中的使用，重点讲解了如何通过该矩阵求取7参数坐标转换方法。内容包括矩阵理论基础，C#和MATLAB实现代码示例，以及实际应用中的注意事项。
扫一扫
1万+
到【灌水乐园】发言
