七参数/六参数坐标转换（小角度）-- 公共点间接平差

C++七参数/六参数坐标转换（小角度）-- 公共点间接平差

三维空间坐标转换，基本数学模型如下：
$$\left[\begin{array}{c}{X}_B\\ Y_B\\ Z_B\end{array}\right]=\left[\begin{array}{c}{X}_A\\ Y_A\\ Z_A\end{array}\right]+\left[\begin{array}{ccccccc}1& 0& 0& 0& -Z_A& Y_A& X_A\\ 0& 1& 0& Z_A& 0& -X_A& Y_A\\ 0& 0& 1& -Y_A& X_A& 0& Z_A\end{array}\right]\left[\begin{array}{c}DX\\ DY\\ DZ\\ RX\\ RY\\ RZ\\ DK\end{array}\right]$$
上式中(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的参数，即$[DX,DY,DZ,RX,RY,RZ,DK{]}^T$
  注意不论是七参数还是六参数模型，输入的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 
#include 
#include 
#include 
#include 
#include 
#include 

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();

		}
	}





}