单片空间后方交会是利用三个不在一条直线的已知点计算相片外方位元素的方法。
获取已知数据，包括：
x[]、y[]：改正后的控制点的像方坐标X[]、Y[]、Z[]：控制点的物方坐标f: 相机焦距
在空间后方交会中，通常采用像片四个角上（或者更多控制点），同时采用最小二乘原理进行平差计算。
确定未知数的初始值
在竖直摄影情况下，三个角元素初值都为0，三个线元素初始值：
/************************************************************************//* 外方位元素的初值 *//************************************************************************/for (int i = 0; i < 4; i++){Xs += X[i];Ys += Y[i];Zs += Z[i];}Xs = Xs / 4;Ys = Ys / 4;Zs = Zs / 4 + H;a1 = 0.0f;a2 = 0.0f;a3 = 0.0f;
进入循环，角度改正数小于指定值(一般为0.1)退出循环
计算旋转矩阵R
computeRMatrix(a1, a2, a3, mR);
逐点计算像点坐标近似值
/***计算像平面的近似坐标*/for (int i = 0; i < 4; i++){_x[i] = -f*(R[0][0] * (X[i] - Xs) + R[1][0] * (Y[i] - Ys) + R[2][0] * (Z[i] - Zs))/ (R[0][2] * (X[i] - Xs) + R[1][2] * (Y[i] - Ys) + R[2][2] * (Z[i] - Zs));_y[i] = -f*(R[0][1] * (X[i] - Xs) + R[1][1] * (Y[i] - Ys) + R[2][1] * (Z[i] - Zs))/ (R[0][2] * (X[i] - Xs) + R[1][2] * (Y[i] - Ys) + R[2][2] * (Z[i] - Zs));}
组成误差方程式
每个点的误差方程式：
误差方程的系数矩阵L
/************************************************************************//* 误差方程的系数矩阵L*//************************************************************************/ for (int i = 0; i < 4; i++){L[2 * i] = x[i] - _x[i]; L[2 * i + 1] = y[i] - _y[i]; }
首先求解TZ：
float TZ = R[0][2] * (X[i / 2] - Xs) + R[1][2] * (Y[i / 2] - Ys) + R[2][2] * (Z[i / 2] - Zs);
偏导XS求解（YS、ZS类似）
偏导phi求解（偏导omega、偏导kapa类似）
根据平差原理，法方程为：
这里采用高斯消元法进行求解，也可以采用逆矩阵求解。
/************************************************************************//* 误差方程的系数矩阵A*//************************************************************************/ for (int i = 0; i < 8; i = i + 2) {float TZ = R[0][2] * (X[i / 2] - Xs) + R[1][2] * (Y[i / 2] - Ys) + R[2][2] * (Z[i / 2] - Zs); A[i][0] = 1 / TZ*(R[0][0] * f + R[0][2] * _x[i / 2]); mA.write(i, 0, A[i][0]); A[i][1] = 1 / TZ*(R[1][0] * f + R[1][2] * _x[i / 2]); mA.write(i, 1, A[i][1]); A[i][2] = 1 / TZ*(R[2][0] * f + R[2][2] * _x[i / 2]); mA.write(i, 2, A[i][2]); A[i][3] = _y[i / 2] * sin(omega) - (_x[i / 2] / f*(_x[i / 2] * cos(kapa) - _y[i / 2] * sin(kapa)) + f*cos(kapa))*cos(omega); mA.write(i, 3, A[i][3]); A[i][4] = -f*sin(kapa) - _x[i / 2] / f*(_x[i / 2] * sin(kapa) + _y[i / 2] * cos(kapa));; mA.write(i, 4, A[i][4]); A[i][5] = _y[i / 2]; mA.write(i, 5, A[i][5]); }
更新角度值
phi += Dx.read(3, 0);omega += Dx.read(4, 0);kapa += Dx.read(5, 0);
工程地址：共线方程求解外方位元素–单片空间后方交会
main.cpp
#include "_Matrix.h"#include <iostream>#include <cmath>#include <fstream>using namespace std;_Matrix_Calc matixCalc;_Matrix mA(8, 6), mAT(6, 8), mATA(6, 6), mATL(6, 1), mL(8, 1), Dx(6, 1), mR(3, 3);float f = 0.15324f;float m = 0;float x[] = { -0.08615f, -0.05340f, -0.01478f, 0.01046f };float y[] = { -0.06899f, 0.08221f, -0.07663f, 0.06443f };float X[] = { 36589.41f, 37631.08f, 39100.97f, 40426.54f };float Y[] = { 25273.32f, 31324.51f, 24934.98f, 30319.81f };float Z[] = { 2195.17f, 728.69f, 2386.50f, 757.31f };float Xs, Ys, Zs, dXs, dYs, dZs;float phi, omega, kapa, dphi, domega, dkapa;float H;float R[3][3];float _x[4], _y[4];float A[8][6];float L[8];void computeRMatrix(float&phi, float&omega, float&kapa, _Matrix&m){R[0][0] = cos(phi)*cos(kapa) - sin(phi)*sin(omega)*sin(kapa); m.write(0, 0, R[0][0]);R[0][1] = -cos(phi)*sin(kapa) - sin(phi)*sin(omega)*cos(kapa); m.write(0, 1, R[0][1]);R[0][2] = -sin(phi)*cos(omega); m.write(0, 2, R[0][2]);R[1][0] = cos(omega)*sin(kapa); m.write(1, 0, R[1][0]);R[1][1] = cos(omega)*cos(kapa); m.write(1, 1, R[1][1]);R[1][2] = -sin(omega); m.write(1, 2, R[1][2]);R[2][0] = sin(phi)*cos(kapa) + cos(phi)*sin(omega)*sin(kapa); m.write(2, 0, R[2][0]);R[2][1] = -sin(phi)*sin(kapa) + cos(phi)*sin(omega)*cos(kapa); m.write(2, 1, R[2][1]);R[2][2] = cos(phi)*cos(omega); m.write(2, 2, R[2][2]);}int main(){mA.init_matrix();mAT.init_matrix();mATA.init_matrix();mATL.init_matrix();mL.init_matrix();Dx.init_matrix();mR.init_matrix();float temp1 = 0, temp2 = 0;for (int i = 0; i < 3; i++){temp1 += sqrt(pow(x[i] - x[i + 1], 2) + pow(y[i] - y[i + 1], 2));temp2 += sqrt(pow(X[i] - X[i + 1], 2) + pow(Y[i] - Y[i + 1], 2));}m = temp2 / temp1;H = m*f;for (int i = 0; i < 4; i++){Xs += X[i];Ys += Y[i];Zs += Z[i];}Xs = Xs / 4;Ys = Ys / 4;Zs = Zs / 4 + H;phi = 0.0f;omega = 0.0f;kapa = 0.0f;int num = 0;while (num == 0 || abs(Dx.read(3, 0)) * 206265 > 0.1 || abs(Dx.read(4, 0)) * 206265 > 0.1 || abs(Dx.read(5, 0)) * 206265 > 0.1){computeRMatrix(phi, omega, kapa, mR);for (int i = 0; i < 4; i++){_x[i] = -f*(R[0][0] * (X[i] - Xs) + R[1][0] * (Y[i] - Ys) + R[2][0] * (Z[i] - Zs))/ (R[0][2] * (X[i] - Xs) + R[1][2] * (Y[i] - Ys) + R[2][2] * (Z[i] - Zs));_y[i] = -f*(R[0][1] * (X[i] - Xs) + R[1][1] * (Y[i] - Ys) + R[2][1] * (Z[i] - Zs))/ (R[0][2] * (X[i] - Xs) + R[1][2] * (Y[i] - Ys) + R[2][2] * (Z[i] - Zs));}for (int i = 0; i < 4; i++){L[2 * i] = x[i] - _x[i];L[2 * i + 1] = y[i] - _y[i];}mL.arr = L;for (int i = 0; i < 8; i = i + 2){float TZ = R[0][2] * (X[i / 2] - Xs) + R[1][2] * (Y[i / 2] - Ys) + R[2][2] * (Z[i / 2] - Zs);A[i][0] = 1 / TZ*(R[0][0] * f + R[0][2] * _x[i / 2]); mA.write(i, 0, A[i][0]);A[i][1] = 1 / TZ*(R[1][0] * f + R[1][2] * _x[i / 2]); mA.write(i, 1, A[i][1]);A[i][2] = 1 / TZ*(R[2][0] * f + R[2][2] * _x[i / 2]); mA.write(i, 2, A[i][2]);A[i][3] = _y[i / 2] * sin(omega) - (_x[i / 2] / f*(_x[i / 2] * cos(kapa) - _y[i / 2] * sin(kapa)) + f*cos(kapa))*cos(omega); mA.write(i, 3, A[i][3]);A[i][4] = -f*sin(kapa) - _x[i / 2] / f*(_x[i / 2] * sin(kapa) + _y[i / 2] * cos(kapa));; mA.write(i, 4, A[i][4]);A[i][5] = _y[i / 2]; mA.write(i, 5, A[i][5]);}for (int i = 1; i < 8; i = i + 2){float TZ = R[0][2] * (X[(i - 1) / 2] - Xs) + R[1][2] * (Y[(i - 1) / 2] - Ys) + R[2][2] * (Z[(i - 1) / 2] - Zs);A[i][0] = 1 / TZ*(R[0][1] * f + R[0][2] * _y[(i - 1) / 2]); mA.write(i, 0, A[i][0]);A[i][1] = 1 / TZ*(R[1][1] * f + R[1][2] * _y[(i - 1) / 2]); mA.write(i, 1, A[i][1]);A[i][2] = 1 / TZ*(R[2][1] * f + R[2][2] * _y[(i - 1) / 2]); mA.write(i, 2, A[i][2]);A[i][3] = _x[(i - 1) / 2] * sin(omega) - (_y[(i - 1) / 2] / f*(_x[(i - 1) / 2] * cos(kapa) - _y[(i - 1) / 2] * sin(kapa)) - f*sin(kapa))*cos(omega); mA.write(i, 3, A[i][3]);A[i][4] = -f*cos(kapa) - _y[(i - 1) / 2] / f*(_x[(i - 1) / 2] * sin(kapa) + _y[(i - 1) / 2] * cos(kapa)); mA.write(i, 4, A[i][4]);A[i][5] = -_x[(i - 1) / 2]; mA.write(i, 5, A[i][5]);}matixCalc.transpos(&mA, &mAT);matixCalc.multiply(&mAT, &mA, &mATA);matixCalc.multiply(&mAT, &mL, &mATL);matixCalc.Gauss_Col(&mATA, &mATL, &Dx);Xs += Dx.read(0, 0);Ys += Dx.read(1, 0);Zs += Dx.read(2, 0);phi += Dx.read(3, 0);omega += Dx.read(4, 0);kapa += Dx.read(5, 0);num++;}printf("一共迭代了%d次\n", num);printf("外方位元素\n");printf("%f %f %f \n", Xs, Ys, Zs);printf("%f %f %f \n", phi, omega, kapa);printf("旋转矩阵\n");mR.print();return 0;}
