/// <summary>
/// 计算旋转矩阵R
/// </summary>
/// <returns>R阵</returns>
private Matrix MatrixR(BackMatch b)
{
double[,]
Rp =
{
{ Cos(b.p), 0, -Sin(b.p) },
{ 0, 1, 0 },
{ Sin(b.p), 0, Cos(b.p) }
},
Rw =
{
{ 1, 0, 0 },
{ 0, Cos(b.w), -Sin(b.w) },
{ 0, Sin(b.w), Cos(b.w) }
},
Rk =
{
{ Cos(b.k), -Sin(b.k), 0 },
{ Sin(b.k), Cos(b.k), 0 },
{ 0, 0, 1 }
};
Matrix mp = new Matrix(Rp, true),
mw = new Matrix(Rw, true),
mk = new Matrix(Rk, true);
return(mp * mw * mk);
}
/// <summary>
/// 前方交会
/// </summary>
/// <param name="left">左影像的后方交会结果</param>
/// <param name="right">右影像的后方交会结果</param>
/// <param name="tar">要匹配的点</param>
/// <param name="cam">相机参数</param>
public FrontMatch(BackMatch left, BackMatch right, List <DataList> tar, CameraPara cam)
{
this.HasProcessed = false;
this._l = left;
this._r = right;
this._cam = cam;
this._targetList = tar;
_l0 = (cam.WidthPix - 1) / 2.0 + cam.MainPosX / cam.PixSize;
_h0 = (cam.HeightPix - 1) / 2.0 + cam.MainPosY / cam.PixSize;
MakeTargetList(tar);
this._r1r2 = GetR1R2();
this._bUvw = CalcBaselineB();
}