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, }); }