C++七参数/六参数坐标转换（小角度）-- 公共点间接平差
三维空间坐标转换，基本数学模型如下：
上式中(XB,YB,ZB)为目标坐标系坐标，(XA,YA,ZA)为源坐标系坐标，DX、DY、DZ为平移参数，RX、RY、RZ为旋转参数，DK为尺度因子。六参数转换时认为DK已知不变，值为0。
输入的数据是
- origin_data, target_data：原坐标系下数据，转换坐标系后观测数据
nx4的“N（点号）,X,Y,Z（三维坐标）"的Eigen3 Matrix - params：7x1的参数，即
注意不论是七参数还是六参数模型，输入的params都为7x1大小，函数结束后会返回最后一次迭代的params - sigma：单位权中误差，可以根据返回的sigma判断是否终止迭代
- p_share：说明公共点的个数，为数据中前p_share个
- num_params：两个值（6或7），说明是采用6参数还是7参数算法
输出的数据是：
- trans_data：平差后公共点数据
- 更新后的params，sigma
注：点号+三维坐标(N,X,Y,Z)文本文件的读入可以参考我的另一篇文章：
https://blog.csdn.net/Canvaskan/article/details/105215077
#include <fstream>
#include <iostream>
#include <string>
#include <Eigen/Dense>
#include <vector>
#include <cmath>
#include <iomanip>
using namespace std;
using namespace Eigen;
// 基于七参数的算法，可以扩展得到四参数，六参数，若要使用，输入params仍为7x1，指定num_params=4或6(默认为7)
void Parameter(MatrixXd& origin_data, MatrixXd& target_data, MatrixXd& trans_data, MatrixXd& params, int p_share, double& sigma, int num_params = 7)
{
// 检验Matrix params大小
if (params.rows() != 7 || params.cols() != 1)
{
cout << "Warning! Parameters size not match!" << endl;
exit(1);
}
MatrixXd N_origin = origin_data.col(0);
MatrixXd X_origin = origin_data.col(1);
MatrixXd Y_origin = origin_data.col(2);
MatrixXd Z_origin = origin_data.col(3);
MatrixXd N_target = target_data.col(0);
MatrixXd X_target = target_data.col(1);
MatrixXd Y_target = target_data.col(2);
MatrixXd Z_target = target_data.col(3);
// 输出格式
cout << setiosflags(ios::scientific) << setprecision(2);
////////////////// 三种参数算法公共部分///////////////////
// 创建与参数个数对应的B, L, d, l矩阵
MatrixXd B(3 * p_share, 7);
MatrixXd L(3 * p_share, 1);
MatrixXd d(3 * p_share, 1);
for (int iter_p = 0; iter_p < p_share; iter_p++)
{
double X = X_origin(iter_p);
double Y = Y_origin(iter_p);
double Z = Z_origin(iter_p);
double X1 = X_target(iter_p);
double Y1 = Y_target(iter_p);
double Z1 = Z_target(iter_p);
B.middleRows(3 * iter_p, 3) <<
1, 0, 0, 0, -Z, Y, X,
0, 1, 0, Z, 0, -X, Y,
0, 0, 1, -Y, X, 0, Z;
L.middleRows(3 * iter_p, 3) <<
X1,
Y1,
Z1;
d.middleRows(3 * iter_p, 3) <<
X,
Y,
Z;
}
MatrixXd l(3 * p_share, 1);
l = L - B * params - d;
// 确定权阵P
MatrixXd P = MatrixXd::Identity(3 * p_share, 3 * p_share);
//////////////////////// 七参数部分 ///////////////////////////////
if (num_params == 7)
{
// 计算x_cap（七参数）
MatrixXd x_cap(7, 1);
MatrixXd t1 = B.transpose() * P * B;
MatrixXd t2 = B.transpose() * P * l;
x_cap = t1.inverse() * t2;
// 计算V = B*x_cap - l
MatrixXd V = B * x_cap - l;
// 由于矩阵乘法返回的是矩阵
MatrixXd sigma_m = V.transpose() * P * V;
// 输出params, sigma
params = params + x_cap;
sigma = sigma_m(0);
// 按格式输出trans_data
for (int iter_p = 0; iter_p < p_share; iter_p++)
{
// 便于表示的标记
MatrixXd xyz_t = origin_data.block(iter_p, 1, 1, 3).transpose();
MatrixXd t1 = B.middleRows(3 * iter_p, 3) * params;
MatrixXd XYZ_t = xyz_t + t1;
trans_data.row(iter_p) << N_origin.row(iter_p), XYZ_t.transpose();
}
}
////////////////////// 六参数部分 /////////////////////////////////
if (num_params == 6)
{
// 计算x_cap（六参数）
MatrixXd x_cap(6, 1);
MatrixXd B_ = B.leftCols(6); // 不能InPlace赋值
B = B_;
MatrixXd t1 = B.transpose() * P * B;
MatrixXd t2 = B.transpose() * P * l;
x_cap = t1.inverse() * t2;
//cout << "B=\n" << B << endl;
//cout << "p=\n" << P << endl;
//cout << "l=\n" << l << endl;
//cout << x_cap << endl;
// 计算V = B*x_cap - l
MatrixXd V = B * x_cap - l;
// 由于矩阵乘法返回的是矩阵
MatrixXd sigma_m = V.transpose() * P * V;
// 输出params, sigma
params.topRows(6) = params.topRows(6) + x_cap;
sigma = sigma_m(0);
// 按格式输出trans_data
for (int iter_p = 0; iter_p < p_share; iter_p++)
{
// 便于表示的标记
MatrixXd xyz_t = origin_data.block(iter_p, 1, 1, 3).transpose();
MatrixXd t1 = B.middleRows(3 * iter_p, 3) * params.topRows(6);
MatrixXd XYZ_t = xyz_t + t1;
trans_data.row(iter_p) << N_origin.row(iter_p), XYZ_t.transpose();
}
}
}
