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 GMatrix GetH1(PointFloat e, PointFloat imgSize, GMatrix F, GMatrix H2,
PointFloat[] leftPts, PointFloat[] rightPts)
{
var ex = new GMatrix(new double[, ]
{
{ 0, -1, e.Y },
{ 1, 0, -e.X },
{ -e.Y, e.X, 0 }
});
var e_111 = new GMatrix(new double[, ]
{
{ e.X, e.X, e.X },
{ e.Y, e.Y, e.Y },
{ 1, 1, 1 },
});
var m = ex.dot(F).add(e_111);
var h0h = H2.dot(m);
var m1 = perspectiveTransform(leftPts, h0h);
var m2 = perspectiveTransform(rightPts, H2);
var abuf = new double[leftPts.Length, 3];
for (var i = 0; i < leftPts.Length; i++)
{
var pt = leftPts[i];
abuf[i, 0] = pt.X;
abuf[i, 1] = pt.Y;
abuf[i, 2] = 1;
}
var A = new GMatrix(abuf);
var svcr = JacobSvd.JacobiSVD(A);
double[] B = new double[rightPts.Length];
for (var i = 0; i < rightPts.Length; i++)
{
B[i] = m2[i].X;
}
var x = Util.SVBkSb(svcr.U, svcr.W, svcr.Vt, B);
var Ha = new GMatrix(new double[, ]
{
{ x[0], x[1], x[2] },
{ 0, 1, 0 },
{ 0, 0, 1 }
});
var res = Ha.dot(h0h);
return(res);
}
public static GMatrix EstimateHomography(PointFloat[] points, int w = 6, int h = 3)
{
var pos = new PointFloat[w * h];
int at = 0;
for (var j = 0; j < h; j++)
{
for (var i = 0; i < w; i++)
{
pos[at++] = new PointFloat(i, j);
}
}
return(EstimateHomography(points, pos));
}
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,
});
}