空间后方交会的求解是一个非线性问题，通常采用最小二乘法进行迭代解算。下面我将详细介绍具体的求解步骤：
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!");
}
}
![[OS] vDSO + vvar（频繁调用的处理） | 存储：寄存器(高效)和栈（空间大）| ELF标准包装规范（加速程序加载）](https://i-blog.csdnimg.cn/img_convert/5e9339cb8d1041c1072eb4b1b3906960.png)
