餐鲦怎么钓鲈鱼:3维空间转换的7参数求解和应用

转载

作者：Kiseigo
日期： 2009.02.21

前言：由于一直想写7参数的代码，但是却不会，近日得到Blue.Pan的帮助，写下了这些东西。07年在集思学院看到有人写过，但是感觉不太好，不过还是非常感谢作者的开源思想。在此，基于同样的考虑，写了这篇文章，希望对大家有所帮助。如果有错误，希望各位指出，共同学习。

   代码太多，还有矩阵运算，结构定义等，如果有需要C#代码，请留email，但是应该用户名和网站分开（如ZhangSan123，126的邮箱等方式），否则sina的系统常常会出不来.

     7参数主要用于三维空间坐标系的变换。非常的神奇。

    在工程测量中，用的最多，同时从数学角度来说也是最严密的转换方法，是经典的三维赫尔墨特法。由于结果中最多可求得七个转换参数，即三个平移参数( 、 、 )、三个旋转参数(Ex、Ey、Ez)和一个尺度缩放因子(m)，因此，通常也被称为七参数法。如图：

 
对两个不同坐标系经过平移，以及三次旋转，尺度改换，可以得到如下的公式。
 


 
求解7参数的核心代码如下：
///

    double[,] arrA = new double[aPtSource.Length * 3, 7]; // 如果是4个已知点， 12 * 7矩阵  A*X=B中的矩阵A
    for (int i = 0; i <= arrA.GetLength(0) - 1; i++)
    {
        if (i % 3 == 0)
        {
            arrA[i, 0] = 1;
            arrA[i, 1] = 0;
            arrA[i, 2] = 0;
            arrA[i, 3] = aPtSource[i / 3].X;
            arrA[i, 4] = 0;
            arrA[i, 5] = -aPtSource[i / 3].Z;
            arrA[i, 6] = aPtSource[i / 3].Y;
        }
        else if (i % 3 == 1)
        {
            arrA[i, 0] = 0;
            arrA[i, 1] = 1;
            arrA[i, 2] = 0;
            arrA[i, 3] = aPtSource[i / 3].Y;
            arrA[i, 4] = aPtSource[i / 3].Z;
            arrA[i, 5] = 0;
            arrA[i, 6] = -aPtSource[i / 3].X;
        }
        else if (i % 3 == 2)
        {
            arrA[i, 0] = 0;
            arrA[i, 1] = 0;
            arrA[i, 2] = 1;
            arrA[i, 3] = aPtSource[i / 3].Z;
            arrA[i, 4] = -aPtSource[i / 3].Y;
            arrA[i, 5] = aPtSource[i / 3].X;
            arrA[i, 6] = 0;
        }
    }

    double[,] arrB = new double[aPtSource.Length * 3, 1]; // A * X = B 中的矩阵B, 如果有4个点，就是 12*1矩阵
    for (int i = 0; i <= arrB.GetLength(0) - 1; i++)
    {
        if (i % 3 == 0)
        {
            arrB[i, 0] = aPtTo[i / 3].X;
        }
        else if (i % 3 == 1)
        {
            arrB[i, 0] = aPtTo[i / 3].Y;
        }
        else if (i % 3 == 2)
        {
            arrB[i, 0] = aPtTo[i / 3].Z;
        }
    }

    #endregion

    Matrix mtrA = new Matrix(arrA); // A矩阵
    Matrix mtrAT = mtrA.Transpose(); // A的转置
    Matrix mtrB = new Matrix(arrB); // B矩阵

    Matrix mtrATmulA = mtrAT.Multiply(mtrA); // A的转置×A

    // 求(A的转置×A)的逆矩阵
    mtrATmulA.InvertGaussJordan();

    // A的转置 × B

    Matrix mtrATmulB = mtrAT.Multiply(mtrB); // A的转置 * B

    // 结果
    Matrix mtrResult = mtrATmulA.Multiply(mtrATmulB);

    sep.Xdelta = mtrResult[0, 0];
    sep.Ydelta = mtrResult[1, 0];
    sep.Zdelta = mtrResult[2, 0];
    sep.scale = mtrResult[3, 0];
    sep.Ex = mtrResult[4, 0] / sep.scale;
    sep.Ey = mtrResult[5, 0] / sep.scale;
    sep.Ez = mtrResult[6, 0] / sep.scale;
   

    // PS: 必须考虑cosA = 0 不能作为分母的情况
    // Add code
}

利用7参数计算XYZ的代码如下：
///

    aPtTo = new PointXYZdbl[aPtSource.Length];

    for (int i = 0; i <= aPtSource.Length - 1; i++)
    {
        aPtTo[i].X = sep.Xdelta + k * aPtSource[i].X + 0 - a3 * aPtSource[i].Z + a4 * aPtSource[i].Y;
        aPtTo[i].Y = sep.Ydelta + k * aPtSource[i].Y + a2 * aPtSource[i].Z + 0 - a4 * aPtSource[i].X;
        aPtTo[i].Z = sep.Zdelta + k * aPtSource[i].Z - a2 * aPtSource[i].Y + a3 * aPtSource[i].X + 0;
    }
}

代码太多，还有矩阵运算，结构定义等，如果有需要C#代码，请留email，但是应该用户名和网站分开（如ZhangSan123，126的邮箱等方式），否则sina的系统常常会出不来.