摄影测量之空间后方交会程序
1. 空间后方交会概念
以单幅影像为基础，从该影像所覆盖地面范围内的若干控制点的已知地面坐标和相应点的像坐标量测值出发，根据共线条件方程，运用最小二乘间接平差，求解该影像在航空摄影时刻的外方位元素 ，并进行精度评定。
2. 空间后方交会原理
参见相关摄影测量学课件或者百度百科https://baike.baidu.com/item/%E7%A9%BA%E9%97%B4%E5%90%8E%E6%96%B9%E4%BA%A4%E4%BC%9A/8747035?fr=aladdin
3. 空间后方交会程序设计
1、首先编写一个处理矩阵的模块文件：MatrixOperations.h和MatrixOperations.cpp
MatrixOperations.h
#pragma once
//transpose数组将from进行转置，转置结果存储在to里
//from为row行、col列
void transpose(double* from, double* to, int row, int col);
//矩阵a与矩阵b相乘，相乘的结果放在矩阵c里
//矩阵a为m行n列，矩阵b为n行、l列
void multiply(double* a, double* b, double* c, int m, int n, int l);
//矩阵a与矩阵b相加，结果存放在矩阵a中
void mat_add(double* a, double* b, int m, int n);
void make2array(double**& a, int n);
void deletarray(double**& a, int n);
//矩阵求逆
//ip为要求逆的矩阵，rp是返回的逆矩阵，矩阵维数为n
//行列式为0，返回0；否则返回值为1
int converse(double* ip, double* rp, int n);
MatrixOperations.cpp
#include "MatrixOperations.h"
//transpose数组将from进行转置，转置结果存储在to里
//from为row行、col列
void transpose(double* from, double* to, int row, int col)
{
int i, j;
for (i = 0; i < row; i++)
{
for (j = 0; j < col; j++)
{
to[j * row + i] = from[i * col + j];
}
}
}
//矩阵a与矩阵b相乘，相乘的结果放在矩阵c里
//矩阵a为m行n列，矩阵b为n行、l列
void multiply(double* a, double* b, double* c, int m, int n, int l)
{
int i, j, k;
for (i = 0; i < m * l; i++)
{
c[i] = 0;
}
for (i = 0; i < m; i++)
{
for (j = 0; j < l; j++)
{
for (k = 0; k < n; k++)
{
c[i * l + j] += a[i * n + k] * b[k * l + j];
}
}
}
}
//矩阵a与矩阵b相加，结果存放在矩阵a中
void mat_add(double* a, double* b, int m, int n)
{
int i, j;
for (i = 0; i < m; i++)
{
for (j = 0; j < n; j++)
{
a[i * n + j] += b[i * n + j];
}
}
}
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;
}
//矩阵求逆
//ip为要求逆的矩阵，rp是返回的逆矩阵，矩阵维数为n
//行列式为0，返回0；否则返回值为1
int converse(double* ip, double* rp, int n)
{
double** mat; //做行变换的矩阵
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矩阵释放
deletarray(mat, n);
return 1;
}
2、然后编写主程序main.cpp，求解该影像在航空摄影时刻的外方位元素 ，并进行精度评定。
#include<stdio.h>
#include<math.h>
#include"MatrixOperations.h"
#define N 4
#define f 153.24E-3
#define m0 40000
#pragma warning(disable : 4996)
int main()
{
double x[N], y[N], X[5], Y[5], Z[5], G[N], V[2 * N][1], Vt[1][2 * N], vtpv[1][1];
double a[3], b[3], c[3], sgm, m[6];
double Xs = 0, Ys = 0, Zs = 0, phi = 0, omega = 0, kappa = 0;
double lx[4], ly[4], L[2 * N][1], A[2 * N][6];
double B[6][2 * N], C[6][6], D[6][6], E[6][2 * N], Xx[6][1];
double v[100][5]; //开一个足够大的数组。
int i, j, k;
FILE* fp; //文件指针
fp = fopen("rsdata.txt", "r");//以文本方式打开文件。
if (fp == NULL) //打开文件出错。
return -1;
for (i = 0; i < N; i++)
for (j = 0; j < 5; j++)
fscanf(fp, "%lf ", &v[i][j]); //读取数据到数组，直到文件结尾(返回EOF)
fclose(fp); //关闭文件
for (i = 0; i < N; i++) {
x[i] = v[i][0] * 1e-3;
y[i] = v[i][1] * 1e-3;
X[i] = v[i][2];
Y[i] = v[i][3];
Z[i] = v[i][4];
}
for (i = 0; i < N; i++) {
Xs += X[i] / N;
Ys += Y[i] / N;
}
Zs = m0 * f;
printf("\n\n\n外方位元素的初始值为:\n\n");
printf(" Xs = %12.6lf\n", Xs);
printf(" Ys = %12.6lf\n", Ys);
printf(" Zs = %12.6lf\n", Zs);
printf(" phi = %12.6lf\n", phi);
printf("omega= %12.6lf\n", omega);
printf("kappa= %12.6lf\n", kappa);
do
{
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);
for (i = 0; i < N; i++) {
lx[i] = x[i] + f * (a[0] * (X[i] - Xs) + b[0] * (Y[i] - Ys) + c[0] * (Z[i] - Zs)) / (a[2] * (X[i] - Xs) + b[2] * (Y[i] - Ys) + c[2] * (Z[i] - Zs));
ly[i] = y[i] + f * (a[1] * (X[i] - Xs) + b[1] * (Y[i] - Ys) + c[1] * (Z[i] - Zs)) / (a[2] * (X[i] - Xs) + b[2] * (Y[i] - Ys) + c[2] * (Z[i] - Zs));
G[i] = a[2] * (X[i] - Xs) + b[2] * (Y[i] - Ys) + c[2] * (Z[i] - Zs);
} //G即Z拔
for (i = 0, k = 0; i < N; i++) {
L[k++][0] = lx[i];
L[k++][0] = ly[i];
}
for (i = 0, k = 0; k < 2 * N; i++, k += 2) {
A[k][0] = (a[0] * f + a[2] * x[i]) / G[i];
A[k][1] = (b[0] * f + b[2] * x[i]) / G[i];
A[k][2] = (c[0] * f + c[2] * x[i]) / G[i];
A[k][3] = b[0] * x[i] * y[i] / f - b[1] * (f + x[i] * x[i] / f) - b[2] * y[i];
A[k][4] = -f * sin(kappa) - x[i] * (x[i] * sin(kappa) + y[i] * cos(kappa)) / f;
A[k][5] = y[i];
A[k + 1][0] = (a[1] * f + a[2] * y[i]) / G[i];
A[k + 1][1] = (b[1] * f + b[2] * y[i]) / G[i];
A[k + 1][2] = (c[1] * f + c[2] * y[i]) / G[i];
A[k + 1][3] = b[0] * (f + y[i] * y[i] / f) - b[1] * x[i] * y[i] / f + b[2] * x[i];
A[k + 1][4] = -f * cos(kappa) - y[i] * (x[i] * sin(kappa) + y[i] * cos(kappa)) / f;
A[k + 1][5] = -x[i];
} //构建系数矩阵A
transpose(*A, *B, 2 * N, 6);
multiply(*B, *A, *C, 6, 2 * N, 6);
converse(*C, *D, 6);
multiply(*D, *B, *E, 6, 6, 2 * N);
multiply(*E, *L, *Xx, 6, 2 * N, 1);
Xs = Xs + Xx[0][0];
Ys = Ys + Xx[1][0];
Zs = Zs + Xx[2][0];
phi = phi + Xx[3][0];
omega = omega + Xx[4][0];
kappa = kappa + Xx[5][0];
} while ((fabs(Xx[3][0]) > 1E-6) && (fabs(Xx[4][0]) > 1E-6) && (fabs(Xx[5][0]) > 1E-6));
multiply(*A, *Xx, *V, 2 * N, 6, 1);
for (i = 0; i < 2 * N; i++)
V[i][0] = V[i][0] - L[i][0];
transpose(*V, *Vt, 2 * N, 1);
multiply(*Vt, *V, *vtpv, 1, 2 * N, 1);
sgm = sqrt(vtpv[0][0] / (2 * N - 6));
for (i = 0; i < 6; i++)
m[i] = sgm * sqrt(D[i][i]); //空间后方交会的精度估算
printf("\n\n像片的外方位元素为:\n\n");
printf(" Xs = %12.6lf\t", Xs);
printf("m=%7.6lf\n", m[0]);
printf(" Ys = %12.6lf\t", Ys);
printf("m=%7.6lf\n", m[1]);
printf(" Zs = %12.6lf\t", Zs);
printf("m=%7.6lf\n", m[2]);
printf(" phi = %12.6lf\t", phi);
printf("m=%7.6lf\n", m[3]);
printf("omega= %12.6lf\t", omega);
printf("m=%7.6lf\n", m[4]);
printf("kappa= %12.6lf\t", kappa);
printf("m=%7.6lf\n", m[5]); //输出像片的外方位元素和中误差
printf("\n\n旋转矩阵R为:\n");
printf("\n");
for (i = 0; i < 3; i++)
printf("%12.6lf\t", a[i]);
printf("\n");
for (i = 0; i < 3; i++)
printf("%12.6lf\t", b[i]);
printf("\n");
for (i = 0; i < 3; i++)
printf("%12.6lf\t", c[i]);
printf("\n"); //输出旋转矩阵R
return 0;
}
3、需要读入的的数据是像坐标量测值和已知的相应点的地面坐标值文件rsdata.txt
-86.15 -68.99 36589.41 25273.32 2195.17
-53.40 82.21 37631.08 31324.51 728.69
-14.78 -76.63 39100.97 24934.98 2386.50
10.46 64.43 40426.54 30319.81 757.31
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
