布尔沙七参数python代码 如何获取布尔莎七参数

七参数转换法(布尔莎模型)

七参数法（包括布尔莎模型，一步法模型，海尔曼特等）是解决此问题的比较严密和通用的方法。一般含有7个转换参数：X平移，Y平移，Z平移，X旋转，Y旋转，Z旋转，尺度变化m。通过3个以上的公共点由最小二乘法拟合出相应的转换参数, 然后由求得的转换参数进行坐标转换。

七参数公式如下:

$\left[\begin{array}{l} X \\ Y \\ Z \end{array}\right]=\left[\begin{array}{c} \Delta X \\ \Delta Y \\ \Delta Z \end{array}\right]+(1+m)\left[\begin{array}{ccc} 1 & \theta_{z} & -\theta_{y} \\ -\theta_{z} & 1 & \theta_{x} \\ \theta_{y} & -\theta_{x} & 1 \end{array}\right]\left[\begin{array}{l} X_{0} \\ Y_{0} \\ Z_{0} \end{array}\right]$

式中, $\Delta X, \Delta Y, \Delta Z$为三个平移参数; $\theta_{x}, \theta_{y}, \theta_{z}$为3个旋转参数，m为尺度参数。将七参数作为未知数，并且按参数线性化，则可以转换为：

$\left[\begin{array}{c} X-X\left(X_{0}\right) \\ Y-Y\left(Y_{0}\right) \\ Z-Z\left(Z_{0}\right) \end{array}\right]=\left[\begin{array}{ccccccc} 1 & 0 & 0 & 0 & -(1+m) Z_{0} & (1+m) Y_{0} & X_{0}+\theta_{z} Y_{0}-\theta_{y} Z_{0} \\ 0 & 1 & 0 & (1+m) Z_{0} & 0 & -(1+m) X_{0} & -\theta_{z} X_{0}+Y_{0}+\theta_{x} Z_{0} \\ 0 & 0 & 1 & -(1+m) Y_{0} & (1+m) X_{0} & 0 & \theta_{y} X_{0}-\theta_{x} Y_{0}+Z_{0} \end{array}\right]\left[\begin{array}{c} \Delta X \\ \Delta Y \\ \Delta Z \\ \theta_{x} \\ \theta_{y} \\ \theta_{z} \\ m \end{array}\right]$
其中，$X(X_0),Y(Y_0),Z(Z_0)$由七参数公式给出，利用最小二乘法法即可对该线性化的方程进行迭代求解。

七参数转换法实例

下面以C++代码为例，利用已有四个控制点坐标，根据最小二乘来求解七参数模型的参数，并利用一个检查点来验证其转换精度。

1.导入数据

使用4个同名点的原坐标(x, y, z)和目标坐标（x’, y’, z’)来求解七参数

2.正向计算

根据坐标转换公式来列立系数方程定义函数返回系数矩阵。使用4个同名点的坐标转换公式作为误差方程进行最小二乘平差，根据坐标转换公式来列立系数方程 $W x=b$

3.系数求解

组成法方程 $(W’W) x = (W’b) $并利用最小二乘求解$x$

4.模型精度评定和结果验证

评定最小二乘模型的精度结果，并使用预留的已知坐标同名点来验证七参数模型的效果

C++代码：

#include <arrayfire.h>
#include <iostream>

using namespace af;

array point2matrix(double& x, double& y, double& z, array& args)
{
    /*
    定义函数返回系数矩阵 B， l
        定义函数： point2matrix，
        通过给定的同名点坐标列立误差方程B系数阵的部分
        x, y, z： 原坐标值
        args: 七参数误差值[Delta_X, Delta_Y, Delta_Z, theta_x, theta_y, theta_z, m]
        返回值： W系数阵的部分
    */
    array W = constant(0,3,7,f64);
    W(seq(3), seq(3))= identity(3, 3);
    W(0, 4) = -(1 + args(6)) * z;
    W(0, 5) = (1 + args(6)) * y;
    W(0, 6) = x + args(5) * y - args(4) * z;

    W(1, 3) = (1 + args(6)) * z;
    W(1, 5) = -(1 + args(6)) * x;
    W(1, 6) = -(args(5) * x) + y + args(3) * z;

    W(2, 3) = -(1 + args(6)) * y;
    W(2, 4) = (1 + args(6)) * x;
    W(2, 6) = args(4) * x - args(3) * y + z;
    return W;
}


array points2W(array &points, array &args)
{
    /*
    定义函数： points2W
        通过同名点序列列立误差方程B系数阵的整体
        x, y, z： 同名点序列
        args: 七参数误差值[Delta_X, Delta_Y, Delta_Z, theta_x, theta_y, theta_z, m]
        返回值： W系数阵
    */
    array big_mat;
    double x, y, z;
    for(int i =0;i<points.dims(0);++i)
    {
        array p = points(i, span);
        x = p(0).scalar<double>();
        y = p(1).scalar<double>();
        z = p(2).scalar<double>();
        array mat = point2matrix(x, y, z, args);
        if (big_mat.elements() == 0)
        {
            big_mat = mat;
        }
        else
        {
            big_mat = join(0,big_mat, mat);
        }
    }
    return big_mat;
}

array ordinationConvert(double& x1, double& y1, double& z1, array& args)
{
    array p = constant(0,1,3,f64);
    p(0) = args(0) + (1 + args(6)) * (x1 + args(5) * y1 - args(4) * z1);
    p(1) = args(1) + (1 + args(6)) * (-(args(5) * x1) + y1 + args(3) * z1);
    p(2) = args(2) + (1 + args(6)) * (args(4) * x1 - (args(3) * y1) + z1);
    return p;
}

array points2b(array &source_points, array &target_points, array &args)
{
    /*
    定义函数： points2b
          通过同名点坐标转换关系列立误差方程B系数阵的整体
          x, y, z： 同名点的原坐标和目标坐标对组成的序列
          args: 七参数误差值[Delta_X, Delta_Y, Delta_Z, theta_x, theta_y, theta_z, m]
          返回值： b系数阵
    */

    int len = source_points.dims(0);
    array big_mat = constant(0,len,3,f64);
    //array p0, p1, p2;
    double x1, y1, z1;
    for(int i=0;i<len;++i)
    {
        array p1 = source_points(i, span);
        x1 = p1(0).scalar<double>(); y1 = p1(1).scalar<double>(); z1 = p1(2).scalar<double>();
        array p2 = target_points(i, span);
        array p0 = ordinationConvert(x1, y1, z1, args);
        big_mat(i, span) = p2 - p0;
    }
    return flat(big_mat.T());
}

int main()
{
    try {
        /*int device = 2;
        setDevice(device);*/
        array Args = constant(0, 7,f64);
        array parameters = constant(1,7,f64);
        // 原坐标(x, y, z)
        double h_source_points[12] = {
            3381400.980, 395422.030, 32.956,
            3381404.344, 395844.239, 32.207,
            3382149.810, 396003.592, 33.290,
            3382537.793, 395985.359, 33.412 };
        // 目标坐标（x’, y’, z’)  
        double h_target_points[12] = {
            3380968.194, 539468.888, 13.875,
            3380977.154, 539890.934, 13.179,
            3381724.612, 540040.47, 14.273,
            3381724.636, 540040.485, 14.282 };

        array source_points = array(3, 4, h_source_points).T();
        array target_points = array(3, 4, h_target_points).T();

        //归一化处理便于模型的快速迭代
        array ave_src = mean(source_points,0);
        array ave_tar = mean(target_points,0);
        source_points -= tile(ave_src,4,1,1,1);
        target_points -= tile(ave_tar,4,1,1,1);

        timer::start();
        // 当七参数的误差值之和大于1e - 10时，迭代运算得到更精确的结果
        array W, b, qxx;
        while ((sum)(abs(parameters)).scalar<double>() > 1e-10)
        {
            W = points2W(source_points, Args);
            b = points2b(source_points, target_points, Args);
            qxx = inverse(matmul(W.T(), W));
            parameters = matmul(matmul(qxx, W.T()), b);
            Args += parameters;
        }

        //af_print(parameters); 
        std::cout << "七参数输出: " << std::endl;
        af_print(Args);

        // 检查点坐标
        double hsource_test_points[3] = { 3381402.058, 395657.940, 32.728 };
        double htarget_test_points[3] = { 3380972.424, 539704.811, 13.665 };

        array source_test_points(1, 3, hsource_test_points);
        array target_test_points(1, 3, htarget_test_points);

        // 归一化处理
        source_test_points = source_test_points - ave_src;

        // 单位权标准差，即所得模型的坐标精度
        double sigma0 = (sqrt)(sum(b * b) / 2).scalar<double>();
        // 参数标准差，即所得模型的七参数精度
        array sigmax = sigma0 * (sqrt)(diag(qxx));
 
        double x, y, z;
        x = source_test_points(0).scalar<double>(); 
        y = source_test_points(1).scalar<double>(); 
        z = source_test_points(2).scalar<double>();
        array p_test = ordinationConvert(x, y, z, Args) + ave_tar;

        std::cout << "单位权中误差: " << sigma0 << std::endl;

        std::cout << "参数中误差:" << std::endl;
        af_print(sigmax);
        std::cout << "模型预测结果: " << std::endl;
        af_print(p_test);
        std::cout << "真实结果: " << std::endl;
        af_print(target_test_points);
        std::cout << "消耗时间: " << timer::stop() <<" s" << std::endl;
    }
    catch (af::exception& e)
    {
        std::cerr << e.what() << std::endl;
    }
}

结果：

七参数输出:
Args
[7 1 1 1]
    0.0000
    0.0000
    0.0000
   -0.0000
    0.0002
   -0.0717
   -0.2149

单位权中误差: 162.711
参数中误差:
sigmax
[7 1 1 1]
   81.3557
   81.3557
   81.3557
    0.6505
    0.3111
    0.1912
    0.1497

模型预测结果:
p_test
[1 3 1 1]
3380987.5078 539711.3111    13.6542
真实结果:
target_test_points
[1 3 1 1]
3380972.4240 539704.8110    13.6650
消耗时间: 1.39431 s