C++实现七参数转换法(布尔莎模型)

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

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

七参数公式如下:

[XYZ]=[ΔXΔYΔZ]+(1+m)[1θz−θy−θz1θxθy−θx1][X0Y0Z0]\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] ⎣⎡XYZ⎦⎤=⎣⎡ΔXΔYΔZ⎦⎤+(1+m)⎣⎡1−θzθyθz1−θx−θyθx1⎦⎤⎣⎡X0Y0Z0⎦⎤

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

[X−X(X0)Y−Y(Y0)Z−Z(Z0)]=[1000−(1+m)Z0(1+m)Y0X0+θzY0−θyZ0010(1+m)Z00−(1+m)X0−θzX0+Y0+θxZ0001−(1+m)Y0(1+m)X00θyX0−θxY0+Z0][ΔXΔYΔZθxθyθzm]\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(X0)Y−Y(Y0)Z−Z(Z0)⎦⎤=⎣⎡1000100010(1+m)Z0−(1+m)Y0−(1+m)Z00(1+m)X0(1+m)Y0−(1+m)X00X0+θzY0−θyZ0−θzX0+Y0+θxZ0θyX0−θxY0+Z0⎦⎤⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡ΔXΔYΔZθxθyθzm⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤
其中，X(X0),Y(Y0),Z(Z0)X(X_0),Y(Y_0),Z(Z_0)X(X0),Y(Y0),Z(Z0)由七参数公式给出，利用最小二乘法法即可对该线性化的方程进行迭代求解。

七参数转换法实例

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

1.导入数据

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

2.正向计算

根据坐标转换公式来列立系数方程定义函数返回系数矩阵。使用4个同名点的坐标转换公式作为误差方程进行最小二乘平差，根据坐标转换公式来列立系数方程 Wx=bW x=bWx=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.00000.00000.0000-0.00000.0002-0.0717-0.2149单位权中误差: 162.711
参数中误差:
sigmax
[7 1 1 1]81.355781.355781.35570.65050.31110.19120.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

参考资料：

空间坐标与投影系统系列(四)：七参数转换实例 | 武汉大学CVEO小组(张晓东教授团队) (whu-cveo.com)

https://mp.weixin.qq.com/s/1VcDLRTKuAX5W-dme6nX3Q

陈宇,白征东,罗腾.基于改进的布尔沙模型的坐标转换方法[J].大地测量与地球动力学,2010,30(03):71-73+78.