大角度非迭代的空間座標旋轉C#實現

Aidan_Lee發表於2022-12-16

1. 緒論

在前面文章中提到空間直角座標系相互轉換，測繪座標轉換時，一般涉及到的情況是：兩個直角座標系的小角度轉換。這個就是我們經常在測繪資料處理中，WGS-84座標系、54北京座標系、80西安座標系、國家2000座標系之間的轉換。

所謂小角度轉換，指直角座標系\(XOY\)和直角座標系\(X'O'Y'\)之間，對應軸的旋轉角度很小，滿足泰勒級數展開後的線性模型。

1671185879422

常見的三維座標轉換模型有^[1]：

布林沙模型
莫洛琴斯基模型
正規化模型

但，當兩個座標系對應軸的旋轉角度大道一定程度時，則無法使用低階的泰勒級數展開，且迭代的計算量、精度、速度無法取得平衡^[2]。存在以下缺點：

僅適用於滿足近似處理的小角度轉換
設計複雜的三角函式運算
需要迭代計算

羅德里格矩陣是攝影測量中的常見方法，在該方法中，不需要進行三角函式的計算和迭代運算。計算過程簡單明瞭，易於程式設計實現。不僅適用於小角度的座標轉換，也適用於大角度的空間座標轉換。

本文將介紹羅德里格矩陣的基本原理和C#實現，並用例項證明解算的有效性。

2. 羅德里格矩陣座標轉換原理

2.1 座標轉換基本矩陣

兩個空間直角座標系分別為\(XOY\)和\(X'O'Y'\)，座標系原點不一致，存在三個平移引數\(\Delta X\)、\(\Delta Y\)、\(\Delta Z\)。它們間的座標軸也相互不平行，存在三個旋轉引數\(\epsilon x\)、\(\epsilon y\)、\(\epsilon z\)。同一點A在兩個座標系中的座標分別為\((X,Y,Z)\)和\((X',Y',Z')\)。

顯然，這兩個座標系透過座標軸的平移和旋轉變換可取得，座標間的轉換關係如下:

\[\left[\begin{array}{l} X \\ Y \\ Z \end{array}\right]=\lambda R\left[\begin{array}{l} X^{\prime} \\ Y^{\prime} \\ Z^{\prime} \end{array}\right]+\left[\begin{array}{l} \Delta X \\ \Delta Y \\ \Delta Z \end{array}\right] \tag{1} \]

其中，\(\lambda\)是比例因子，\(R\left(\varepsilon_Y\right) R\left(\varepsilon_X\right) R\left(\varepsilon_Z\right)\)分別是繞Y軸，X軸，Z軸的旋轉矩陣。注意，旋轉的順序不同，\(R\) 的表達形式不同。

\[\begin{aligned} R & =R\left(\varepsilon_Y\right) R\left(\varepsilon_X\right) R\left(\varepsilon_Z\right) \\ & =\left[\begin{array}{ccc} \cos \varepsilon_Y \cos \varepsilon_Z-\sin \varepsilon_Y \sin \varepsilon_X \sin \varepsilon_Z & -\cos \varepsilon_Y \sin \varepsilon_Z-\sin \varepsilon_Y \sin \varepsilon_X \cos \varepsilon_Z & -\sin \varepsilon_Y \cos \varepsilon_X \\ \cos \varepsilon_X \sin \varepsilon_Z & \cos \varepsilon_X \cos \varepsilon_Z & -\sin \varepsilon_X \\ \sin \varepsilon_Y \cos \varepsilon_Z+\cos \varepsilon_Y \sin \varepsilon_X \sin \varepsilon_Z & -\sin \varepsilon_Y \sin \varepsilon_Z+\cos \varepsilon_Y \sin \varepsilon_X \cos \varepsilon_Z & \cos \varepsilon_Y \cos \varepsilon_X \end{array}\right] \end{aligned} \]

習慣上稱\(R\)為旋轉矩陣，\([\Delta X,\Delta Y,\Delta Z]^T\)為平移矩陣。只要求出\(\Delta X\)、\(\Delta Y\) 、\(\Delta Z\)，\(\varepsilon_X\)、\(\varepsilon_Y\)、\(\varepsilon_Z\)，這7個轉換引數,或者直接求出旋轉矩陣和平移矩陣,就可以實現兩個座標系間的轉換。

2.2 計算技巧-重心矩陣

為計算方便，對所用到的座標進行重心化處理。將兩個座標系的公共點的座標均化算為以重心為原點的重心化座標。分別記為\((\bar{X}, \bar{Y}, \bar{Z})\)和\(\left(\bar{X}^{\prime}, \bar{Y}^{\prime}, \bar{Z}^{\prime}\right)\)兩個座標系的重心的座標分別為\((X_g, Y_g, Z_g)\)和\((X'_g, Y'_g, Z'_g)\)。

\[\left\{\begin{array}{l} X_k=\frac{\sum_{i=1}^n X_i}{n}, Y_k=\frac{\sum_{i=1}^n Y_i}{n}, Z_k=\frac{\sum_{i=1}^n Z_i}{n} \\ X_k^{\prime}=\frac{\sum_{i=1}^n X_i^{\prime}}{n}, Y_k^{\prime}=\frac{\sum_{i=1}^n Y_i^{\prime}}{n}, Z_k^{\prime}=\frac{\sum_{i=1}^n Z_i^{\prime}}{n} \\ \bar{X}_i=X_i-X_k, \bar{Y}_i=Y_i-Y_k, \bar{Z}_i=Z_i-Z_k \\ \bar{X}_i^{\prime}=X_i^{\prime}-X_k^{\prime}, \bar{Y}_i^{\prime}=Y_i^{\prime}-Y_k^{\prime}, \bar{Z}_i^{\prime}=Z_i^{\prime}-Z_k^{\prime} \end{array}\right. \]

因此，可以將式(1)變為：

\[\left[\begin{array}{l} \bar{X} \\ \bar{Y} \\ \bar{Z} \end{array}\right]=\lambda R\left[\begin{array}{l} \bar{X}^{\prime} \\ \bar{Y}^{\prime} \\ \bar{Z}^{\prime} \end{array}\right] \tag{2} \]

\[\left[\begin{array}{l} \Delta X \\ \Delta Y \\ \Delta Z \end{array}\right]=\left[\begin{array}{l} X_g \\ Y_g \\ Z_g \end{array}\right]-\lambda R\left[\begin{array}{l} X_g^{\prime} \\ Y_g^{\prime} \\ Z_g^{\prime} \end{array}\right] \tag{3} \]

因而，轉換引數可分兩步來求解。先用式(2)求出旋轉引數和比例因子，再用式(,3)求出平移引數。

2.3 基於羅德里格斯矩陣的轉換方法

對式(2)兩邊取2-範數，由於\(\lambda > 0\)，旋轉矩陣為正交陣的特性，可得:

\[\Vert [\bar{X}, \bar{Y}, \bar{Z}]^T \Vert = \lambda \Vert [\bar{X'}, \bar{Y'}, \bar{Z'}]^T \Vert \tag{4} \]

對於n個公共點，可得\(\lambda\)的最小均方估計:

\[\lambda=\frac{\sum_{i=1}^n\left(\left\|\left[\bar{X}_i \bar{Y}_i \bar{Z}_i\right]^{\mathrm{T}}\right\| \cdot\left\|\left[\bar{X}_i^{\prime} \bar{Y}_i^{\prime} \bar{Z}_i^{\prime}\right]^{\mathrm{T}}\right\|\right)}{\sum_i^n\left(\left\|\left[\bar{X}_{\prime}^{\prime} \bar{Y}_i^{\prime} \bar{Z}_i^{\prime}\right]^{\mathrm{T}}\right\|\right)^2} \]

得到比例因子的最小均方估計後，可將旋轉矩陣 \(R\) 表示為:

\[R=(I-S)^{-1} (I+S) \tag{5} \]

其中，\(I\)為單位矩陣，\(S\)為反對稱矩陣。將式(5)帶入式(3)，可得：

\[\left[\begin{array}{c} \bar{X}-\lambda \bar{X}^{\prime} \\ \bar{Y}-\lambda \bar{Y}^{\prime} \\ \bar{Z}-\lambda \bar{Z}^{\prime} \end{array}\right]=\left[\begin{array}{ccc} 0 & -\left(\bar{Z}+\lambda \bar{Z}^{\prime}\right) & -\left(\bar{Y}+\lambda \bar{Y}^{\prime}\right) \\ -\left(\bar{Z}+\lambda \bar{Z}^{\prime}\right) & 0 & \bar{X}+\lambda \bar{X}^{\prime} \\ \bar{Y}+\lambda \bar{Y}^{\prime} & \bar{X}+\lambda \bar{X}^{\prime} & 0 \end{array}\right]\left[\begin{array}{l} a \\ b \\ c \end{array}\right] \tag{6} \]

3. C#程式碼實現

矩陣運算使用MathNet.Numerics庫，初始化欄位MatrixBuilder<double> mb = Matrix<double>.Build和VectorBuilder<double> vb = Vector<double>.Build

3.1 計算矩陣重心座標

Vector<double> BarycentricCoord(Matrix<double> coordinate)
{
    Vector<double> barycentric = vb.Dense(3, 1);

    int lenCoord = coordinate.ColumnCount;

    if (lenCoord > 2)
        barycentric = coordinate.RowSums();

    barycentric /= lenCoord;

    return barycentric;
}

3.2 計算比例因子

取2-範數使用點乘函式PointwisePower(2.0)：

double ScaleFactor(Matrix<double> sourceCoord, Matrix<double> targetCoord)
{
    double k = 0;

    double s1 = 0;
    double s2 = 0;

    Vector<double> sourceColL2Norm = sourceCoord.PointwisePower(2.0).ColumnSums();

    Vector<double> targetColL2Norm = targetCoord.PointwisePower(2.0).ColumnSums();

    int lenSourceCoord = sourceCoord.ColumnCount;

    int lenTargetCoord = targetCoord.ColumnCount;

    //只有在目標矩陣和源矩陣大小一致時，才能計算
    if (lenSourceCoord == lenTargetCoord)
    {
        s1 = sourceColL2Norm.PointwiseSqrt().PointwiseMultiply(targetColL2Norm.PointwiseSqrt()).Sum();

        s2 = sourceColL2Norm.Sum();
    }

    k = s1 / s2;
    return k;
}

3.3 計算羅德里格引數

這裡的羅德里格引數就是式(6)中的\([a, b, c]^T\)。

Vector<double> RoderickParas(double scalceFactor, Matrix<double> sourceCoord, Matrix<double> targetCoord)
{
    Vector<double> roderick = vb.Dense(new double[] { 0, 0, 0 });

    int lenData = sourceCoord.ColumnCount;

    //常係數矩陣
    var lConstant = vb.Dense(new double[3 * lenData]);

    //係數矩陣
    var coefficient = mb.DenseOfArray(new double[3 * lenData, 3]);

    //構造相應矩陣 
    for (int i = 0; i < lenData; i++)
    {
        lConstant[3 * i] = targetCoord[0, i] - scalceFactor * sourceCoord[0, i];
        lConstant[3 * i + 1] = targetCoord[1, i] - scalceFactor * sourceCoord[1, i];
        lConstant[3 * i + 2] = targetCoord[2, i] - scalceFactor * sourceCoord[2, i];

        coefficient[3 * i, 0] = 0;
        coefficient[3 * i, 1] = -(targetCoord[2, i] + scalceFactor * sourceCoord[2, i]);
        coefficient[3 * i, 2] = -(targetCoord[1, i] + scalceFactor * sourceCoord[1, i]);
        coefficient[3 * i + 1, 0] = -(targetCoord[2, i] + scalceFactor * sourceCoord[2, i]);
        coefficient[3 * i + 1, 1] = 0;
        coefficient[3 * i + 1, 2] = targetCoord[0, i] + scalceFactor * sourceCoord[0, i];
        coefficient[3 * i + 2, 0] = targetCoord[1, i] + scalceFactor * sourceCoord[1, i];
        coefficient[3 * i + 2, 1] = targetCoord[0, i] + scalceFactor * sourceCoord[0, i];
        coefficient[3 * i + 2, 2] = 0;

    }


    roderick = coefficient.TransposeThisAndMultiply(coefficient).Inverse() * coefficient.Transpose() * lConstant;

    return roderick;
}

3.4 解析羅德里格矩陣

此處，就是式(5)的實現。

/// <summary>
/// 解析羅德里格矩陣為旋轉矩陣和平移矩陣
/// </summary>
/// <param name="scaleFactor">比例因子</param>
/// <param name="roderick">羅德里格矩陣</param>
/// <param name="coreSourceCoord">原座標系座標</param>
/// <param name="coreTargetCoord">目標座標系座標</param>
/// <returns></returns>
(Matrix<double>, Vector<double>) RotationMatrix(double scaleFactor, Vector<double> roderick, Vector<double> coreSourceCoord, Vector<double> coreTargetCoord)
{
    Matrix<double> rotation = mb.DenseOfArray(new double[,]
    {
        {0,0,0 },
        {0,0,0 },
        {0,0,0 }
    });
    
    //反對稱矩陣
    Matrix<double> antisymmetric = mb.DenseOfArray(new double[,]
    {
        {          0, -roderick[2], -roderick[1] },
        {roderick[2],            0, -roderick[0] },
        {roderick[1],  roderick[0],            0 }
    });

    // 建立單位矩陣
    // 然後與式(5)的 S 執行 + 和 - 操作
    rotation = (DenseMatrix.CreateIdentity(3) - antisymmetric).Inverse() * (DenseMatrix.CreateIdentity(3) + antisymmetric);

    translation = coreTargetCoord - scaleFactor * rotation * coreSourceCoord;


    return (rotation, translation);
}

3.5 呼叫邏輯

// 1. 欄位值準備
MatrixBuilder<double> mb = Matrix<double>.Build;
VectorBuilder<double> vb = Vector<double>.Build;

// 2. 寫入源座標系的座標。注意這裡的x,y,z輸入順序
Matrix<double> source = mb.DenseOfArray(new double[,]
{
    {-17.968, -12.829, 11.058 },
    {-0.019 , 7.117,   11.001 },
    {0.019  , -7.117,  10.981 }
}).Transpose();

// 3. 寫入目標座標系的座標
Matrix<double> target = mb.DenseOfArray(new double[,]
{
    { 3392088.646,504140.985,17.958 },
    { 3392089.517,504167.820,17.775 },
    { 3392098.729,504156.945,17.751 }
}).Transpose();

// 4. 重心化
var coreSource = BarycentricCoord(source);
var coreTarget = BarycentricCoord(target);

var sourceCoords = source - mb.DenseOfColumnVectors(coreSource, coreSource, coreSource);
var targetCoords = target - mb.DenseOfColumnVectors(coreTarget, coreTarget, coreTarget);

// 5. 求比例因子
double k = ScaleFactor(sourceCoords, targetCoords);

// 6. 解算咯德里格引數
var roderick = RoderickParas(k, sourceCoords, targetCoords);

// 7. 旋轉
(Matrix<double> ro, Vector<double> tran) = RotationMatrix(k, roderick, coreSource, coreTarget);

Console.WriteLine("比例因子為：");
Console.WriteLine(k);

Console.WriteLine("旋轉矩陣為：");
Console.WriteLine(ro.ToString());

Console.WriteLine("平移引數為：");
Console.WriteLine(tran.ToString());

Console.WriteLine("計算結果為：");
Console.WriteLine(source2.ToString());

4. 總結

基於羅德里格矩陣的轉換方法，在求解兩個座標系間的轉換引數，特別是旋轉角較大時，實現簡單、快速。

1671195869776

朱華統,楊元喜,呂志平.GPS座標系統的變換[M].北京:測繪出版社,1994. ↩︎
詹銀虎,鄭勇,駱亞波,等.無需初值及迭代的天文導航新演算法0﹒測繪科學技術學報，2015,32(5):445-449. ↩︎

三維空間座標系變換-旋轉矩陣
2019-10-30
矩陣
三維座標系旋轉——旋轉矩陣到旋轉角之間的換算
2019-04-23
矩陣
JavaScript 空間座標
2020-08-19
JavaScript
視覺化基礎：已知2點座標，如何求旋轉角度？
2022-07-09
視覺化
工程座標轉換方法C#程式碼實現
2022-12-04
C#
vue 實現高德座標轉GPS座標
2022-04-20
Vue
三維座標系的旋轉矩陣
2017-05-31
矩陣
座標曲線的程式設計實現 (轉)
2008-01-27
程式設計
利用三維旋轉矩陣在空間中旋轉平面
2016-03-24
矩陣
CSS3實現的div元素旋轉一定角度效果
2017-02-17
CSSS3
張正友標定Opencv實現、標定流程以及影像座標轉為世界座標
2018-09-22
OpenCV
c# 迭代器實現
2018-12-09
C#
空間旋轉矩陣怎麼表示
2018-05-22
矩陣
火星座標和正常座標之間的轉換
2014-04-08
| / - 的旋轉效果實現(轉)
2007-08-11
三維空間中物體兩次旋轉之間的連續旋轉矩陣計算
2018-08-30
矩陣
原地旋轉矩陣(不申請新的記憶體空間旋轉矩陣)
2012-07-14
矩陣記憶體
ENVI為遙感影像設定空間座標系的方法
2024-03-13
剛體在三維空間的旋轉(關於旋轉矩陣、DCM、旋轉向量、四元數、尤拉角)
2016-04-24
矩陣
liunx使用者空間和核心空間之間的通訊實現(在PPC下的實現)(轉)
2007-08-15
iOS地球座標、火星座標和百度座標之間轉換(Swift3.0)
2017-01-13
iOSSwift
三維空間旋轉矩陣四元數尤拉角相互轉換組合旋轉
2016-02-16
矩陣
canvas 旋轉角度不需要累加
2019-08-24
Canvas
影像旋轉的FPGA實現（一）
2021-07-28
FPGA
SVG實現的圓環旋轉效果
2017-04-09
SVG
C++ opencv的圓轉矩形，極座標轉笛卡爾座標系
2020-10-29
C++OpenCV
三維旋轉矩陣的推導過程（以右手座標系為例）
2020-02-12
矩陣
Qt 求圓和橢圓上任意角度點的座標
2024-04-02
QT
梯度下降法、座標下降法、牛頓迭代法
2017-11-22
梯度
canvas 繪製旋轉一定角度的矩形
2017-02-17
Canvas
【CV】三維空間的旋轉問題(Rotation in 3D space)
2018-07-19
3D
php實現圖片旋轉
2022-06-13
PHP
實現反轉連結串列--遞迴、迭代、雙指標、棧
2021-01-02
遞迴指標
如何實現圖片的3D旋轉，而且是不停旋轉？
2016-10-13
3D
ogre世界座標魚螢幕座標相互轉換
2014-05-08
Python下獲取視訊的旋轉角度資訊
2020-12-29
Python
canvas繪製旋轉一定角度的矩形效果
2017-02-17
Canvas
小專案實現大目標（轉）
2007-08-10