#define _CRT_SECURE_NO_WARNINGS
#include<stdio.h>
#include<iostream>
#include<malloc.h>
#include<stdlib.h>
#include<math.h>
#include<conio.h>
#include<ctime>
using namespace std;
#define N 4
#define M 4
void RotationMatrix(double omega, double phi, double kappa, double* a, double* b, double* c);
void ErrorEquation(double omega, double kappa, double *A, double *l, double *a, double *b, double *c, double f, double *x, double *y, double *x0, double *y0, double *z0);
double *inv(double *a, int n);
void Multiply(double *A, double *B, double *C, int rows1, int cols1, int rows2, int cols2);
void Transpose(double *A, double AT[], int m, int n);
/*-----------------------------------------------主函数--------------------------------------------*/
int main()
{
//获取已知数据
double x[N], y[N], x0[N], y0[N], z0[N], X[N], Y[N], Z[N];
x[0] = -86.15; y[0] = -68.99; X[0] = 36589.41; Y[0] = 25273.32; Z[0] = 2195.17;
x[1] = -53.40; y[1] = 82.21; X[1] = 37631.08; Y[1] = 31324.51; Z[1] = 728.69;
x[2] = -14.78; y[2] = -76.63; X[2] = 39100.97; Y[2] = 24934.98; Z[2] = 2386.50;
x[3] = 10.46; y[3] = 64.43; X[3] = 40426.54; Y[3] = 30319.81; Z[3] = 757.31;
int i, j = 0;
printf("控制点坐标:꣺\n");
for (i = 0; i < N; i++)
printf("%lf %lf %lf %lf %lf\n", x[i], y[i], X[i], Y[i], Z[i]);
int m = 50000;
double f = 153.24;
printf("\n摄影机主距和摄影比例尺分母:f = %f, m = %d\n", f, m);
f = f / 1000.0;
double phi, omega, kappa, Xs0, Ys0, Zs0;
//初始化参数
double Sum1 = 0, Sum2 = 0, Sum3 = 0;
for (i = 0; i < N; i++)
{
x[i] /= 1000.0;
y[i] /= 1000.0;
Sum1 += X[i];
Sum2 += Y[i];
Sum3 += Z[i];
}
Xs0 = Sum1 / N;
Ys0 = Sum2 / N;
Zs0 = m * f + Sum3 / N;
phi = omega = kappa = 0;
printf("\n外方位元素初始值Xs0 = %f,Ys0 = %f, Zs0 = %f\nphi = %f, omega = %f, kappa = %f", Xs0, Ys0, Zs0, phi, omega, kappa);
double m0;
//迭代循环,每一轮迭代都估计外方位元素的值,并更新这些值并逐步得到最优值
while (j < M) //M=4
{
//计算旋转矩阵
double a[3], b[3], c[3];
RotationMatrix(omega, phi, kappa, a, b, c);
//利用共线方程根据当前的外方位元素和控制点的空间坐标,计算新的像点坐标
for (i = 0; i < N; i++)
{
x0[i] = -f * (a[0] * (X[i] - Xs0) + b[0] * (Y[i] - Ys0) + c[0] * (Z[i] - Zs0)) / (a[2] * (X[i] - Xs0) + b[2] * (Y[i] - Ys0) + c[2] * (Z[i] - Zs0));
y0[i] = -f * (a[1] * (X[i] - Xs0) + b[1] * (Y[i] - Ys0) + c[1] * (Z[i] - Zs0)) / (a[2] * (X[i] - Xs0) + b[2] * (Y[i] - Ys0) + c[2] * (Z[i] - Zs0));
z0[i] = a[2] * (X[i] - Xs0) + b[2] * (Y[i] - Ys0) + c[2] * (Z[i] - Zs0);
}
//构建误差方程
double A[2 * N * 6], l[2 * N];
ErrorEquation(omega, kappa, A, l, a, b, c, f, x, y, x0, y0, z0);
//矩阵相乘为后文所用
double AT[6 * 2 * N], ATA[6 * 6], *InvofATA = NULL, ATl[6], V[6];
Transpose(A, AT, 2 * N, 6);
Multiply(AT, A, ATA, 6, 2 * N, 2 * N, 6);
//计算ATA的逆矩阵
InvofATA = inv(ATA, 6);
//解误差方程,计算AT*l得到ATl
Multiply(AT, l, ATl, 6, 2 * N, 2 * N, 1);
//计算未知数的向量解,可以得到外方位元素近似值的改正数
Multiply(InvofATA, ATl, V, 6, 6, 6, 1);
//迭代更新外方位元素
Xs0 += V[0];
Ys0 += V[1];
Zs0 += V[2];
phi += V[3];
omega += V[4];
kappa += V[5];
printf("\n第%d轮迭代:外方位元素为:\n", j);
printf("Xs=%.5lf,Ys=%.5lf,Zs=%.5lf,t=%.5lf,w=%.5lf,k=%.5lf\n", Xs0, Ys0, Zs0, phi, omega, kappa);
//使用计算出的外方位元素计算误差向量v,并计算中误差以评估模型质量,
double s = 0, AX[8], v[8];
Multiply(A, V, AX, 8, 6, 6, 1);
for (i = 0; i < 8; i++)
{
v[i] = AX[i] - l[i];
s += v[i] * v[i];
}
m0 = sqrt(s / 2);
j++;
}
printf("\n\n\nXs=%.5lf,Ys=%.5lf,Zs=%.5lf,t=%.5lf,w=%.5lf,k=%.5lf\n", Xs0, Ys0, Zs0, phi, omega, kappa);
printf("\n中误差为:%f\n", m0);
system("pause");
return 0;
}
//计算旋转矩阵
//a\b\c为旋转矩阵的三个基向量, phi\omega\kappa为外方位元素
void RotationMatrix(double omega, double phi, double kappa, double* a, double* b, double* c)
{
a[0] = cos(phi)*cos(kappa) - sin(phi)*sin(omega)*sin(kappa);
a[1] = -cos(phi)*sin(kappa) - sin(phi)*sin(omega)*cos(kappa);
a[2] = -sin(phi)*cos(omega);
b[0] = cos(omega)*sin(kappa);
b[1] = cos(omega)*cos(kappa);
b[2] = -sin(omega);
c[0] = sin(phi)*cos(kappa) + cos(phi)*sin(omega)*sin(kappa);
c[1] = -sin(phi)*sin(kappa) + cos(phi)*sin(omega)*cos(kappa);
c[2] = cos(phi)*cos(omega);
}
//计算误差差方程式
//omega、kappa为俩外方位元素值,f为摄像机主距、N为控制点的数量、a\b\c为旋转矩阵的三个基向量、x\y是控制点的坐标、x0\y0\z0为近似值
//A是误差方程的系数矩阵、l是误差方程的常数项向量
void ErrorEquation(double omega, double kappa, double *A, double *l, double *a, double *b, double *c, double f, double *x, double *y, double *x0, double *y0, double *z0)
{
for (int i = 0; i < N; i++)
{
//计算近似值改正数的系数
A[i * 12 + 0] = (a[0] * f + a[2] * x[i]) / z0[i];
A[i * 12 + 1] = (b[0] * f + b[2] * x[i]) / z0[i];
A[i * 12 + 2] = (c[0] * f + c[2] * x[i]) / z0[i];
A[i * 12 + 3] = y[i] * sin(omega) - (x[i] * (x[i] * cos(kappa) - y[i] * sin(kappa)) / f + f * cos(kappa))*cos(omega);
A[i * 12 + 4] = -f * sin(kappa) - x[i] * (x[i] * sin(kappa) + y[i] * cos(kappa)) / f;
A[i * 12 + 5] = y[i];
A[i * 12 + 6] = (a[1] * f + a[2] * y[i]) / z0[i];
A[i * 12 + 7] = (b[1] * f + b[2] * y[i]) / z0[i];
A[i * 12 + 8] = (c[1] * f + c[2] * y[i]) / z0[i];
A[i * 12 + 9] = -x[i] * sin(omega) - (y[i] * (x[i] * cos(kappa) - y[i] * sin(kappa)) / f - f * sin(kappa))*cos(omega);
A[i * 12 + 10] = -f * cos(kappa) - y[i] * (x[i] * sin(kappa) + y[i] * cos(kappa)) / f;
A[i * 12 + 11] = -x[i];
//计算常数项向量
l[i * 2 + 0] = x[i] - x0[i];
l[i * 2 + 1] = y[i] - y0[i];
}
}
//计算矩阵的转置
//A为要转置矩阵的指针、AT是存放转置结果的矩阵的指针、m是原始矩阵的行数、n是原始矩阵的列数
void Transpose(double *A, double AT[], int m, int n)
{
int i, j;
for (i = 0; i < m; i++)
for (j = 0; j < n; j++)
//将原始矩阵中第i行第j列的元素赋给转置矩阵中的第j行第i列
AT[j*m + i] = A[i*n + j];
}
//计算两矩阵相乘
//A为第一个矩阵的指针,B为第二个矩阵的指针,C是存放结果的矩阵的指针,rows1、cols1、rows2、cols2分别是第一个、第二个矩阵的行列数
void Multiply(double *A, double *B, double *C, int rows1, int cols1, int rows2, int cols2)
{
int i, j, l, u;
//检查两个矩阵的纬度是否满足相乘要求
if (cols1 != rows2)
{
printf("error!cannot do the multiplication.\n");
return;
}
//矩阵相乘
for (i = 0; i < rows1; i++)
for (j = 0; j < cols2; j++)
{
u = i * cols2 + j;
//初始化结果矩阵
C[u] = 0.0;
//l用于遍历两个矩阵中参与乘法的对应元素,
for (l = 0; l < cols1; l++)
//通过 A[i*cols1 + l] 和 B[l*cols2 + j] 分别取得要相乘的两个元素,进行乘法运算并累加到矩阵 C 的当前位置。
C[u] += A[i*cols1 + l] * B[l*cols2 + j];
}
return;
}
//矩阵逆运算函数
double *inv(double *a, int n)
{
//分配了两个整数型数组is和js,分别用于记录主元素所在的行列索引
int *is, *js, i, j, k, l, u, v;
double d, p;
is = (int*)malloc(n * sizeof(int));
js = (int*)malloc(n * sizeof(int));
//主元素提取:在每次迭代中,通过循环遍历矩阵的行和列,找到当前迭代中绝对值最大的元素,该元素为主元素
//并记录主元素所在的行列索引,以备后续使用
for (k = 0; k <= n - 1; k++)
{
d = 0.0;
//获取主元素所在位置的索引,然后将主元素归一化
for (i = k; i < n; i++)
for (j = k; j < n; j++)
{
l = i * n + j;
p = fabs(a[l]);
if (p > d)
{
d = p;
is[k] = i;
js[k] = j;
}
}
if (d + 1.0 == 1.0)
{
free(is);
free(js);
printf("error not inv\n");
return NULL;
}
if (is[k] != k)
for (j = 0; j < n; j++)
{
u = k * n + j;
v = is[k] * n + j;
p = a[u];
a[u] = a[v];
a[v] = p;
}
if (js[k] != k)
for (i = 0; i < n; i++)
{
u = i * n + k;
v = i * n + js[k];
p = a[u];
a[u] = a[v];
a[v] = p;
}
//归一化计算
l = k * n + k;
a[l] = 1.0 / a[l];
//对当前行进行消元
for (j = 0; j < n; j++)
if (j != k)
{
u = k * n + j;
a[u] = a[u] * a[l];
}
//以下两个嵌套循环对其他行进行消元
for (i = 0; i < n; i++)
if (i != k)
for (j = 0; j < n; j++)
if (j != k)
{
u = i * n + j;
a[u] = a[u] - a[i*n + k] * a[k*n + j];
}
for (i = 0; i < n; i++)
if (i != k)
{
u = i * n + k;
a[u] = -a[u] * a[l];
}
}
//将其他行中主元素所在列的元素消元为0,更新其他行中的元素
//并进行回代求逆得到单位矩阵、逆序恢复:对消元过程中所做的行列交换进行逆序操作
for (k = n - 1; k >= 0; k--)
{
if (js[k] != k)
for (j = 0; j <= n - 1; j++)
{
u = k * n + j;
v = js[k] * n + j;
p = a[u];
a[u] = a[v];
a[v] = p;
}
if (is[k] != k)
for (i = 0; i < n; i++)
{
u = i * n + k;
v = i * n + is[k];
p = a[u];
a[u] = a[v];
a[v] = p;
}
}
//释放内存
free(is);
free(js);
return a;
}
具体的项目文件可以私聊我。