通過控制點信息計算4參和7參

最近通過OSGearth加載3維模型信息，由於施工方是平面座標，無法正常加載到WGS84的球上，就需要將通過控制點，將施工座標系轉化到84地理座標系上，一下是核心代碼

#pragma once
#include <vector>
class Matrix
{
 
public:
	//矩陣乘法
	bool MatrixMultiply(std::vector<std::vector<double>> a, std::vector<std::vector<double>> b, std::vector<std::vector<double>>  & c);

	//  矩陣相加
	bool MatrixAdd(std::vector<std::vector<double>> a, std::vector<std::vector<double>>b, std::vector<std::vector<double>> & c);
	

	//矩陣相減
	bool MatrixSubtration(std::vector<std::vector<double>> a, std::vector<std::vector<double>>  b, std::vector<std::vector<double>> & c);

	//// 矩陣的行列式的值 
	double MatrixSurplus(std::vector<std::vector<double>> a);

	//   矩陣轉置
	bool MatrixInver(std::vector<std::vector<double>> a, std::vector<std::vector<double>> &b);

	//矩陣求逆
	bool MatrixOppstd(std::vector<std::vector<double>> a, std::vector<std::vector<double>>  &b);

	std::vector<std::vector<double>>  MatrixOpp(std::vector<std::vector<double>> Array);
};

#include "Matrix.h"
bool Matrix::MatrixMultiply(std::vector<std::vector<double>> a, std::vector<std::vector<double>> b, std::vector<std::vector<double>>  & c)
{
	if (a[0].size() != b.size())
		return false;
	if (a.size() != c.size() || b[0].size() != c[0].size())
		return false;
	for (int i = 0; i < a.size(); i++)
	{
		for (int j = 0; j < b[0].size(); j++)
		{
			c[i][j] = 0.0;
			for (int k = 0; k < b.size(); k++)
			{
				c[i][j] += a[i][k] * b[k][j];
			}
		}
	}
	return true;
}
bool Matrix::MatrixAdd(std::vector<std::vector<double>> a, std::vector<std::vector<double>>b, std::vector<std::vector<double>> & c)
{
	if (a.size() != b.size() || a[0].size() != b[0].size()
		|| a.size() != c.size() || a[0].size() != c[0].size())
		return false;
	for (int i = 0; i < a.size(); i++)
	{
		for (int j = 0; j < a[0].size(); j++)
		{
			c[i][j] = a[i][j] + b[i][j];
		}
	}
	return true;
}

bool Matrix::MatrixSubtration(std::vector<std::vector<double>> a, std::vector<std::vector<double>>  b, std::vector<std::vector<double>> & c)
{
	if (a.size() != b.size() || a[0].size() != b[0].size()
		|| a.size() != c.size() || a[0].size() != c[0].size())
		return false;
	for (int i = 0; i < a.size(); i++)
	{
		for (int j = 0; j < a[0].size(); j++)
		{
			c[i][j] = a[i][j] - b[i][j];
		}
	}

	return true;
}
double Matrix::MatrixSurplus(std::vector<std::vector<double>> a)
{
	int i, j, k, p, r, m, n;
	m = a.size();
	n = a[0].size();
	double X, temp = 1, temp1 = 1, s = 0, s1 = 0;

	if (n == 2)
	{
		for (i = 0; i < m; i++)
			for (j = 0; j < n; j++)
				if ((i + j) % 2 > 0) temp1 *= a[i][j];
				else temp *= a[i][j];
		X = temp - temp1;
	}
	else
	{
		for (k = 0; k < n; k++)
		{
			for (i = 0, j = k; i < m && j < n; i++, j++)
				temp *= a[i][j];
			if (m - i > 0)
			{
				for (p = m - i, r = m - 1; p > 0; p--, r--)
					temp *= a[r][p - 1];
			}
			s += temp;
			temp = 1;
		}

		for (k = n - 1; k >= 0; k--)
		{
			for (i = 0, j = k; i < m && j >= 0; i++, j--)
				temp1 *= a[i][j];
			if (m - i > 0)
			{
				for (p = m - 1, r = i; r < m; p--, r++)
					temp1 *= a[r][p];
			}
			s1 += temp1;
			temp1 = 1;
		}

		X = s - s1;
	}
	return X;
}
bool Matrix::MatrixInver(std::vector<std::vector<double>> a, std::vector<std::vector<double>> &b)
{
	if (a.size() != b[0].size() || a[0].size() != b.size())
		return false;
	for (int i = 0; i < a[0].size(); i++)
		for (int j = 0; j < a.size(); j++)
			b[i][j] = a[j][i];

	return true;
}
bool Matrix::MatrixOppstd(std::vector<std::vector<double>> a, std::vector<std::vector<double>>  &b)
{
	double X = MatrixSurplus(a);
	if (X == 0) return false;
	X = 1 / X;

	std::vector<std::vector<double>> B(a.size(), std::vector<double>(a[0].size()));
	std::vector<std::vector<double>> SP(a.size(), std::vector<double>(a[0].size()));
	std::vector<std::vector<double>> AB(a.size(), std::vector<double>(a[0].size()));

	for (int i = 0; i < a.size(); i++)
		for (int j = 0; j < a[0].size(); j++)
		{
			for (int m = 0; m < a.size(); m++)
				for (int n = 0; n < a[0].size(); n++)
					B[m, n] = a[m, n];
			{
				for (int x = 0; x < a[0].size(); x++)
					B[i][x] = 0;
				for (int y = 0; y < a.size(); y++)
					B[y][j] = 0;
				B[i][j] = 1;
				SP[i][j] = MatrixSurplus(B);
				AB[i][j] = X * SP[i][j];
			}
		}
	MatrixInver(AB, b);
	return true;
}
//矩陣求逆的重載，精度比較高
std::vector<std::vector<double>>  Matrix::MatrixOpp(std::vector<std::vector<double>> Array)
{
	int m = 0;
	int n = 0;
	m = Array.size();
	n = Array[0].size();
	std::vector<std::vector<double>> array(2 * m + 1, std::vector<double>(2 * n + 1));
	for (int k = 0; k < 2 * m + 1; k++)  //初始化數組
	{
		for (int t = 0; t < 2 * n + 1; t++)
		{
			array[k][t] = 0.00000000;
		}
	}
	for (int i = 0; i < m; i++)
	{
		for (int j = 0; j < n; j++)
		{
			array[i][j] = Array[i][j];
		}
	}

	for (int k = 0; k < m; k++)
	{
		for (int t = n; t <= 2 * n; t++)
		{
			if ((t - k) == m)
			{
				array[k][t] = 1.0;
			}
			else
			{
				array[k][t] = 0;
			}
		}
	}
	//得到逆矩陣
	for (int k = 0; k < m; k++)
	{
		if (array[k][k] != 1)
		{
			double bs = array[k][k];
			array[k][k] = 1;
			for (int p = k + 1; p < 2 * n; p++)
			{
				array[k][p] /= bs;
			}
		}
		for (int q = 0; q < m; q++)
		{
			if (q != k)
			{
				double bs = array[q][k];
				for (int p = 0; p < 2 * n; p++)
				{
					array[q][p] -= bs * array[k][p];
				}
			}
			else
			{
				continue;
			}
		}
	}
	std::vector<std::vector<double>> NI(m, std::vector<double>(n));
	for (int x = 0; x < m; x++)
	{
		for (int y = n; y < 2 * n; y++)
		{
			NI[x][y - n] = array[x][y];
		}
	}
	return NI;
}

擬合相關工具類

#pragma once
#include "gdal/ogr_geometry.h"
#include <QtMath>
#include <QMap>
#include "Matrix.h"
#include <cmath>
typedef enum Canshu4_enum
{
	enum_rota,
	enum_scale,
	enum_dx,
	enum_dy
}Canshu4_enum;
typedef enum Canshu7_enum
{
	enum_rotax,
	enum_rotay,
	enum_rotaz,
	enum_canshu7_scale,
	enum_canshu7_dx,
	enum_canshu7_dy,
	enum_canshu7_dz
}Canshu7_enum;
class PramSCals
{
public:
	static int sign(double x);
	static double FWJ(OGRPoint p1, OGRPoint p2);

	//兩點之間的距離公式
	static double Dist(OGRPoint p1, OGRPoint p2);
	 
	static QMap<int, double> Canshu4(std::vector<OGRPoint> p1, std::vector<OGRPoint> p2);
 
	static QMap<int, double> Canshu4(std::vector<OGRPoint>  p1, std::vector<OGRPoint>  p2, int PointCount);
 
	static QMap<int, double> Canshu7(std::vector<OGRPoint> p1, std::vector<OGRPoint> p2, int PointCount);
 
};

#include "PramSCals.h"
int PramSCals::sign(double x)
{
	if (x < 0) return -1;
	else return 1;
}
double PramSCals::FWJ(OGRPoint p1, OGRPoint p2)
{
	double dx, dy;
	dx = p2.getX() - p1.getX();
	dy = p2.getY() - p1.getY();
	return M_PI - sign(dy) - qAtan(dx / dy);
}
//兩點之間的距離公式
double PramSCals::Dist(OGRPoint p1, OGRPoint p2)
{
	double d;
	d = qSqrt(qPow((p2.getX() - p1.getX()), 2) + qPow((p2.getY() - p1.getY()), 2));
	return d;
}
QMap<int, double> PramSCals::Canshu4(std::vector<OGRPoint> p1, std::vector<OGRPoint> p2)
{
	QMap<int, double> params;
	params[enum_rota] = FWJ(p2[0], p2[1]) - FWJ(p1[0], p1[1]);
	params[enum_scale] = Dist(p2[0], p2[1]) / Dist(p1[0], p1[1]);
	params[enum_dx] = p2[0].getX() - params[enum_scale] * qCos(params[enum_rota]) * p1[0].getX() + params[enum_scale] * qSin(params[enum_rota]) * p1[0].getY();
	params[enum_dy] = p2[0].getY() - params[enum_scale] * qSin(params[enum_rota]) * p1[0].getX() - params[enum_scale] * qCos(params[enum_rota]) * p1[0].getY();
	return params;
}
QMap<int, double> PramSCals::Canshu4(std::vector<OGRPoint>  p1, std::vector<OGRPoint>  p2, int PointCount)
{
	QMap<int, double> params;
	double u = 1.0, v = 0, Dx = 0.0, Dy = 0.0;

	int intCount = PointCount;
	//Matrix dx1 ;//誤差方程改正數
	Matrix * B = NULL;//誤差方程係數矩陣
			 // Matrix W ;//誤差方程常數項
	std::vector<std::vector<double>> dx1(4, std::vector<double>(1));
	std::vector<std::vector<double>> B1(2 * intCount, std::vector<double>(4));
	std::vector<std::vector<double>> W1(2 * intCount, std::vector<double>(1));
	// Matrix BT, N, InvN, BTW;
	std::vector<std::vector<double>> BT(4, std::vector<double>(2 * intCount));
	std::vector<std::vector<double>> N(4, std::vector<double>(4));
	std::vector<std::vector<double>> InvN(4, std::vector<double>(4));
	std::vector<std::vector<double>> BTW(4, std::vector<double>(1));
	for (int i = 0; i < intCount; i++)
	{
		//計算誤差方程係數
		B1[2 * i][0] = 1;
		B1[2 * i][1] = 0;
		B1[2 * i][2] = p1[i].getX();
		B1[2 * i][3] = -p1[i].getY();


		B1[2 * i + 1][0] = 0;
		B1[2 * i + 1][1] = 1;
		B1[2 * i + 1][2] = p1[i].getY();
		B1[2 * i + 1][3] = p1[i].getX();
	}
	B = new Matrix();
	for (int i = 0; i < intCount; i++)
	{
		//計算誤差方程系常數
		W1[2 * i][0] = p2[i].getX() - u * p1[i].getX() + v * p1[i].getY() - Dx;
		W1[2 * i + 1][0] = p2[i].getY() - u * p1[i].getY() - v * p1[i].getX() - Dy;

	}
	//最小二乘求解
	B->MatrixInver(B1, BT);//轉置
	B->MatrixMultiply(BT, B1, N);
	InvN = B->MatrixOpp(N);
	B->MatrixMultiply(BT, W1, BTW);
	B->MatrixMultiply(InvN, BTW, dx1);
	Dx = Dx + dx1[0][0];
	Dy = Dy + dx1[1][0];
	u = u + dx1[2][0];
	v = v + dx1[3][0];
	params[enum_dx] = Dx;
	params[enum_dy] = Dy;
	params[enum_rota] = qAtan(v / u);
	params[enum_scale] = u / qCos(params[enum_rota]);
	params[enum_dx] = params[enum_dx];
	params[enum_dy] =  params[enum_dy];
	params[enum_rota] =  params[enum_rota];
	params[enum_scale] = params[enum_scale];
	return params;
}
QMap<int, double> PramSCals::Canshu7(std::vector<OGRPoint> p1, std::vector<OGRPoint> p2, int PointCount)
{
	std::vector<std::vector<double>> B1(PointCount * 3, std::vector<double>(7));
	std::vector<std::vector<double>> dx1(7, std::vector<double>(1));
	std::vector<std::vector<double>> L(PointCount * 3, std::vector<double>(1));
	std::vector<std::vector<double>> BT(7, std::vector<double>(PointCount * 3));
	std::vector<std::vector<double>> N(7, std::vector<double>(7));
	std::vector<std::vector<double>> InvN(7, std::vector<double>(7));
	std::vector<std::vector<double>> BTL(7, std::vector<double>(1));
	Matrix * B = new Matrix();

	//初始化L矩陣
	for (int i = 0; i < PointCount * 3; i++)
	{
		if (i % 3 == 0)
		{
			L[i][0] = p2[i / 3].getX();
		}
		else if (i % 3 == 1)
		{
			L[i][0] = p2[i / 3].getY();
		}
		else if (i % 3 == 2)
		{
			L[i][0] = p2[i / 3].getZ();
		}
	}
	//初始化B矩陣
	for (int i = 0; i < PointCount * 3; i++)
	{
		if (i % 3 == 0)
		{
			B1[i][0] = 1;
			B1[i][1] = 0;
			B1[i][2] = 0;
			B1[i][3] = p1[i / 3].getX();
			B1[i][4] = 0;
			B1[i][5] = -p1[i / 3].getZ();
			B1[i][6] = p1[i / 3].getY();

		}
		else if (i % 3 == 1)
		{
			B1[i][0] = 0;
			B1[i][1] = 1;
			B1[i][2] = 0;
			B1[i][3] = p1[i / 3].getY();
			B1[i][4] = p1[i / 3].getZ();
			B1[i][5] = 0;
			B1[i][6] = -p1[i / 3].getX();
		}
		else if (i % 3 == 2)
		{
			B1[i][0] = 0;
			B1[i][1] = 0;
			B1[i][2] = 1;
			B1[i][3] = p1[i / 3].getZ();
			B1[i][4] = -p1[i / 3].getY();
			B1[i][5] = p1[i / 3].getX();
			B1[i][6] = 0;
		}

	}
	//轉置
	B->MatrixInver(B1, BT);
	//法方程矩陣
	//N=BT*B
	B->MatrixMultiply(BT, B1, N);
	//求逆
	InvN = B->MatrixOpp(N);
	//BTL=BT*L
	B->MatrixMultiply(BT, L, BTL);
	//dx1=invN*BTL;
	B->MatrixMultiply(InvN, BTL, dx1);
	//
	QMap<int, double> params;
	params[enum_canshu7_dx] = qRound(dx1[0][0]);
	params[enum_canshu7_dy] = qRound(dx1[1][0]);
	params[enum_canshu7_dz] = qRound(dx1[2][0]);
	params[enum_canshu7_scale] = qRound(dx1[3][0]);
	params[enum_rotax] = qRound(dx1[4][0] / dx1[3][0]);
	params[enum_rotay] = qRound(dx1[5][0] / dx1[3][0]);
	params[enum_rotaz] = qRound(dx1[6][0] / dx1[3][0]);
	return params;
}

七參和四參反推