通过控制点信息计算4参和7参
最近通过OSGearth加载3维模型信息，由于施工方是平面坐标，无法正常加载到WGS84的球上，就需要将通过控制点，将施工坐标系转化到84地理坐标系上，一下是核心代码
#pragma once
#include #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 #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;
}
七参和四参反推