public static GMatrix GetH2(PointFloat e, PointFloat imgSize)
{
var w2 = -imgSize.X / 2;
var h2 = -imgSize.Y / 2;
GMatrix T = new GMatrix(new double[, ] {
{ 1, 0, w2 },
{ 0, 1, h2 },
{ 0, 0, 1 }
});
var e1 = e.X + w2;
var e2 = e.Y + h2;
var l = Math.Sqrt((e1 * e1) + (e2 * e2));
var alph = e1 >= 0 ? 1 : -1;
GMatrix R = new GMatrix(new double[, ] {
{ alph *e1 / l, alph *e2 / l, 0 },
{ -1 * alph * e2 / l, alph * e1 / l, 0 },
{ 0, 0, 1 }
});
GMatrix G = new GMatrix(new double[, ]
{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ -1 / e1, 0, 1 }
});
return(GMatrix.Inverse3x3(T).dot(G.dot(R.dot(T))));
}
public static RectifyResult Rectify(List <StereoPoints> allPts, PointFloat imgSize, int CalibGridRow = 6, int CalibGridCol = 3)
{
var leftPts = allPts.SelectMany(x => x.Left).ToArray();
var rightPts = allPts.SelectMany(x => x.Right).ToArray();
var F = Calib.CalcFundm(leftPts, rightPts);
var epol = CalibRect.FindEpipole(leftPts, PointSide.Left, F);
var h2 = CalibRect.GetH2(epol, new PointFloat(imgSize.X, imgSize.Y));
var h1 = CalibRect.GetH1(epol, new PointFloat(imgSize.X, imgSize.Y), F, h2, allPts[0].Left, allPts[0].Right);
Func <Func <StereoPoints, PointFloat[]>, PointFloat[][]> fetch = f =>
{
PointFloat[][] res = new PointFloat[allPts.Count][];
for (var i = 0; i < allPts.Count; i++)
{
res[i] = f(allPts[i]);
}
return(res);
};
var li = Calib.EstimateIntranics(fetch(x => x.Left), CalibGridRow, CalibGridCol);
var ri = Calib.EstimateIntranics(fetch(x => x.Right), CalibGridRow, CalibGridCol);
var E = GMatrix.Inverse3x3(li).tranpose().dot(F).dot(GMatrix.Inverse3x3(ri));
return(new RectifyResult
{
H1 = h1,
H2 = h2,
F = F,
E = E,
el = epol,
LeftIntrinics = li,
RightIntrinics = ri,
});
}
