unit SingleCross;
{-------------------------------------------------
摄影测量单像空间会方交会算法单元
== ◎◎◎ 作者：陈明(龙在天) ◎◎◎ ==
== ◎◎◎ 任课老师： 邓 非 ◎◎◎ ==
QQ:810492306
--------------------------------------------------}
interface
uses SysUtils,Matrice,PerlRegEx,Classes,Dialogs;
type KZPoint=record //地面控制点坐标
X:extended;
Y:extended;
Z:extended;
end;
type YXPoint=record //影像点坐标
X:extended;
Y:extended;
end;
type IData=record //三个输入不方便的角度
ψ:extended;
ω:extended;
κ:extended;
end;
var m, f, x0, y0, Xs, Ys, Zs:extended; //m,f,x0,y0为内方位元素
PointNum:integer; //控制点的个数
LowLimit:extended; //精度控制
a1, a2, a3, b1, b2, b3, c1, c2, c3:extended; //R矩阵各值
KZPs:array of KZPoint; //用户输入的已知控制点
YXPs:array of YXPoint; //用户输入的已知像点
F3:IData; //三个输入不方便的角度
A,X,L:RCOMat; //平差矩阵, X=[dXs,dYs,dZs,dψ,dω,dκ]T; A[2n,6], L[2n,1]
var splitStr:string=' *'; //正则表达式的分隔符
//===========================从文本文件中读取初始数据===========================
procedure DataFromText(filename:string);
//===========================初始化数据=========================================
procedure InitData;
//===========================计算R矩阵==========================================
procedure CalcR;
//===========================计算L矩阵==========================================
procedure CalcL;
//===========================计算A矩阵==========================================
procedure CalcA;
//===========================计算X矩阵==========================================
procedure CalcX;
//===========================循环计算直到误差够小===============================
procedure Calc;
implementation
uses Unit1;
//===========================从文本文件中读取初始数据===========================
procedure DataFromText(filename:string);
var f:TextFile;
Reg:TPerlRegEx;
s:string;
List:TStrings;
rows,i:integer;
begin
Reg:=TPerlRegEx.Create(nil);
List:=TStringList.Create;
assignfile(f,filename);
reset(f);
s:=''; rows:=0;
while not Eof(f) do
begin
Readln(f,s); s:=trim(s); //第一行
while s='' do begin readln(f,s); s:=trim(s); end; //处理空行
rows:=rows+1;
end;
SetLength(KZPs,rows);
SetLength(YXPs,rows);
PointNum:=rows;
Reset(f);
for I := 0 to rows - 1 do
begin
Readln(f,s); s:=Trim(s);
while s='' do begin readln(f,s); s:=trim(s); end;
reg.Subject := UTF8string(s);
reg.RegEx :=UTF8string(splitStr);
reg.Split(list,MaxInt);
KZPs[i].X:=StrToFloat(List[1]);
//Form1.Memo1.Lines.Add(FloatToStr(Kzps[i].X));
KZPs[i].Y:=StrToFloat(List[2]);
//Form1.Memo1.Lines.Add(FloatToStr(Kzps[i].Y));
KZPs[i].Z:=StrToFloat(List[3]);
//Form1.Memo1.Lines.Add(FloatToStr(Kzps[i].Z));
YXPs[i].X:=StrToFloat(List[4]) / 1000;
//Form1.Memo1.Lines.Add(FloatToStr(Yxps[i].X));
YXPs[i].Y:=StrToFloat(List[5]) / 1000;
//Form1.Memo1.Lines.Add(FloatToStr(yxps[i].y));
List.Clear;
end;
Reg.Free;
List.Free;
CloseFile(f);
end;
//===========================初始化数据=========================================
procedure InitData;
var tempX,tempY:extended;
I: Integer;
begin
tempX:=0; tempY:=0;
F3.ψ :=0;
F3.ω :=0;
F3.κ :=0;
for I := 0 to PointNum - 1 do
begin
tempX:=tempX+KZPs[i].X;
tempY:=tempX+KZPs[i].Y;
end;
Zs:=m*f;
Xs:=tempX / PointNum;
Ys:=tempY / PointNum;
end;
//===========================计算R矩阵==========================================
procedure CalcR;
begin
a1:=Cos(F3.ψ)*Cos(F3.κ)-Sin(F3.ψ)*Sin(F3.ω)*Sin(F3.κ);
a2:=-Cos(F3.ψ)*Sin(f3.κ)-Sin(f3.ψ)*Sin(f3.ω)*Cos(f3.κ);
a3:=-Sin(f3.ψ)*Cos(f3.ω);
b1:=Cos(f3.ω)*Sin(f3.κ);
b2:=Cos(f3.ω)*Cos(f3.κ);
b3:=-Sin(f3.ω);
c1:=Sin(f3.ψ)*Cos(f3.κ)+Cos(f3.ψ)*Sin(f3.ω)*Sin(f3.κ);
c2:=-Sin(f3.ψ)*Sin(f3.κ)+Cos(f3.ψ)*Sin(f3.ω)*Cos(f3.κ);
c3:=Cos(f3.ψ)*Cos(f3.ω);
end;
//===========================计算L矩阵==========================================
procedure CalcL;
var JSx, JSy:extended; //近似值
I: Integer;
begin
SetLength(L,2*PointNum,1); //L[2n,1]
for I := 0 to PointNum - 1 do
begin
JSx:=x0/1000-f*( (a1*(KZPs[i].X-Xs) + b1*(KZPs[i].Y-Ys) + c1*(KZPs[i].Z-Zs) ) / ( a3*(KZPs[i].X-Xs) + b3*(KZPs[i].Y-Ys) + c3*(KZPs[i].Z-Zs) ) );
JSy:=y0/1000-f*( (a2*(KZPs[i].X-Xs) + b2*(KZPs[i].Y-Ys) + c2*(KZPs[i].Z-Zs) ) / ( a3*(KZPs[i].X-Xs) + b3*(KZPs[i].Y-Ys) + c3*(KZPs[i].Z-Zs) ) );
L[2*i,0]:=YXPs[i].X-JSx;
L[2*i+1,0]:=YXPs[i].Y-JSy;
end;
end;
//===========================计算A矩阵==========================================
procedure CalcA;
var a11, a12, a13, a21, a22, a23, a14, a15, a16, a24, a25, a26,Z1:extended;
i:integer;
begin
SetLength(A,2*PointNum,6); //A[2n,6]
for I := 0 to PointNum - 1 do
begin
Z1:=a3*(KZPs[i].X-Xs) + b3*(KZPs[i].Y-Ys) + c3*(KZPs[i].Z-Zs);
a11:=(a1*f+a3*YXPs[i].X) / Z1;
a12:=(b1*f+b3*YXPs[i].X) / Z1;
a13:=(c1*f+c3*YXPs[i].X) / Z1;
a21:=(a2*f+a3*YXPs[i].Y) / Z1;
a22:=(b2*f+b3*YXPs[i].Y) / Z1;
a23:=(c2*f+c3*YXPs[i].Y) / Z1;
a14:=YXPs[i].Y*Sin(f3.ω)-( (YXPs[i].X*(YXPs[i].X*Cos(f3.κ)-YXPs[i].Y*Sin(f3.κ)))/f+f*Cos(f3.κ) )*Cos(f3.ω);
a15:=-f*Sin(f3.κ)-YXPs[i].X*(YXPs[i].X*Sin(f3.κ)+YXPs[i].Y*Cos(f3.κ)) / f;
a16:=YXPs[i].Y;
a24:=-YXPs[i].X*Sin(f3.ω)-( (YXPs[i].X*(YXPs[i].X*Cos(f3.κ)-YXPs[i].Y*Sin(f3.κ)))/f-f*Sin(f3.κ) )*Cos(f3.ω);
a25:=-f*Cos(f3.κ)-YXPs[i].Y*(YXPs[i].X*Sin(f3.κ)+YXPs[i].Y*Cos(f3.κ)) / f;
a26:=-YXPs[i].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;
end;
end;
//===========================计算X矩阵==========================================
procedure CalcX;
var AT,ATA,ATA1,ATA1AT:RCOMat;
det:extended;
begin
InitMat(2*PointNum);
SetLength(AT,6,2*PointNum);
TransMat(A,AT,2*PointNum,6); //AT=A的转置
SetLength(ATA,6,6);
InitMat(6);
MatMatR(AT,A,2*PointNum,6,ATA); //ATA[6,6]=AT*A
SetLength(ATA1,6,6);
InitMat(6);
InvMat(ATA,ATA1,det); //ATA1=ATA的逆
SetLength(ATA1AT,6,2*PointNum);
InitMat(6);
MatMatR(ATA1,AT,6,2*PointNum,ATA1AT);//ATA1AT[6,2*n]=ATA1*AT
SetLength(X,6,1);
InitMat(6);
MatMatR(ATA1AT,L,2*PointNum,1,X); //最后求得X
end;
//===========================循环计算直到误差够小===============================
procedure Calc;
var rs:string;
times:integer;
begin
times:=0;
repeat
CalcR;
CalcL;
CalcA;
CalcX;
Xs:=Xs+X[0,0]; Ys:=Ys+X[1,0]; Zs:=Zs+X[2,0];
f3.ψ:=f3.ψ+X[3,0]; f3.ω:=f3.ω+X[4,0]; f3.κ:=f3.κ+X[5,0];
times:=times+1;
Form1.Memo1.Lines.Add('第'+IntToStr(times)+'次迭代============');
Form1.Memo1.Lines.Add('Xs='+FloatToStr(Xs)+';Ys='+FloatToStr(Ys)+';Zs='+FloatToStr(Zs)+';');
Form1.Memo1.Lines.Add('ψ='+FloatToStr(f3.ψ)+';ω='+FloatToStr(f3.ω)+';κ='+FloatToStr(f3.κ)+';');
Form1.Memo1.Lines.Add('dXs='+FloatToStr(X[0,0])+';dYs='+FloatToStr(X[1,0])+';dZs='+FloatToStr(X[2,0])+';dψ='+FloatToStr(X[3,0])+';dω='+FloatToStr(X[4,0])+';dκ='+FloatToStr(X[5,0]));
Form1.Memo1.Lines.Add('');
until (abs(X[0,0])<LowLimit) and (abs(X[1,0])<LowLimit) and (abs(X[2,0])<LowLimit) and (abs(X[3,0])<LowLimit) and (abs(X[4,0])<LowLimit) and (abs(X[5,0])<LowLimit);
rs:=rs+FloatToStr(Xs)+' '+FloatToStr(Ys)+' '+FloatToStr(Zs)+#13#10;
rs:=rs+FloatToStr(f3.ψ)+' '+FloatToStr(f3.ω)+FloatToStr(f3.κ)+#13#10;
rs:=rs+IntToStr(times);
ShowMessage(rs);
end;
end.
用法说明：
procedure TForm1.FormCreate(Sender: TObject);
begin
Form1.Position:=poScreenCenter;
DataFromText('data.txt');
m:=50000; f:=0.0281359; x0:=0; y0:=0; LowLimit:=0.000029;
InitData;
Calc;
end;