摄影测量——单像空间后方交会

空间后方交会的求解是一个非线性问题，通常采用最小二乘法进行迭代解算。下面我将详细介绍具体的求解步骤：

1. 基本公式（共线条件方程）

共线条件方程是后方交会的基础：

复制

x - x₀ = -f * [m₁₁(X-Xₛ) + m₁₂(Y-Yₛ) + m₁₃(Z-Zₛ)] / [m₃₁(X-Xₛ) + m₃₂(Y-Yₛ) + m₃₃(Z-Zₛ)]
y - y₀ = -f * [m₂₁(X-Xₛ) + m₂₂(Y-Yₛ) + m₂₃(Z-Zₛ)] / [m₃₁(X-Xₛ) + m₃₂(Y-Yₛ) + m₃₃(Z-Zₛ)]

其中mᵢⱼ是旋转矩阵元素，由三个外方位角元素(φ,ω,κ)决定。

2. 求解步骤

步骤1：数据准备

• 已知：至少3个地面控制点的地面坐标(X,Y,Z)和对应的像点坐标(x,y)

• 已知：相机的内方位元素(x₀,y₀,f)

• 需要：提供外方位元素的初始近似值(Xₛ⁰,Yₛ⁰,Zₛ⁰,φ⁰,ω⁰,κ⁰)

步骤2：线性化处理

将共线方程在近似值处进行泰勒展开，保留一阶项：

复制

vₓ = a₁₁ΔXₛ + a₁₂ΔYₛ + a₁₃ΔZₛ + a₁₄Δφ + a₁₅Δω + a₁₆Δκ - lₓ
vᵧ = a₂₁ΔXₛ + a₂₂ΔYₛ + a₂₃ΔZₛ + a₂₄Δφ + a₂₅Δω + a₂₆Δκ - lᵧ

其中：

• vₓ,vᵧ为残差

• aᵢⱼ为偏导数系数

• lₓ,lᵧ为常数项（观测值与计算值之差）

步骤3：建立误差方程

对于n个控制点，可建立2n个误差方程，矩阵形式为：

复制

V = AΔ - L

其中：

• V为残差向量

• A为设计矩阵（偏导数矩阵）

• Δ为外方位元素改正数向量

• L为常数项向量

步骤4：组成法方程并求解

法方程为：

复制

AᵀPAΔ = AᵀPL

解为：

Δ = (AᵀPA)⁻¹AᵀPL

其中P为权矩阵，通常初始设为单位矩阵。

步骤5：更新参数

Xₛ¹ = Xₛ⁰ + ΔXₛ
Yₛ¹ = Yₛ⁰ + ΔYₛ
Zₛ¹ = Zₛ⁰ + ΔZₛ
φ¹ = φ⁰ + Δφ
ω¹ = ω⁰ + Δω
κ¹ = κ⁰ + Δκ

步骤6：迭代计算

重复步骤2-5，直到改正数Δ小于设定的阈值（如1e-6）。

3. 偏导数计算

设计矩阵A中的偏导数系数计算如下：

4. 精度评定

单位权中误差：

σ₀ = √(VᵀPV)/(2n-6)

参数协方差矩阵：

Dₓₓ = σ₀²(AᵀPA)⁻¹

5. C#代码

输入文件格式：

using System;
using System.Collections.Generic;
using System.IO;
using MathNet.Numerics.LinearAlgebra;// 定义 Points 结构体
public struct Points
{public double x0;public double y0;public double X;public double Y;public double Z;
}// 定义 OuterElements 结构体
public struct OuterElements
{public double f;public double Xs;public double Ys;public double Zs;public double phi;public double omega;public double kappa;
}// 定义 Accurracy 结构体
public struct Accurracy
{public double sigema;public double[] m;public Accurracy(){sigema = 0;m = new double[6];}
}class Program
{// 从文件中读取数据static int ReadFile(string filePath, out List<Points> p, out OuterElements o){p = new List<Points>();o = new OuterElements();try{string[] lines = File.ReadAllLines(filePath);int linenumber = 0;Points p0 = new Points();foreach (string line in lines){linenumber++;if (linenumber >= 2 && linenumber <= 5){string[] values = line.Split(new char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);p0.x0 = double.Parse(values[0]);p0.y0 = double.Parse(values[1]);p0.X = double.Parse(values[2]);p0.Y = double.Parse(values[3]);p0.Z = double.Parse(values[4]);p.Add(p0);}else if (linenumber == 7){string[] values = line.Split(new char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);o.f = double.Parse(values[0]);}else if (linenumber == 10){string[] values = line.Split(new char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);o.Xs = double.Parse(values[0]);o.Ys = double.Parse(values[1]);o.Zs = double.Parse(values[2]);o.phi = double.Parse(values[3]);o.omega = double.Parse(values[4]);o.kappa = double.Parse(values[5]);}}return 1;}catch (Exception){Console.WriteLine("file open error");return 0;}}// 构建旋转矩阵static Matrix<double> BuildRotationMatrix(OuterElements eo){Matrix<double> R = Matrix<double>.Build.Dense(3, 3);double cos_phi = Math.Cos(eo.phi);double sin_phi = Math.Sin(eo.phi);double cos_omega = Math.Cos(eo.omega);double sin_omega = Math.Sin(eo.omega);double cos_kappa = Math.Cos(eo.kappa);double sin_kappa = Math.Sin(eo.kappa);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;R[0, 0] = a1; R[0, 1] = b1; R[0, 2] = c1;R[1, 0] = a2; R[1, 1] = b2; R[1, 2] = c2;R[2, 0] = a3; R[2, 1] = b3; R[2, 2] = c3;return R;}// 构建系数矩阵static void BuildCoefficientsMatrix(List<Points> p, OuterElements eo, Matrix<double> R, out Matrix<double> A, out Matrix<double> L){int n = p.Count;double f = eo.f;double a1, a2, a3, b1, b2, b3, c1, c2, c3;double a11, a12, a13, a14, a15, a16;double a21, a22, a23, a24, a25, a26;A = Matrix<double>.Build.Dense(2 * n, 6);L = Matrix<double>.Build.Dense(2 * n, 1);for (int i = 0; i < p.Count; i++){Vector<double> V = Vector<double>.Build.Dense(3);V[0] = p[i].X - eo.Xs;V[1] = p[i].Y - eo.Ys;V[2] = p[i].Z - eo.Zs;Vector<double> Xmean = R * V;double x_approx = -f * Xmean[0] / Xmean[2];double y_approx = -f * Xmean[1] / Xmean[2];double obs_x = p[i].x0 / 1000.0;double obs_y = p[i].y0 / 1000.0;L[2 * i, 0] = obs_x - x_approx;L[2 * i + 1, 0] = obs_y - y_approx;a1 = R[0, 0]; b1 = R[0, 1]; c1 = R[0, 2];a2 = R[1, 0]; b2 = R[1, 1]; c2 = R[1, 2];a3 = R[2, 0]; b3 = R[2, 1]; c3 = R[2, 2];double denom = Xmean[2];double x = x_approx;double y = y_approx;a11 = (a1 * f + a3 * x) / denom;a12 = (b1 * f + b3 * x) / denom;a13 = (c1 * f + c3 * x) / denom;a21 = (a2 * f + a3 * y) / denom;a22 = (b2 * f + b3 * y) / denom;a23 = (c2 * f + c3 * y) / denom;double term1_x = y * Math.Sin(eo.omega);double term2_x = (x / f * (x * Math.Cos(eo.kappa) - y * Math.Sin(eo.kappa)) + f * Math.Cos(eo.kappa)) * Math.Cos(eo.omega);a14 = term1_x - term2_x;a15 = -f * Math.Sin(eo.kappa) - (x / f) * (x * Math.Sin(eo.kappa) + y * Math.Cos(eo.kappa));a16 = y;double term1_y = -x * Math.Sin(eo.omega);double term2_y = (y / f * (x * Math.Cos(eo.kappa) - y * Math.Sin(eo.kappa)) - f * Math.Sin(eo.kappa)) * Math.Cos(eo.omega);a24 = term1_y - term2_y;a25 = -f * Math.Cos(eo.kappa) - (y / f) * (x * Math.Sin(eo.kappa) + y * Math.Cos(eo.kappa));a26 = -x;A[2 * i, 0] = a11; A[2 * i, 1] = a12; A[2 * i, 2] = a13;A[2 * i, 3] = a14; A[2 * i, 4] = a15; A[2 * i, 5] = a16;A[2 * i + 1, 0] = a21; A[2 * i + 1, 1] = a22; A[2 * i + 1, 2] = a23;A[2 * i + 1, 3] = a24; A[2 * i + 1, 4] = a25; A[2 * i + 1, 5] = a26;}}// 最小二乘法static int LeastSquares(List<Points> p, ref OuterElements o, ref Accurracy acc, string outFilePath){double threshold = 1e-6;int n = p.Count;int NumberofIteration = 0;Vector<double> x = Vector<double>.Build.Dense(6, 1);Matrix<double> R = Matrix<double>.Build.Dense(3, 3);Matrix<double> A = Matrix<double>.Build.Dense(2 * n, 6);Matrix<double> L = Matrix<double>.Build.Dense(2 * n, 1);Matrix<double> V = Matrix<double>.Build.Dense(2 * n, 1);Matrix<double> Q = Matrix<double>.Build.Dense(6, 6);while (!x.All(d => d < threshold)){R = BuildRotationMatrix(o);BuildCoefficientsMatrix(p, o, R, out A, out L);Q = (A.Transpose() * A).Inverse();x = Q * (A.Transpose() * L);V = A * x - L;o.Xs += x[0];o.Ys += x[1];o.Zs += x[2];o.phi += x[3];o.omega += x[4];o.kappa += x[5];Accuracy_assessment(V, Q, ref acc);NumberofIteration++;if (NumberofIteration > 10){Console.WriteLine("Not converging!");return 0;}Save_Adjustment_report(outFilePath, o, acc, NumberofIteration);}return 1;}// 精度评估static void Accuracy_assessment(Matrix<double> V, Matrix<double> Q, ref Accurracy acc){int n = V.RowCount / 2;double m0 = 0.0;acc.sigema = Math.Sqrt((V.Transpose() * V)[0, 0] / (2 * n - 6));for (int i = 0; i < 6; i++){m0 = acc.sigema * Math.Sqrt(Q[i, i]);acc.m[i] = m0;}}// 初始化外方位元素static void Initialize_Outerelements(List<Points> p, ref OuterElements o){double X_all = 0.0;double Y_all = 0.0;int n = p.Count;for (int i = 0; i < n; i++){X_all += p[i].X;Y_all += p[i].Y;}o.Xs = X_all / n;o.Ys = Y_all / n;o.Zs = 50000 * o.f;o.phi = 0.0;o.omega = 0.0;o.kappa = 0.0;}// 保存调整报告static void Save_Adjustment_report(string outFilePath, OuterElements o, Accurracy acc, int NumberofIteration){using (StreamWriter writer = new StreamWriter(outFilePath, true)){writer.WriteLine($"Iteration: {NumberofIteration}");writer.WriteLine($"Xs: {o.Xs}, Ys: {o.Ys}, Zs: {o.Zs}, phi: {o.phi}, omega: {o.omega}, kappa: {o.kappa}");writer.WriteLine($"sigema: {acc.sigema}");for (int i = 0; i < 6; i++){writer.WriteLine($"m[{i}]: {acc.m[i]}");}}}static void Main(){List<Points> p;OuterElements o = new OuterElements();Accurracy acc = new Accurracy();string filename = "input.txt";string outfilename = "result.txt";if (ReadFile(filename, out p, out o) == 1){o.f = o.f / 1000.0;LeastSquares(p, ref o, ref acc, outfilename);}Console.WriteLine("The calculation is complete!");}
}