摄影测量单像后方交会实验报告
一、理论基础
外方位元素是确定摄影瞬间像片在地面直角坐标系中空间位置和姿态的参数，根据6个外方位元素，可以恢复像片与目标间的位置关系，重建目标的模型，其重要性不言而喻。
单像后方交会，即根据像片覆盖范围内一定数量的分布合理的已知坐标的地面控制点，利用共线方程求解像片的外方位元素。
二、问题
三、源程序
#include<iostream>
#include<iomanip>
#include<string.h>
#include<math.h>
using namespace std;
void PrintArray(double* a, int b, int c); //输出矩阵
void TranspositionArray(double* a, double* aT, int b, int c); //转置矩阵
void MultiplyArray(double* a, double* b, double* c, int m, int n, int l); //矩阵相乘
void make2array(double**& a, int n); //矩阵扩充（进行初等变换）
void deletarray(double**& a, int n); //释放内存
int ConverseArray(double* ip, double* rp, int n); //矩阵求逆
void SubtratArray(double* a, double* b, double* c, int m, int n); //矩阵相减
int main()
{
//像点坐标
double ICP[4][2] = { -86.15,-68.99,
-53.40,82.21,
-14.78,-76.63,
10.46,64.43
};
//地面点坐标
double GCP[4][3] = { 36589.41, 25273.32,2195.17,
37631.08,31324.51,728.69,
39100.97,24934.98,2386.50,
40426.54,30319.81,757.31
};
//估算摄影比例尺
double scale = (GCP[0][0] - GCP[1][0]) / (ICP[0][0] - ICP[1][0])*1000;
double f = 0.15324;
//统一长度单位为米
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 2; j++)
{
ICP[i][j] /= 1000.0;
}
}
//定义外方位元素
double Xs = 0.0, Ys = 0.0, Zs = 0.0, FAI = 0.0, OMEGA = 0.0, KAPPA = 0.0;
//外方位元素的改正数
double delta[6] = { 0.0 };
//定义旋转矩阵
double Rotate[3][3] = { 0.0 };
//确定Xs,YS,Zs初值
for (int i = 0; i < 4; i++)
{
Xs = (Xs + GCP[i][0]) / 4.0;
Ys = (Ys + GCP[i][1]) / 4.0;
}
Zs = f * scale;
//输出外方位元素的初始值
cout << "外方位元素的初始值为：" << endl;
cout <<"Xs初值："<< Xs << " " <<"Ys初值：" <<Ys << " " << "Zs初值："<<Zs << endl;
cout << "FAI初值："<<FAI << " " << "OMEGA初值："<<OMEGA << " " <<"KAPPA初值："<< KAPPA << endl;
//定义
double x[4] = { 0 }, y[4] = { 0 }, A[8][6] = { 0 }, AT[6][8] = { 0 }, ATA[6][6] = { 0 }, ATAInv[6][6] = { 0 }, L[8] = { 0 }, ATAAT[6][8] = { 0 };
double v[8] = { 0 }, m0 = 0, m[6] = { 0 }, AX[8] = { 0 }, vv = 0;
int n = 0;//累计循环次数n
//主循环
while (1)
{
//求旋转矩阵
Rotate[0][0] = cos(FAI) * cos(KAPPA) - sin(FAI) * sin(OMEGA) * sin(KAPPA); //a1
Rotate[0][1] = (-1.0) * cos(FAI) * sin(KAPPA) - sin(FAI) * sin(OMEGA) * cos(KAPPA); //a2
Rotate[0][2] = (-1.0) * sin(FAI) * cos(OMEGA); //a3
Rotate[1][0] = cos(OMEGA) * sin(KAPPA); //b1
Rotate[1][1] = cos(OMEGA) * cos(KAPPA); //b2
Rotate[1][2] = (-1.0) * sin(OMEGA); //b3
Rotate[2][0] = sin(FAI) * cos(KAPPA) + cos(FAI) * sin(OMEGA) * sin(KAPPA); //c1
Rotate[2][1] = (-1.0) * sin(FAI) * sin(KAPPA) + cos(FAI) * sin(OMEGA) * cos(KAPPA); //c2
Rotate[2][2] = cos(FAI) * cos(OMEGA); //c3
for (int i = 0; i < 4; i++)
{
//共线方程
x[i] = (-1.0) * f * (Rotate[0][0] * (GCP[i][0] - Xs) + Rotate[1][0] * (GCP[i][1] - Ys)
+ Rotate[2][0] * (GCP[i][2] - Zs)) / (Rotate[0][2] * (GCP[i][0] - Xs)
+ Rotate[1][2] * (GCP[i][1] - Ys) + Rotate[2][2] * (GCP[i][2] - Zs));
y[i] = (-1.0) * f * (Rotate[0][1] * (GCP[i][0] - Xs) + Rotate[1][1] * (GCP[i][1] - Ys)
+ Rotate[2][1] * (GCP[i][2] - Zs)) / (Rotate[0][2] * (GCP[i][0] - Xs)
+ Rotate[1][2] * (GCP[i][1] - Ys) + Rotate[2][2] * (GCP[i][2] - Zs));
//矩阵LL=像点坐标-共线方程计算值
L[i * 2] = ICP[i][0] - x[i];
L[i * 2 + 1] = ICP[i][1] - y[i];
//偏导数矩阵
A[i * 2][0] = (-1.0) * f / (Zs - GCP[i][2]); //a11
A[i * 2][1] = 0.0; //a12
A[i * 2][2] = (-1.0) * x[i] / (Zs - GCP[i][2]); //a13
A[i * 2][3] = (-1.0) * f * (1 + (x[i] * x[i]) / (f * f)); //a14
A[i * 2][4] = (-1.0) * x[i] * y[i] / f; //a15
A[i * 2][5] = y[i]; //a16
A[i * 2 + 1][0] = 0.0; //a21
A[i * 2 + 1][1] = A[i * 2][0]; //a22
A[i * 2 + 1][2] = (-1.0) * y[i] / (Zs - GCP[i][2]); //a23
A[i * 2 + 1][3] = A[i * 2][4]; //a24
A[i * 2 + 1][4] = (-1.0) * f * (1 + (y[i] * y[i]) / (f * f)); //a25
A[i * 2 + 1][5] = (-1.0) * x[i]; //a26
}
//求矩阵A的转置
TranspositionArray(*A, *AT, 8, 6);
//矩阵A的转置与矩阵A相乘
MultiplyArray(*AT, *A, *ATA, 6, 8, 6);
//矩阵A的转置与矩阵A相乘的逆
ConverseArray(*ATA, *ATAInv, 6);
//矩阵A的转置与矩阵A相乘的逆与A的转职相乘
MultiplyArray(*ATAInv, *AT, *ATAAT, 6, 6, 8);
//求得误差方程的解
MultiplyArray(*ATAAT, L, delta, 6, 8, 1);
Xs += delta[0];
Ys += delta[1];
Zs += delta[2];
FAI += delta[3];
OMEGA += delta[4];
KAPPA += delta[5];
++n;
if ((fabs(delta[0]) <1e-4 ) && (fabs(delta[1]) < 1e-4) && (fabs(delta[2]) < 1e-4)&&n<1000)
{
break;
}
else if(n>=1000)
{
cout << "1000次循环未解出来" << endl;
}
}
cout << "总计" << n << "次循环" << endl;
MultiplyArray(*A, delta, AX, 8, 6, 1);
SubtratArray(AX, L, v, 8, 1);
for (int i = 0; i < 8; i++)
{
vv += v[i] * v[i];
}
m0 = sqrt(vv / 2);
for (int i = 0; i < 6; i++)
{
m[i] = m0 * sqrt(fabs(ATAInv[i][i]));
}
cout << "相片的外方位元素为：" << endl;
cout << "Xs=" << setprecision(7) << Xs << endl;
cout << "Ys=" << setprecision(7) << Ys << endl;
cout << "Zs=" << setprecision(7) << Zs << endl;
cout << "FAI=" << FAI << endl;
cout << "OMEGA=" << OMEGA << endl;
cout << "KAPPA=" << KAPPA << endl;
cout << "旋转矩阵R为：" << endl;
PrintArray(*Rotate, 3, 3);
return 0;
}
//输出矩阵
void PrintArray(double* a, int b, int c)
{
for (int i = 0; i < b; i++)
{
for (int j = 0; j < c; j++)
{
cout << a[i * c + j] << " ";
}
cout << endl;
}
}
//转置矩阵
void TranspositionArray(double* a, double* aT, int b, int c)
{
for (int i = 0; i < b; i++)
{
for (int j = 0; j < c; j++)
{
aT[j * b + i] = a[i * c + j];
}
}
}
//矩阵a，b相乘
void MultiplyArray(double* a, double* b, double* c, int m, int n, int l)
{
for (int i = 0; i < m * l; i++) //矩阵初始化为0
{
c[i] = 0;
}
for (int i = 0; i < m; i++)
{
for (int j = 0; j < l; j++)
{
for (int k = 0; k < n; k++)
{
c[i * l + j] += a[i * n + k] * b[k * l + j];
}
}
}
}
//把n阶方阵扩充为n×2n矩阵并初始化为0
void make2array(double**& a, int n)
{
int i, j;
a = new double* [n];
for (i = 0; i < n; i++)
{
a[i] = new double[2 * n];
}
for (i = 0; i < n; i++)
{
for (j = 0; j < 2 * n; j++)
{
a[i][j] = 0;
}
}
}
//释放内存
void deletarray(double**& a, int n)
{
int i;
for (i = 0; i < n; i++)
{
delete[]a[i];
}
delete[]a;
}
//利用初等行变换法进行矩阵求逆
int ConverseArray(double* ip, double* rp, int n)
{
double** mat = NULL; //做行变换的矩阵
int i, j, r;
double k, temp;
//初始化mat为0,大小为n*2n
make2array(mat, n);
for (i = 0; i < n; i++)
{
for (j = 0; j < n; j++)
{
mat[i][j] = ip[i * n + j];
}
mat[i][n + i] = 1; //初始化右侧的单位矩阵
}
//做初等行变换化为上三角阵
for (i = 0; i < n; i++)
{
if (mat[i][i] == 0) //第i行i列为0
{
for (j = i + 1; j < n; j++)
{
if (mat[j][i] != 0) //找一个非0行
{
for (r = i; r < 2 * n; r++)
{
temp = mat[j][r];
mat[j][r] = mat[i][r];
mat[i][r] = temp;
}
break; //跳出j循环
}
}
}
if (mat[i][i] == 0)
return 0; //行列式为0则返回
for (j = i + 1; j < n; j++)
{
if (mat[j][i] != 0) //i行i列下方的j行i列元素不为0
{
k = -mat[j][i] / mat[i][i]; //做行变换
for (r = i; r < 2 * n; r++)
mat[j][r] = mat[j][r] + k * mat[i][r];
}
}
}
//化成单位矩阵
for (i = n - 1; i >= 0; i--)
{
k = mat[i][i];
for (r = i; r < 2 * n; r++)
mat[i][r] = mat[i][r] / k;
for (j = i - 1; j >= 0; j--)
{
k = -mat[j][i];
for (r = i; r < 2 * n; r++)
mat[j][r] = mat[j][r] + k * mat[i][r];
}
}
//将结果输出
for (i = 0; i < n; i++)
for (j = 0; j < n; j++)
rp[i * n + j] = mat[i][j + n];
//mat矩阵n释放
deletarray(mat, n);
return 1;
}
//矩阵相减
void SubtratArray(double* a, double* b, double* c, int m, int n)
{
int i, j;
for (i = 0; i < m; i++)
{
for (j = 0; j < n; j++)
{
c[i * n + j] = a[i * n + j] - b[i * n + j];
}
}
}
四、说明
1.初值
代码如下（示例）：
double scale = (GCP[0][0] - GCP[1][0]) / (ICP[0][0] - ICP[1][0])*1000;
for (int i = 0; i < 4; i++)
{
Xs = (Xs + GCP[i][0]) / 4.0;
Ys = (Ys + GCP[i][1]) / 4.0;
}
Zs = f * scale;
Xs、Ys的初值为四个物点坐标求均值，Zs的初值为f乘以比例尺。本题目中比例尺未给出，由已知数据估算：两个地面控制点的x坐标差除以对应像点的x坐标差。初值的设定关系着外方位元素改正数的收敛性。如果初值不合理，计算结果发散，则会超出设定的循环次数（本程序为1000次），程序终止。
2.迭代次数
if ((fabs(delta[0]) <1e-4 ) && (fabs(delta[1]) < 1e-4) && (fabs(delta[2]) < 1e-4)&&n<1000)
{
break;
}
else if(n>=1000)
{
cout << "1000次循环未解出来" << endl;
}
迭代次数与计算结果精度有关。本程序结果精度为1e-3，如果要继续提高精度，可以适当减小Xs、Ys、Zs的改正数。
本程序的迭代次数为：
如果改为
if ((fabs(delta[0]) <1e-8 ) && (fabs(delta[1]) < 1e-8) && (fabs(delta[2]) < 1e-8)&&n<1000)
{
break;
}
else if(n>=1000)
{
cout << "1000次循环未解出来" << endl;
}
迭代次数为：
因此随着精度要求的提高，迭代次数增加。
3.结果
本程序的运行结果如下：
可见结果与题目提示的结果较好地符合。
4.讨论
4.1关于输出结果的有效数字位数：
cout << "Xs=" << setprecision(7) << Xs << endl;
cout << "Ys=" << setprecision(7) << Ys << endl;
cout << "Zs=" << setprecision(7) << Zs << endl;
通过改变setprecision（）里的值，可以改变输出结果的有效数字位数。本程序为保留七位有效数字。
4.2矩阵求逆：
本程序的矩阵求逆采用初等行变换法进行求逆，也可采用伴随矩阵法求逆。
4.3优化：
本程序的矩阵运算全部采用循环的方法进行计算，可以引入有关矩阵运算库进行简化。
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
