坐标转换四参数/七参数/正形变换 ∈ C# 编程笔记

更新日期：2020/4/7

文章目录

2、平面坐标转换四参数/正形变换
- 2.1 直接法
- 2.2 平差法
- 2.3 正形变换法
- 附简化版正形变换法代码
- 2.4 七参数坐标转换

【注1】其中的代码也许并不完整，您可以作为伪码参看，或者您可以去我主博客逛逛，也许有意外之喜！
【注2】此篇博客是 C# 编程笔记 的子博客。

上一篇：-- 回到主博客下一篇：曲线道路坐标计算

2、平面坐标转换四参数/正形变换

最近（2020/4/2）总结四参数坐标转换的时候，发现了一个问题，就是在之前我大二的时候编写平面四参数坐标转换直接用到了一个公式：

看之前的实习报告也没有说明公式的由来，然后我就开始查阅资料，终于有了点苗头，在这里记录一下。
从最原始开始…

如上图所示，（x,y）是源坐标，（X,Y）是目标坐标，注意，这里旋转角是负的(顺正逆负)，转换公式为：

为了得到这四个参数，我们有两种方法：一种是只知道两对点，用直接参数法求解；另一种是知道不只两对点，我们可以先用两对点求近似值，然后用迭代，前后两次迭代结果小于限差就跳出循环。

2.1 直接法

2.2 平差法

利用两个点（最好是可以控制全局的两个点）用上面的直接求参的方法求得近似值后，再用下面的公式利用源坐标（x,y）计算出目标坐标的近似值（X’,Y’）。不妨将刚才求出来的近似值记作：

将真值记作：

毕竟我们用的是所求参数的近似值，所以必然会存在偏差，但是这个偏差又非常之微小，所以我们可以在

处进行泰勒展开；要想继续进行下去，您首先需要了解泰勒展开：

代码实现：
注【1】：如果您直接复制粘贴这些代码的话也许会发现会缺失函数。这是正常情况，因为有些函数是我编写的类里面的，都粘贴下来的话会十分繁杂。所以您可以当做伪码来看。
注【2】：用此方法也许会出现永远跳不出来循环的情况(我用实验数据尝试过，没有成功)我觉得原因是：1）我设置的限差太过苛刻；2）所用数据本身测量误差很大。

     /*此函数用于求象限角*/ public static double arctan(double dx, double dy){double angle = 0;if(dx==0){if(dy>0) angle=Math.PI*0.5;else     angle=Math.PI*1.5;return angle;}angle = Math.Atan2(Math.Abs(dy), Math.Abs(dx));if (dx < 0){if (dy < 0) angle = Math.PI + angle;else        angle = Math.PI - angle;}else if (dy < 0) angle = 2 * Math.PI - angle;return angle;}/*此函数用于求四参数的近似值,* 输入：po=源坐标,pt=目标坐标* 输出：* 0 尺度因子* 1 旋转角度* 2 x平移参数* 3 y平移参数*/ public static double[] Get_Four_JS(Point po1, Point po2, Point pt1, Point pt2){double[] JS = new double[4];double dx = po1.x - po2.x, dy = po1.y - po2.y;double dX = pt1.x - pt2.x, dY = pt1.y - pt2.y;double s = Math.Sqrt(dx * dx + dy * dy);double S = Math.Sqrt(dX * dX + dY * dY);double A = arctan(dX, dY);double a = arctan(dx, dy);JS[0] = s / S;JS[1] = A - a;JS[2] = pt1.x - po1.x;JS[3] = pt1.y - po1.y;return JS;}/* 此函数用于求取平面四参数 用到坐标重心化* 输入：po是源坐标,pt是目标坐标* 输出：resultMat是结果矩阵数组*     第一个矩阵 X      是所求的四参数*     第二个矩阵 V      是改正数,可以算内符合精度*     第三个矩阵 Sigma2 是验后单位权中误差的平方*/public static Matrix[] Get_Four(Point[] po1, Point[] pt1){int n = po1.Length;int r = 2 * n - 4;//多余观测数Point[] po = Point.ZXH(po1);//坐标重心化Point[] pt = Point.ZXH(pt1);double[] JS = Get_Four_JS(po[0], po[n - 1], pt[0], pt[n - 1]);//求近似double Dx = JS[2] ,Dy = JS[3],m = JS[0],Sita = JS[1];Matrix[] resultMat = new Matrix[3];Matrix B = new Matrix(2 * n, 4); ;Matrix P = new Matrix(2 * n); P.MakeUnitMatrix(2 * n);//默认权矩阵为单位阵Matrix L = new Matrix(2 * n, 1);Matrix X1 = new Matrix(4, 1);//参数while (true){for (int i = 0; i < n; i++){//根据近似参数求观测值double a = Math.Cos(Sita) * po[i].x + Math.Sin(Sita) * po[i].y,b = -Math.Sin(Sita) * po[i].x + Math.Cos(Sita) * po[i].y,X = Dx + a * m,Y = Dy - b * m;B[2 * i, 0] = 1;B[2 * i, 1] = 0;B[2 * i, 2] = a;B[2 * i, 3] = b * m;B[2 * i + 1, 0] = 0;B[2 * i + 1, 1] = 1;B[2 * i + 1, 2] = b;B[2 * i + 1, 3] = -a * m;L[2 * i, 0] = pt[i].x - X;L[2 * i + 1, 0] = pt[i].y - Y;}Matrix Nbb = B.Transpose() * P * B;Nbb.InvertGaussJordan();X1 = Nbb * B.Transpose() * P * L;X1.Eps = 1e-6;if (X1.Equals(0))break;Dx += X1[0, 0];Dy += X1[1, 0];m += X1[2, 0];Sita += X1[3, 0];}           Matrix V = B * X1 - L;Matrix Sigma2 = V.Transpose() * P * V;//验后单位权中误差Sigma2[0, 0] /= r;resultMat[0] = X1;resultMat[1] = V;resultMat[2] = Sigma2;return resultMat;}

2.3 正形变换法

正形变换的公式就比较复杂了，好像还考虑到了投影变形等因素，具体的原理目前还不是特别明晰。先把公式列出来：

代码实现：

     /*此函数用于求d的n次方,因为用Math.Pow()太累赘*/ public static double P(double d, double n){return Math.Pow(d, n);}/* 此函数用于求取平面四参数 未用到坐标重心化,实际上是舍去了正形变换的高阶项* 输入：po是源坐标,pt是目标坐标,可选参数n 取前n(>=2)点平差* 输出：resultMat是结果矩阵数组*     第一个矩阵 X      是所求的四参数*     第二个矩阵 V      是改正数,可以算内符合精度*     第三个矩阵 Sigma2 是验后单位权中误差的平方*/public static Matrix[] Get_Four1(Point[] po, Point[] pt,int n=0){if(n==0) n = po.Length;int r = 2 * n - 4;//多余观测数Matrix[] resultMat = new Matrix[3];Matrix B = new Matrix(2 * n, 4); ;Matrix P = new Matrix(2 * n); P.MakeUnitMatrix(2 * n);//默认权矩阵为单位阵Matrix L = new Matrix(2 * n, 1);Matrix X = new Matrix(4, 1);//参数for (int i = 0; i < n; i++){double x = po[i].x, y = po[i].y;double x1 = pt[i].x, y1 = pt[i].y;B[2 * i, 0] = 1;B[2 * i, 1] = 0;B[2 * i, 2] = x;B[2 * i, 3] = -y;B[2 * i + 1, 0] = 0;B[2 * i + 1, 1] = 1;B[2 * i + 1, 2] = y;B[2 * i + 1, 3] = x;L[2 * i, 0] = x1;L[2 * i + 1, 0] = y1;}Matrix Nbb = B.Transpose() * P * B;Nbb.InvertGaussJordan();X = Nbb * B.Transpose() * P * L;Matrix V = B * X - L;Matrix Sigma2 = V.Transpose() * P * V;//验后单位权中误差if (r != 0) Sigma2[0, 0] /= r;else Sigma2[0, 0] = 0;resultMat[0] = X;resultMat[1] = V;resultMat[2] = Sigma2;return resultMat;}/* 此函数用正形变换求取平面四参数 用到坐标重心化* 输入：po是源坐标,pt是目标坐标* 输出：resultMat是结果矩阵数组*     第一个矩阵 X      是所求的四参数*     第二个矩阵 V      是改正数,可以算内符合精度*     第三个矩阵 Sigma2 是验后单位权中误差的平方*      第四个矩阵 ZX     是源/目重心[po.x,po,y,pt.x,pt.y]*/public static Matrix[] Get_Four_ZX(Point[] po1, Point[] pt1){int n = pt1.Length;Point[] po = Point.ZXH(po1);//坐标重心化Point[] pt = Point.ZXH(pt1);int r = 2 * n - 10;//冗余度Matrix[] resultMat = new Matrix[4];Matrix[] JS = Get_Four1(po, pt, 2); Matrix C = JS[0];double p0 = C[0, 0],//前四个参数的近似值q0 = C[1, 0],p1 = C[2, 0],q1 = C[3, 0];Matrix B = new Matrix(2 * n, 10);Matrix L = new Matrix(2 * n, 1);for (int i = 0; i < n; i++){double x = po[i].x, y = po[i].y;double x1 = pt[i].x, y1 = pt[i].y;B[2 * i, 0] = 1;B[2 * i, 1] = 0;B[2 * i, 2] = x;B[2 * i, 3] = -y;B[2 * i, 4] = P(x, 2) - P(y, 2);B[2 * i, 5] = -2 * x * y;B[2 * i, 6] = P(x, 3) - 3 * x * P(y, 2);B[2 * i, 7] = P(y, 3) - 3 * P(x, 2) * y;B[2 * i, 8] = P(x, 4) - 6 * P(x, 2) * P(y, 2) + P(y, 4);B[2 * i, 9] = 4 * x * P(y, 3) - 4 * P(x, 3) * y;B[2 * i + 1, 0] = 0;B[2 * i + 1, 1] = 1;B[2 * i + 1, 2] = y;B[2 * i + 1, 3] = x;B[2 * i + 1, 4] = 2 * x * y;B[2 * i + 1, 5] = P(x, 2) - P(y, 2);B[2 * i + 1, 6] = 3 * P(x, 2) * y - P(y, 3);B[2 * i + 1, 7] = P(x, 3) - 3 * x * P(y, 2);B[2 * i + 1, 8] = 4 * P(x, 3) * y - 4 * x * P(y, 3);B[2 * i + 1, 9] = P(x, 4) - 6 * P(x, 2) * P(y, 2) + P(y, 4);L[2 * i, 0] = x1 - p0 - x * p1 + y * q1;L[2 * i + 1, 0] = y1 - q0 - y * p1 - x * q1;}Matrix Nbb = B.Transpose() * B;Nbb.InvertGaussJordan();Matrix X = Nbb * B.Transpose() * L;Matrix Xp = X;while (true){p0 += X[0, 0];q0 += X[1, 0];p1 += X[2, 0];q1 += X[3, 0];for (int i = 0; i < n; i++){double x = po[i].x, y = po[i].y;double x1 = pt[i].x, y1 = pt[i].y;L[2 * i, 0] = x1 - p0 - x * p1 + y * q1;L[2 * i + 1, 0] = y1 - q0 - y * p1 - x * q1;}X = Nbb * B.Transpose() * L;X.Eps = 1e-6;if (X.Equals(Xp)) break;Xp = X;}Matrix V = B * X - L;X[0,0]+=p0;X[1,0]+=q0;X[2,0]+=p1;X[3,0]+=q1;Matrix ZX=new Matrix(4,1);//重心矩阵ZX[0,0]=po[n].x; ZX[0,1]=po[n].y;ZX[0,2]=pt[n].x; ZX[0,3]=pt[n].y;Matrix Sigma2 = V.Transpose() * V;//延后单位权中误差if (r != 0) Sigma2[0, 0] /= r;else Sigma2[0, 0] = 0;resultMat[0] = X;resultMat[1] = V;resultMat[2] = Sigma2;resultMat[3] = ZX;return resultMat;}

从源坐标转换为目标坐标的代码：

         /* 此函数用于平面坐标转换，对应函数为 Get_Four_ZX* 输入：po是源坐标; M 是用正形参数求取函数得到的矩阵数组* 输出：pt是目标坐标*/public static Point[] zbTrans_ZX(Point[] po, Matrix[] M){int n=po.Length;Point[] pt = new Point[n];Matrix X = M[0];Matrix ZX=M[3];Matrix B = new Matrix(2, 10);Matrix Pt;Point po_ZX = new Point(ZX[0, 0], ZX[1, 0]);Point pt_ZX = new Point(ZX[2, 0], ZX[3, 0]);for (int j = 0; j < n; j++){int i = 0;double x = po[j].x - po_ZX.x, y = po[j].y - po_ZX.y;B[2 * i, 0] = 1;B[2 * i, 1] = 0;B[2 * i, 2] = x;B[2 * i, 3] = -y;B[2 * i, 4] = P(x, 2) - P(y, 2);B[2 * i, 5] = -2 * x * y;B[2 * i, 6] = P(x, 3) - 3 * x * P(y, 2);B[2 * i, 7] = P(y, 3) - 3 * P(x, 2) * y;B[2 * i, 8] = P(x, 4) - 6 * P(x, 2) * P(y, 2) + P(y, 4);B[2 * i, 9] = 4 * x * P(y, 3) - 4 * P(x, 3) * y;B[2 * i + 1, 0] = 0;B[2 * i + 1, 1] = 1;B[2 * i + 1, 2] = y;B[2 * i + 1, 3] = x;B[2 * i + 1, 4] = 2 * x * y;B[2 * i + 1, 5] = P(x, 2) - P(y, 2);B[2 * i + 1, 6] = 3 * P(x, 2) * y - P(y, 3);B[2 * i + 1, 7] = P(x, 3) - 3 * x * P(y, 2);B[2 * i + 1, 8] = 4 * P(x, 3) * y - 4 * x * P(y, 3);B[2 * i + 1, 9] = P(x, 4) - 6 * P(x, 2) * P(y, 2) + P(y, 4);Pt = B * X;pt[j] = new Point(Pt[0, 0]+pt_ZX.x, Pt[1, 0]+pt_ZX.y);}return pt;}

附简化版正形变换法代码

至此，终于搞清楚了刚开始的公式由来------简化版的正形变换
其实现代码上面有，但为了以后方便复制，下面再列一遍：

         /* 此函数用于求取平面四参数 未用到坐标重心化,实际上是舍去了正形变换的高阶项* 输入：po是源坐标,pt是目标坐标,可选参数n 取前n(>=2)点平差* 输出：resultMat是结果矩阵数组*     第一个矩阵 X      是所求的四参数*     第二个矩阵 V      是改正数,可以算内符合精度*     第三个矩阵 Sigma2 是验后单位权中误差的平方*/public static Matrix[] Get_Four1(Point[] po, Point[] pt,int n=0){if(n==0) n = po.Length;int r = 2 * n - 4;//多余观测数Matrix[] resultMat = new Matrix[3];Matrix B = new Matrix(2 * n, 4); ;Matrix P = new Matrix(2 * n); P.MakeUnitMatrix(2 * n);//默认权矩阵为单位阵Matrix L = new Matrix(2 * n, 1);Matrix X = new Matrix(4, 1);//参数for (int i = 0; i < n; i++){double x = po[i].x, y = po[i].y;double x1 = pt[i].x, y1 = pt[i].y;B[2 * i, 0] = 1;B[2 * i, 1] = 0;B[2 * i, 2] = x;B[2 * i, 3] = -y;B[2 * i + 1, 0] = 0;B[2 * i + 1, 1] = 1;B[2 * i + 1, 2] = y;B[2 * i + 1, 3] = x;L[2 * i, 0] = x1;L[2 * i + 1, 0] = y1;}Matrix Nbb = B.Transpose() * P * B;Nbb.InvertGaussJordan();X = Nbb * B.Transpose() * P * L;Matrix V = B * X - L;Matrix Sigma2 = V.Transpose() * P * V;//验后单位权中误差if (r != 0) Sigma2[0, 0] /= r;else Sigma2[0, 0] = 0;resultMat[0] = X;resultMat[1] = V;resultMat[2] = Sigma2;return resultMat;}

从源坐标转换为目标坐标的代码：

         /* 此函数用于四参数坐标转换，对应函数为 Get_Four1* 输入：po是源坐标; M 是用四参数求取函数得到的矩阵数组* 输出：pt是目标坐标*/public static Point[] zbTrans4(Point[] po, Matrix[] M){int n=po.Length;Point[] pt = new Point[n];Matrix X = M[0];Matrix B = new Matrix(2, 4);Matrix Pt;for (int i = 0; i < n; i++){double x = po[i].x, y = po[i].y;B[0, 0] = 1;B[0, 1] = 0;B[0, 2] = x;B[0, 3] = -y;B[1, 0] = 0;B[1, 1] = 1;B[1, 2] = y;B[1, 3] = x;Pt = B * X;pt[i] = new Point(Pt[0, 0], Pt[1, 0]);}return pt;}

总结：若精度要求高则用正形变换，精度要求低就用简化版的正形变换或直接法。（aiyamaya,累死我了2020/4/2 22:35）

2.4 七参数坐标转换

七参数坐标用于求取空间坐标系1到空间坐标系2之间的转换关系，具体的公式如下所示

公式中XYZ 是目标坐标系中的坐标，xyz 是源坐标系中的坐标，DX DY DZ 是三个平移参量，RX RY RZ 是三个旋转参数，DK 是尺度因子。
注意：上述公式只适用于小角度转换，好像是叫布尔沙模型。实际上，理论上严格推导出来的公式不是如此，此公式进行了一定的化简，所求参数也并不是真正的旋转角。由于时间有限，这里暂且不推导，以后有时间在来完善。
接下来平差就开始表演：
上述方程视为观测方程，误差方程如下：

为待求转换参量。
代码实现：

      /* 此函数用于求取空间面七参数 未用到坐标重心化* 输入：po是源坐标,pt是目标坐标* 输出：resultMat是结果矩阵数组*     第一个矩阵 X      是所求的七参数*     第二个矩阵 V      是改正数,可以算内符合精度*     第三个矩阵 Sigma2 是验后单位权中误差的平方*/public static Matrix[] Get_Seven(Point[] po, Point[] pt){int n = po.Length;int r = 3 * n - 7;//多余观测数Matrix[] resultMat = new Matrix[3];Matrix B = new Matrix(3 * n, 7);Matrix L = new Matrix(3 * n, 1);for (int i = 0; i < n; i++){B[3 * i, 0] = 1;B[3 * i, 1] = 0;B[3 * i, 2] = 0;B[3 * i, 3] = 0;B[3 * i, 4] = -po[i].z;B[3 * i, 5] = po[i].y;B[3 * i, 6] = po[i].x;B[3 * i + 1, 0] = 0;B[3 * i + 1, 1] = 1;B[3 * i + 1, 2] = 0;B[3 * i + 1, 3] = po[i].z;B[3 * i + 1, 4] = 0;B[3 * i + 1, 5] = -po[i].x;B[3 * i + 1, 6] = po[i].y;B[3 * i + 2, 0] = 0;B[3 * i + 2, 1] = 0;B[3 * i + 2, 2] = 1;B[3 * i + 2, 3] = -po[i].y;B[3 * i + 2, 4] = po[i].x;B[3 * i + 2, 5] = 0;B[3 * i + 2, 6] = po[i].z;L[3 * i, 0] = pt[i].x;L[3 * i + 1, 0] = pt[i].y;L[3 * i + 2, 0] = pt[i].z;}Matrix Nbb = B.Transpose() * B;Nbb.InvertGaussJordan();Matrix X = Nbb * B.Transpose() * L;Matrix V = B * X - L;Matrix Sigma2 = V.Transpose() * V;//验后单位权中误差resultMat[0] = X;resultMat[1] = V;resultMat[2] = Sigma2;return resultMat;            }

从源坐标转换为目标坐标的代码：

      /* 此函数用于七参数坐标转换，对应函数为 Get_Seven* 输入：po是源坐标; M 是用七参数求取函数得到的矩阵数组* 输出：pt是目标坐标*/public static Point[] zbTrans7(Point[] po, Matrix[] M){int n=po.Length;Point[] pt = new Point[n];Matrix X = M[0];Matrix B = new Matrix(3, 7);Matrix Pt;for (int i = 0; i < n; i++){B[0, 0] = 1;B[0, 1] = 0;B[0, 2] = 0;B[0, 3] = 0;B[0, 4] = -po[i].z;B[0, 5] = po[i].y;B[0, 6] = po[i].x;B[0 + 1, 0] = 0;B[0 + 1, 1] = 1;B[1, 2] = 0;B[1, 3] = po[i].z;B[1, 4] = 0;B[1, 5] = -po[i].x;B[1, 6] = po[i].y;B[2, 0] = 0;B[2, 1] = 0;B[2, 2] = 1;B[2, 3] = -po[i].y;B[2, 4] = po[i].x;B[2, 5] = 0;B[2, 6] = po[i].z;Pt = B * X;pt[i] = new Point(Pt[0, 0], Pt[1, 0], Pt[2, 0]);}return pt;}

其所依附的类有：

点类：https://blog.csdn.net/Gou_Hailong/article/details/88989274
矩阵类：https://blog.csdn.net/Gou_Hailong/article/details/98451032

上一篇：-- 回到主博客下一篇：曲线道路坐标计算

【注1】其中的代码也许并不完整，您可以作为伪码参看，或者您可以去我主博客逛逛，也许有意外之喜！
【注2】此篇博客是 C# 编程笔记 的子博客。