单张像片空间后方交会
1、目的ios
最小二乘平差方法编程实现解求影像外方位元素。算法
利用必定数量的地面控制点，根据共线条件方程求解像片外方位元素，并评价其精度。编程
2、数据app
已知航摄仪内方位元素：
，摄影比例尺估计为1：15000。spa
下表为4个地面控制点的地面坐标及其对应像点的像片坐标。指针
3、原理code
共线方程为：blog
其中f为焦距，(x, y) 为像素坐标。旋转矩阵采用Y为主轴的ϕ,ω,κ 转角系统，即：io
因为共线方程为非线性方程，平差时首先须要进行线性化。用泰勒级数将(1)式线性化，便可获得用于平差的偏差方程式：class
其中常数项为：
式(4)能够写为 的形式，是典型的间接平差模型。
空间点坐标、外方位线元素和角元素改正数的系数为：
解算的流程图以下：
4、流程
(1)获取已知数据。从摄影资料中查取影像比例尺1/m；影像内方位元素x0，y0，f；获取控制点的地面坐标Xt，Yt，Zt。
(2)量测像点坐标。量测控制点的像平面坐标并进行必要的影像坐标系统偏差改正。
(3)肯定未知数的初始值。在竖直航空摄影且地面控制点大致对称分布的状况下， 和 取控制点地面坐标的均值， 为相对航高，φ、ω、κ的初值都设为0，或者κ的初值可在航迹图上找出或根据控制点坐标经过坐标正反变换求出。
(4)计算旋转矩阵R。利用角元素近似值计算方向余弦值，组成R阵。
(5)逐点计算像点坐标值。利用未知数的近似值按共线条件式计算控制点像点坐标值(x),(y)。
(6)逐点计算偏差方程式的系数和常数项，组成偏差方程式。
(7)计算法方程的系数矩阵与常数项，组成法方程式。
(8)解求外方位元素。根据法方程，解求外方位元素改正数，并与相应的近似值求和，获得外方位元素新的近似值。
(9)检查计算是否收敛。将所求得的外方位元素的改正数与规定的限差比较，一般对φ，ω，κ的改正数△φ，△ω，△κ给予 限差，一般为0.1′′，当3个改正数均小于0.1′′时，迭代结束。不然，用新的近似值重复(4)～(8)步骤的计算，直到知足要求为止。
5、程序源代码
环境： VS 2017
#include "pch.h"
#include <iostream>
#include <stdio.h>
using namespace std;
//fk,x0,y0为内方位元素，num为地面点数量，m为比例尺
const double f = 153.24 / 1000;
const double xx0 = 0, yy0 = 0;
const int n = 4, m = 15000;
void Zero(double *p, int n)
{
if (n == NULL)
return;
while (n--) *p++ = 0;
}
//设A为m,l阶矩阵, B为l,n阶矩阵, C为m,n阶矩阵, 计算 C=A x B的子程序为：
bool MatrixMulti(double * A, double * B, double * C, int M, int N, int L)
{
int i, j, k;
if (A == NULL || B == NULL || C == NULL) return false;
Zero(C, M*N);
for (i = 0; i < M; i++)
{
for (j = 0; j < N; j++)
{
//for (k=0;k<L;k++) C[i*N+j] += A[i*L+k]*B[k*N+j];
for (k = 0; k < L; k++) *(C + i * N + j) += *(A + i * L + k)* *(B + k * N + j); //等效！！！
}
}
return true;
}
//对称正定矩阵求逆 a为n*n阶对称正定矩阵，n为矩阵阶数
int MatrixInversion(double *a, int n)
{
int i, j, k, m;
double w, g, *b;
b = new double[n];
for (k = 0; k <= n - 1; k++)
{
w = a[0] + 1.0e-25;
m = n - k - 1;
for (i = 1; i < n; i++)
{
g = a[i * n];
b[i] = g / w;
if (i <= m)
b[i] = -b[i];
int tmpOffset1 = (i - 1) * n - 1;
int tmpOffset2 = i * n;
for (j = 1; j <= i; j++)
a[tmpOffset1 + j] = a[tmpOffset2 + j] + g * b[j];
}
a[n * n - 1] = 1.0 / w;
for (i = 1; i <= n - 1; i++)
a[(n - 1) * n + i - 1] = b[i];
}
for (i = 0; i <= n - 2; i++)
for (j = i + 1; j <= n - 1; j++)
a[i * n + j] = a[j * n + i];
delete b;
b = NULL;
return(2);
}
double* MatrixTranspose(double *a, int m, int n) {
double t = 0;
double *newMatrix = new double[m*n];
for (int i = 0; i < m; i++) {
for (int j = 0; j < n; j++) {
newMatrix[j*m + i] = a[i*n + j];
}
}
return newMatrix;
}
struct Point {
double x;
double y;
double X;
double Y;
double Z;
};
int main()
{
int t = 20;//最多迭代次数
//未知数
double Xs, Ys, Zs, H;
double phi, omega, kappa;
double error = 0, zeta = 0;
//初始化4个点
Point p[4];
p[0].x = -86.15, p[0].y = -68.99, p[0].X = 36589.41, p[0].Y = 25273.32, p[0].Z = 2195.17;
p[1].x = -53.40, p[1].y = 82.21, p[1].X = 37631.08, p[1].Y = 31324.51, p[1].Z = 728.69;
p[2].x = -14.78, p[2].y = -76.63, p[2].X = 39100.97, p[2].Y = 24934.98, p[2].Z = 2386.50;
p[3].x = 10.46, p[3].y = 64.43, p[3].X = 40426.54, p[3].Y = 30319.81, p[3].Z = 757.31;
Xs = (p[0].X + p[1].X + p[2].X + p[3].X) / n;
Ys = (p[0].Y + p[1].Y + p[2].Y + p[3].Y) / n;
Zs = H = m * f;
phi = omega = kappa = 0;
double data[6] = { Xs,Ys,Zs,phi,omega,kappa };
double Q[6] = { 0,0,0,0,0,0 };
double* v = new double[2 * n * 1];
do {
error = 0;
zeta = 0;
double* A = new double[2 * n * 6];
double* L = new double[2 * n * 1];
double* tempv = new double[2 * n * 1];
double* xMat = new double[6 * 1];
double* AtA = new double[6 * 6];
double* temp = new double[6 * 2 * n];
double a1, a2, a3, b1, b2, b3, c1, c2, c3;
double _X, _Y, _Z, X, Y, Z, x, y;
for (int i = 0; i < n; i++) {
a1 = cos(phi)*cos(kappa) - sin(phi)*sin(omega)*sin(kappa);
a2 = -cos(phi)*sin(kappa) - sin(phi)*sin(omega)*cos(kappa);
a3 = -sin(phi)*cos(omega);
b1 = cos(omega)*sin(kappa);
b2 = cos(omega)*cos(kappa);
b3 = -sin(omega);
c1 = sin(phi)*cos(kappa) + cos(phi)*sin(omega)*sin(kappa);
c2 = -sin(phi)*sin(kappa) + cos(phi)*sin(omega)*cos(kappa);
c3 = cos(phi)*cos(omega);
X = p[i].X;
Y = p[i].Y;
Z = p[i].Z;
x = p[i].x / 1000;
y = p[i].y / 1000;
//_X,_Y,_Z
_X = a1 * (X - Xs) + b1 * (Y - Ys) + c1 * (Z - Zs);
_Y = a2 * (X - Xs) + b2 * (Y - Ys) + c2 * (Z - Zs);
_Z = a3 * (X - Xs) + b3 * (Y - Ys) + c3 * (Z - Zs);
//A矩阵
A[0 + i * 12] = (a1*f + a3 * x) / _Z;
A[1 + i * 12] = (b1*f + b3 * x) / _Z;
A[2 + i * 12] = (c1*f + c3 * x) / _Z;
A[3 + i * 12] = y * sin(omega) - (x / f * (x*cos(kappa) - y * sin(kappa)) + f * cos(kappa))*cos(omega);
A[4 + i * 12] = -f * sin(kappa) - x / f * (x*sin(kappa) + y * cos(kappa));
A[5 + i * 12] = y;
A[6 + i * 12] = (a2*f + a3 * y) / _Z;
A[7 + i * 12] = (b2*f + b3 * y) / _Z;
A[8 + i * 12] = (c2*f + c3 * y) / _Z;
A[9 + i * 12] = -x * sin(omega) - (y / f * (x*cos(kappa) - y * sin(kappa)) - f * sin(kappa))*cos(omega);
A[10 + i * 12] = -f * cos(kappa) - y / f * (x*sin(kappa) + y * cos(kappa));
A[11 + i * 12] = -x;
//L矩阵
L[2 * i] = x + f * _X / _Z;
L[1 + 2 * i] = y + f * _Y / _Z;
}
/*设A为m,l阶矩阵, B为l,n阶矩阵, C为m,n阶矩阵, 计算 C=A x B的子程序为：
MatrixMulti(double * A, double * B, double * C, int M, int N, int L)
对称正定矩阵求逆 a为n*n阶对称正定矩阵，n为矩阵阶数
int MatrixInversion(double *a, int n)
MatrixTranspose(double *a, int m, int n) */
double* At = NULL;
At = MatrixTranspose(A, 2 * n, 6);
MatrixMulti(At, A, AtA, 6, 6, 2 * n);
MatrixInversion(AtA, 6);
for (int j = 0; j < 6; j++) {
Q[j] = AtA[7 * j];
}
MatrixMulti(AtA, At, temp, 6, 2 * n, 6);
MatrixMulti(temp, L, xMat, 6, 1, 2 * n);
MatrixMulti(A, xMat, tempv, 2 * n, 1, 6);
for (int j = 0; j < 2 * n; j++) {
zeta += pow(tempv[j] - L[j], 2);
v[j] = tempv[j] - L[j];
}
Xs += xMat[0];
Ys += xMat[1];
Zs += xMat[2];
phi += xMat[3];
omega += xMat[4];
kappa += xMat[5];
for (int i = 0; i < 6; i++)
error += pow(xMat[i], 2);
//if (error < 1e-11)
if ((xMat[3] < 1e-7) && (xMat[4] < 1e-7) && (xMat[5] < 1e-7))
break;
delete[] A, L, X, xMat, temp, AtA, tempv;
t--;
if (t == 0)
printf("结果不收敛！\n\n\n");
} while (t > 0);
//输出结果
if (t > 0) {
FILE *fp;//创建一个文件操做指针
fopen_s(&fp, "Output.txt", "w");
double data2[6] = { Xs,Ys,Zs,phi,omega,kappa };
printf("\n通过了%d次迭代计算，解算获得Xs=%.2lf,Ys=%.2lf,Zs=%.2lf\n", 20 - t, data2[0], data2[1], data2[2]);
fprintf(fp, "\n通过了%d次迭代计算，解算获得Xs=%.2lf,Ys=%.2lf,Zs=%.2lf\n", 20 - t, data2[0], data2[1], data2[2]);
printf("Phi=%lf rad,Omega=%lf rad,Kappa=%lf rad\n", data2[3],data2[4], data2[5]);
printf("平均比例尺为 1：%f\n\n", Zs / f);
fprintf(fp, "Phi=%lf rad,Omega=%lf rad,Kappa=%lf rad\n", data2[3], data2[4], data2[5]);
fprintf(fp, "平均比例尺为 1：%f\n\n", Zs / f);
double a1 = cos(phi)*cos(kappa) - sin(phi)*sin(omega)*sin(kappa);
double a2 = -cos(phi)*sin(kappa) - sin(phi)*sin(omega)*cos(kappa);
double a3 = -sin(phi)*cos(omega);
double b1 = cos(omega)*sin(kappa);
double b2 = cos(omega)*cos(kappa);
double b3 = -sin(omega);
double c1 = sin(phi)*cos(kappa) + cos(phi)*sin(omega)*sin(kappa);
double c2 = -sin(phi)*sin(kappa) + cos(phi)*sin(omega)*cos(kappa);
double c3 = cos(phi)*cos(omega);
printf("R矩阵为:\n %.5lf %.5lf %.5lf\n%.5lf %.5lf %.5lf\n%.5lf %.5lf %.5lf\n\n", a1, a2, a3, b1, b2, b3, c1, c2, c3);
fprintf(fp, "R矩阵为:\n %.5lf %.5lf %.5lf\n%.5lf %.5lf %.5lf\n%.5lf %.5lf %.5lf\n\n", a1, a2, a3, b1, b2, b3, c1, c2, c3);
zeta = sqrt(zeta / (2 * n - 6));
printf("单位权中偏差为：%lf\n\n", zeta);
printf("Xs精度为：%lfm\n", zeta*sqrt(Q[0]));
printf("Ys精度为：%lfm\n", zeta*sqrt(Q[1]));
printf("Zs精度为：%lfm\n", zeta*sqrt(Q[2]));
printf("Phi精度为：%lf rad\n", zeta*sqrt(Q[3]));
printf("Omega精度为：%lf rad\n", zeta*sqrt(Q[4]));
printf("Kappa精度为：%lf rad\n\n", zeta*sqrt(Q[5]));
fprintf(fp, "单位权中偏差为：%lf\n\n", zeta);
fprintf(fp, "Xs精度为：%lfm\n", zeta*sqrt(Q[0]));
fprintf(fp, "Ys精度为：%lfm\n", zeta*sqrt(Q[1]));
fprintf(fp, "Zs精度为：%lfm\n", zeta*sqrt(Q[2]));
fprintf(fp, "Phi精度为：%lf rad\n", zeta*sqrt(Q[3]));
fprintf(fp, "Omega精度为：%lf rad\n", zeta*sqrt(Q[4]));
fprintf(fp, "Kappa精度为：%lf rad\n\n", zeta*sqrt(Q[5]));
for (int i = 0; i < n; i++) {
printf("第%d个点的x坐标的改正为%lfmm，y坐标的改正为%lfmm\n", i + 1, v[i * 2] * 1000 + p[i].x, v[i * 2 + 1] * 1000 + p[i].y);
fprintf(fp, "第%d个点的x坐标的改正为%lfmm，y坐标的改正为%lfmm\n", i + 1, v[i * 2] * 1000 + p[i].x, v[i * 2 + 1] * 1000 + p[i].y);
}
fclose(fp);
}
delete[] v;
return 0;
}
6、结果
通过了7次迭代计算，解算获得Xs=39795.45,Ys=27476.46,Zs=7572.69
Phi=-0.003987 rad,Omega=0.002114 rad,Kappa=-0.067578 rad
平均比例尺为 1：49417.162057
R矩阵为:
0.99771 0.06753 0.00399
-0.06753 0.99772 -0.00211
-0.00412 0.00184 0.99999
单位权中偏差为：0.000007
Xs精度为：1.107388m
Ys精度为：1.249519m
Zs精度为：0.488128m
Phi精度为：0.000179 rad
Omega精度为：0.000161 rad
Kappa精度为：0.000072 rad
第1个点的x坐标的改正为-86.151300mm，y坐标的改正为-68.986648mm 第2个点的x坐标的改正为-53.406529mm，y坐标的改正为82.207327mm 第3个点的x坐标的改正为-14.778598mm，y坐标的改正为-76.630467mm 第4个点的x坐标的改正为10.466290mm，y坐标的改正为64.429027mm