共线方程求解外方位元素--单片空间后方交会
单片空间后方交会是利用三个不在一条直线的已知点计算相片外方位元素的方法。css
- 获取已知数据，包括：
- x[]、y[]：改正后的控制点的像方坐标
- X[]、Y[]、Z[]：控制点的物方坐标
- f: 相机焦距
在空间后方交会中，一般采用像片四个角上（或者更多控制点），同时采用最小二乘原理进行平差计算。
肯定未知数的初始值
在竖直摄影状况下，三个角元素初值都为0，三个线元素初始值：
java/************************************************************************/ /* 外方位元素的初值 */ /************************************************************************/ 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 ios
computeRMatrix(a1, a2, a3, mR);逐点计算像点坐标近似值web
/** *计算像平面的近似坐标 */ 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)); }组成偏差方程式
每一个点的偏差方程式：
markdown偏差方程的系数矩阵Ldom
/************************************************************************/ /* 偏差方程的系数矩阵L */ /************************************************************************/ for (int i = 0; i < 4; i++){ L[2 * i] = x[i] - _x[i]; L[2 * i + 1] = y[i] - _y[i]; }首先求解TZ：
svgfloat 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相似）
根据平差原理，法方程为：
这里采用高斯消元法进行求解，也能够采用逆矩阵求解。ui/************************************************************************/ /* 偏差方程的系数矩阵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]); }更新角度值spa
phi += Dx.read(3, 0); omega += Dx.read(4, 0); kapa += Dx.read(5, 0);
工程地址：共线方程求解外方位元素–单片空间后方交会.net
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;
}