地理空间坐标系统-不同椭球基准间的坐标转换-相关算法代码实现C#
在上一篇的论述中，不同椭球基准间的坐标转换除了同一个椭球基准内坐标转换算法外，我们还提到了两个算法：
1.已知两个空间直角坐标间转换的七参数△X，△Y，△Z，ωx，ωy，ωz，m的情况情况下，如何进行椭球转换。
2.在无法获取两个空间直角坐标转换所需的七参数的情况下，如何通过三个以上的坐标点对，反算七参数。
已七参数，进行椭球转换
根据上一篇中的公式，具体代码如下：
///
/// 空间直角坐标之间的转换，使用布尔莎七参转换
///
///
/// X轴平移参数，单位"米"
/// Y轴平移参数，单位"米"
/// Z轴平移参数，单位"米"
/// 旋转参数x,单位"弧度"
/// 旋转参数y,单位"弧度"
/// 旋转参数z,单位"弧度"
/// 尺度参数k
/// 返回的坐标
public static void SpaceRectangularToSpaceRectangularByBursa(
Point3d originalPoint,
double dx,double dy,double dz,double rotax,double rotay,double rotaz,
double scale, out Point3d reslutPoint)
{
double a1 = scale + 1;
double a2 = a1 * rotax;
double a3 = a1 * rotay;
double a4 = a1 * rotaz;
reslutPoint.X = dx + a1 * originalPoint.X
- a3 * originalPoint.Z + a4 * originalPoint.Y;
reslutPoint.Y = dy + a1 * originalPoint.Y
+ a2 * originalPoint.Z - a4 * originalPoint.X;
reslutPoint.Z = dz + a1 * originalPoint.Z
- a2 * originalPoint.Y + a3 * originalPoint.X;
//B=AX，下面是想用矩阵计算的方式，来对结果进行验证。可以忽略
double[,] pX = new double[7, 1];
double[,] pA = new double[3, 7];
pX[0, 0] = dx;
pX[1, 0] = dy;
pX[2, 0] = dz;
pX[3, 0] = a1;
pX[4, 0] = a2;
pX[5, 0] = a3;
pX[6, 0] = a4;
///初始化A矩阵第一步
for (int i = 0; i < 3; i++)
{
if (i % 3 == 0)
{
pA[i, 0] = 1;
pA[i, 1] = 0;
pA[i, 2] = 0;
pA[i, 3] = originalPoint.X;
pA[i, 4] = 0;
pA[i, 5] = -originalPoint.Z;
pA[i, 6] = originalPoint.Y;
}
else if (i % 3 == 1)
{
pA[i, 0] = 0;
pA[i, 1] = 1;
pA[i, 2] = 0;
pA[i, 3] = originalPoint.Y;
pA[i, 4] = originalPoint.Z;
pA[i, 5] = 0;
pA[i, 6] = -originalPoint.X;
}
else if (i % 3 == 2)
{
pA[i, 0] = 0;
pA[i, 1] = 0;
pA[i, 2] = 1;
pA[i, 3] = originalPoint.Z;
pA[i, 4] = -originalPoint.Y;
pA[i, 5] = originalPoint.X;
pA[i, 6] = 0;
}
}
GMatrix X = new GMatrix(pX);
GMatrix A = new GMatrix(pA);
GMatrix B = A*X;
}未知七参数，通过三个以上的坐标点对，反算七参数
根据上一篇的中的公式，具体代码实现如下：
///
/// 七参数反算
///
///
///
/// X轴平移参数，单位"米"
/// Y轴平移参数，单位"米"
/// Z轴平移参数，单位"米"
/// 旋转参数x,单位"弧度"
/// 旋转参数y,单位"弧度"
/// 旋转参数z,单位"弧度"
/// 尺度参数k,
public static void CalSevenParaByTwoSpaceRectangularCoords(
Point3d[] originalCooords,Point3d[] targetCooords,
ref double dx, ref double dy, ref double dz,
ref double rotax, ref double rotay, ref double rotaz, ref double scale)
{
int pointCount = originalCooords.Length;
if (pointCount < 3)
{
//坐标点数小于三。
return;
}
if (targetCooords.Length < pointCount)
{
//新旧坐标个数不匹配
return;
}
double[,] pA = new double[pointCount * 3, 7];
double[,] pB = new double[pointCount * 3, 1];
///初始化A矩阵第一步
for (int i=0;i< pointCount*3;i++) {
if (i % 3 == 0)
{
pA[i, 0] = 1;
pA[i, 1] = 0;
pA[i, 2] = 0;
pA[i, 3] = originalCooords[i / 3].X;
pA[i, 4] = 0;
pA[i, 5] = -originalCooords[i / 3].Z;
pA[i, 6] = originalCooords[i / 3].Y;
}
else if (i % 3 == 1) {
pA[i, 0] = 0;
pA[i, 1] = 1;
pA[i, 2] = 0;
pA[i, 3] = originalCooords[i / 3].Y;
pA[i, 4] = originalCooords[i / 3].Z;
pA[i, 5] = 0;
pA[i, 6] = -originalCooords[i / 3].X;
}
else if (i % 3 == 2)
{
pA[i, 0] = 0;
pA[i, 1] = 0;
pA[i, 2] = 1;
pA[i, 3] = originalCooords[i / 3].Z;
pA[i, 4] = -originalCooords[i / 3].Y;
pA[i, 5] = originalCooords[i / 3].X;
pA[i, 6] = 0;
}
}
///初始化B矩阵第一步
for (int i = 0; i < pointCount * 3; i++)
{
if (i % 3 == 0)
{
pB[i, 0] = targetCooords[i / 3].X;
}
else if (i % 3 == 1)
{
pB[i, 0] = targetCooords[i / 3].Y;
}
else if (i % 3 == 2)
{
pB[i, 0] = targetCooords[i / 3].Z;
}
}
GMatrix A = new GMatrix(pA);
GMatrix AT = A.Transpose();
GMatrix B = new GMatrix(pB);
GMatrix W = AT * A;
GMatrix W1 = W.Inverse();
GMatrix reslut = W1 * AT * B;
GMatrix m = ((A.Transpose() * A).Inverse())* A.Transpose() * B;
//规范化
double Xdelta, Ydelta, Zdelta,Scale, Ex, Ey, Ez;
Xdelta = reslut[0, 0];
Ydelta = reslut[1, 0];
Zdelta = reslut[2, 0];
Scale = reslut[3, 0];
Ex = reslut[4, 0] / Scale;
Ey= reslut[5, 0] / Scale;
Ez = reslut[6, 0] / Scale;
//乱的
double a1, a2, a3, a4;
dx = reslut[0, 0];
dy = reslut[1, 0];
dz = reslut[2, 0];
a1= reslut[3, 0];
a2 = reslut[4, 0];
a3= reslut[5, 0];
a4= reslut[6, 0];
scale = a1-1; //m
rotax = a2 / a1; //wx
rotay = a3 / a1; //wy
rotaz = a4 / a1; //wz
//需要考虑cosA=0 不能作为分母的情况。
}矩阵运算类
上面代码中用到了矩阵，为不去引用第三方数据库，这里同时分享一个矩阵运算类，网上找的，亲测挺好用，谢谢库的作者
using System;
using System.Collections.Generic;
namespace GB.GMath
{
[Serializable]
public class GMatrix
{
public double[] element;
private int rows = 0;
private int cols = 0;
///
/// 获取矩阵行数
///
public int Rows
{
get
{
return rows;
}
}
///
/// 获取矩阵列数
///
public int Cols
{
get
{
return cols;
}
}
///
/// 获取或设置第i行第j列的元素值
///
/// 第i行
/// 第j列
/// 返回第i行第j列的元素值
public double this[int i, int j]
{
get
{
if (i < Rows && j < Cols)
{
return element[i * cols + j];
}
else
{
throw new Exception("索引越界");
}
}
set
{
element[i * cols + j] = value;
}
}
///
/// 用二维数组初始化GMatrix
///
/// 二维数组
public GMatrix(double[][] m)
{
this.rows = m.GetLength(0);
this.cols = m.GetLength(1);
int count = 0;
this.element = new double[Rows * Cols];
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < cols; j++)
{
element[count++] = m[i][j];
}
}
}
public GMatrix(double[,] m)
{
this.rows = m.GetLength(0);
this.cols = m.GetLength(1);
this.element = new double[this.rows * this.cols];
int count = 0;
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < cols; j++)
{
element[count++] = m[i, j];
}
}
}
public GMatrix(List> m)
{
this.rows = m.Count;
this.cols = m[0].Count;
this.element = new double[Rows * Cols];
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < cols; j++)
{
this[i, j] = m[i][j];
}
}
}
#region 矩阵数学运算
public static GMatrix MAbs(GMatrix a)
{
GMatrix _thisCopy = a.DeepCopy();
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
_thisCopy[i, j] = System.Math.Abs(a[i, j]);
}
}
return _thisCopy;
}
///
/// 矩阵相加
///
/// 第一个矩阵,和b矩阵必须同等大小
/// 第二个矩阵
/// 返回矩阵相加后的结果
public static GMatrix operator +(GMatrix a, GMatrix b)
{
if (a.cols == b.cols && a.rows == b.rows)
{
double[,] res = new double[a.rows, a.cols];
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
res[i, j] = a[i, j] + b[i, j];
}
}
return new GMatrix(res);
}
else
{
throw new Exception("两个矩阵行列不相等");
}
}
///
/// 矩阵相减
///
/// 第一个矩阵,和b矩阵必须同等大小
/// 第二个矩阵
/// 返回矩阵相减后的结果
public static GMatrix operator -(GMatrix a, GMatrix b)
{
if (a.cols == b.cols && a.rows == b.rows)
{
double[,] res = new double[a.rows, a.cols];
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
res[i, j] = a[i, j] - b[i, j];
}
}
return new GMatrix(res);
}
else
{
throw new Exception("两个矩阵行列不相等");
}
}
///
/// 对矩阵每个元素取相反数
///
/// 二维矩阵
/// 得到矩阵的相反数
public static GMatrix operator -(GMatrix a)
{
GMatrix res = a;
for (int i = 0; i < a.rows; i++)
{
for (int j = 0; j < a.cols; j++)
{
res.element[i * a.cols + j] = -res.element[i * a.cols + j];
}
}
return res;
}
///
/// 矩阵相乘
///
/// 第一个矩阵
/// 第二个矩阵,这个矩阵的行要与第一个矩阵的列相等
/// 返回相乘后的一个新的矩阵
public static GMatrix operator *(GMatrix a, GMatrix b)
{
if (a.cols == b.rows)
{
double[,] res = new double[a.rows, b.cols];
for (int i = 0; i < a.rows; i++)
{
for (int j = 0; j < b.cols; j++)
{
for (int k = 0; k < a.cols; k++)
{
res[i, j] += a[i, k] * b[k, j];
}
}
}
return new GMatrix(res);
}
else
{
throw new Exception("两个矩阵行和列不等");
}
}
///
/// 矩阵与数相乘
///
/// 第一个矩阵
/// 一个实数
/// 返回相乘后的新的矩阵
public static GMatrix operator *(GMatrix a, double num)
{
GMatrix res = a;
for (int i = 0; i < a.rows; i++)
{
for (int j = 0; j < a.cols; j++)
{
res.element[i * a.cols + j] *= num;
}
}
return res;
}
///
/// 矩阵转置
///
/// 返回当前矩阵转置后的新矩阵
public GMatrix Transpose()
{
double[,] res = new double[cols, rows];
{
for (int i = 0; i < cols; i++)
{
for (int j = 0; j < rows; j++)
{
res[i, j] = this[j, i];
}
}
}
return new GMatrix(res);
}
///
/// 矩阵求逆
///
/// 返回求逆后的新的矩阵
public GMatrix Inverse()
{
//最后原始矩阵并不变，所以需要深拷贝一份
GMatrix _thisCopy = this.DeepCopy();
if (cols == rows && this.Determinant() != 0)
{
//初始化一个同等大小的单位阵
GMatrix res = _thisCopy.EGMatrix();
for (int i = 0; i < rows; i++)
{
//首先找到第i列的绝对值最大的数，并将该行和第i行互换
int rowMax = i;
double max = Math.Abs(_thisCopy[i, i]);
for (int j = i; j < rows; j++)
{
if (Math.Abs(_thisCopy[j, i]) > max)
{
rowMax = j;
max = Math.Abs(_thisCopy[j, i]);
}
}
//将第i行和找到最大数那一行rowMax交换
if (rowMax != i)
{
_thisCopy.Exchange(i, rowMax);
res.Exchange(i, rowMax);
}
//将第i行做初等行变换，将第一个非0元素化为1
double r = 1.0 / _thisCopy[i, i];
_thisCopy.Exchange(i, -1, r);
res.Exchange(i, -1, r);
//消元
for (int j = 0; j < rows; j++)
{
//到本行后跳过
if (j == i)
continue;
else
{
r = -_thisCopy[j, i];
_thisCopy.Exchange(i, j, r);
res.Exchange(i, j, r);
}
}
}
return res;
}
else
{
throw new Exception("矩阵不是方阵无法求逆");
}
}
#region 重载比较运算符
public static bool operator <(GMatrix a, GMatrix b)
{
bool issmall = true;
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
if (a[i, j] >= b[i, j]) issmall = false;
}
}
return issmall;
}
public static bool operator >(GMatrix a, GMatrix b)
{
bool issmall = true;
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
if (a[i, j] <= b[i, j]) issmall = false;
}
}
return issmall;
}
public static bool operator <=(GMatrix a, GMatrix b)
{
bool issmall = true;
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
if (a[i, j] > b[i, j]) issmall = false;
}
}
return issmall;
}
public static bool operator >=(GMatrix a, GMatrix b)
{
bool issmall = true;
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
if (a[i, j] < b[i, j]) issmall = false;
}
}
return issmall;
}
public static bool operator !=(GMatrix a, GMatrix b)
{
bool issmall = true;
issmall = ReferenceEquals(a, b);
if (issmall) return issmall;
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
if (a[i, j] == b[i, j]) issmall = false;
}
}
return issmall;
}
public static bool operator ==(GMatrix a, GMatrix b)
{
bool issmall = true;
issmall = ReferenceEquals(a, b);
if (issmall) return issmall;
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
if (a[i, j] != b[i, j]) issmall = false;
}
}
return issmall;
}
public override bool Equals(object obj)
{
GMatrix b = obj as GMatrix;
return this == b;
}
public override int GetHashCode()
{
return base.GetHashCode();
}
#endregion
public double Determinant()
{
if (cols == rows)
{
GMatrix _thisCopy = this.DeepCopy();
//行列式每次交换行，都需要乘以-1
double res = 1;
for (int i = 0; i < rows; i++)
{
//首先找到第i列的绝对值最大的数
int rowMax = i;
double max = Math.Abs(_thisCopy[i, i]);
for (int j = i; j < rows; j++)
{
if (Math.Abs(_thisCopy[j, i]) > max)
{
rowMax = j;
max = Math.Abs(_thisCopy[j, i]);
}
}
//将第i行和找到最大数那一行rowMax交换,同时将单位阵做相同初等变换
if (rowMax != i)
{
_thisCopy.Exchange(i, rowMax);
res *= -1;
}
//消元
for (int j = i + 1; j < rows; j++)
{
double r = -_thisCopy[j, i] / _thisCopy[i, i];
_thisCopy.Exchange(i, j, r);
}
}
//计算对角线乘积
for (int i = 0; i < rows; i++)
{
res *= _thisCopy[i, i];
}
return res;
}
else
{
throw new Exception("不是行列式");
}
}
#endregion
#region 初等变换
///
/// 初等变换：交换第r1和第r2行
///
/// 第r1行
/// 第r2行
/// 返回交换两行后的新的矩阵
public GMatrix Exchange(int r1, int r2)
{
if (Math.Min(r2, r1) >= 0 && Math.Max(r1, r2) < rows)
{
for (int j = 0; j < cols; j++)
{
double temp = this[r1, j];
this[r1, j] = this[r2, j];
this[r2, j] = temp;
}
return this;
}
else
{
throw new Exception("超出索引");
}
}
///
/// 初等变换：将r1行乘以某个数加到r2行
///
/// 第r1行乘以num
/// 加到第r2行，若第r2行为负，则直接将r1乘以num并返回
/// 某行放大的倍数
///
/// 得到一个同等大小的单位矩阵
///
/// 返回一个同等大小的单位矩阵
public GMatrix EGMatrix()
{
if (rows == cols)
{
double[,] res = new double[rows, cols];
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < cols; j++)
{
if (i == j)
res[i, j] = 1;
else
res[i, j] = 0;
}
}
return new GMatrix(res);
}
else
throw new Exception("不是方阵，无法得到单位矩阵");
}
#endregion
///
/// 深拷贝，仅仅将值拷贝给一个新的对象
///
/// 返回深拷贝后的新对象
public GMatrix DeepCopy()
{
double[,] ele = new double[rows, cols];
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < cols; j++)
{
ele[i, j] = this[i, j];
}
}
return new GMatrix(ele);
}
public override string ToString()
{
string str = "";
for (int i = 0; i < Rows; i++)
{
for (int j = 0; j < Cols; j++)
{
str += this[i, j].ToString();
if (j != Cols - 1)
str += " ";
else if (i != Rows - 1)
str += Environment.NewLine;
}
}
return str;
}
}
}