Example #1
0
        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))));
        }
Example #2
0
        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);
        }
Example #3
0
        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));
        }
Example #4
0
        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,
            });
        }