Exemplo n.º 1
0
        private static JacobSvd.SvdRes SolveSvd(GMatrix m1)
        {
            //double max = 1;
            //double min = -1;
            //for (var i = 0; i < m1.rows; i++)
            //{
            //    for (var j = 0; j < m1.cols; j++)
            //    {
            //        var v = m1.storage[i][j];
            //        if (v > max) max = v;
            //        if (v < min) min = v;
            //    }
            //}
            //var scal = Math.Max(Math.Abs(min), max);
            //for (var i = 0; i < m1.rows; i++)
            //{
            //    for (var j = 0; j < m1.cols; j++)
            //    {
            //        var v = m1.storage[i][j];
            //        m1.storage[i][j] = v / scal;
            //    }
            //}
            var svdA = JacobSvd.JacobiSVD(m1);

            return(svdA);
        }
Exemplo n.º 2
0
        public static GMatrix CalcFundm(PointFloat[] points1, PointFloat[] points2)
        {
            var m1 = new GMatrix(points1.Length, 9);

            for (var i = 0; i < points1.Length; i++)
            {
                var pp1 = points1[i];
                var pp2 = points2[i];
                var y1  = pp1.X;
                var y2  = pp1.Y;
                var p1  = pp2.X;
                var p2  = pp2.Y;
                var cur = m1.storage[i];
                cur[0] = y1 * p1;
                cur[1] = p1 * y2;
                cur[2] = p1;
                cur[3] = p2 * y1;
                cur[4] = p2 * y2;
                cur[5] = p2;
                cur[6] = y1;
                cur[7] = y2;
                cur[8] = 1;
            }

            var Fhat = SolveSvd3x3(m1);

            var FhatSvd = JacobSvd.JacobiSVD(Fhat);
            var d       = FhatSvd.getWMat();

            d.storage[2][2] = 0;

            var res = FhatSvd.U.dot(d).dot(FhatSvd.Vt);

            return(res);
        }
Exemplo n.º 3
0
        public static PointFloat FindEpipole(PointFloat[] pts, PointSide side, GMatrix f)
        {
            if (side == PointSide.Right)
            {
                f = f.tranpose();
            }

            GMatrix lines   = new GMatrix(pts.Length, 3);
            var     storage = lines.storage;

            for (int i = 0; i < pts.Length; i++)
            {
                var cur = storage[i];
                var pt  = pts[i];
                var gm  = f.dot(new GMatrix(new double[3, 1] {
                    { pt.X }, { pt.Y }, { 1 }
                }));
                var ms = gm.storage;
                for (int j = 0; j < 3; j++)
                {
                    cur[j] = ms[j][0];
                }
            }

            var svdA = JacobSvd.JacobiSVD(lines);
            var res  = svdA.Vt.storage[2];

            //Console.WriteLine($"{res[0].ToString("0.00")},{res[1].ToString("0.00")},{res[2].ToString("0.0000000")}");
            //Console.WriteLine($"{(res[0]/res[2]).ToString("0.00")},{(res[1]/res[2]).ToString("0.00")}");
            return(new PointFloat((float)(res[0] / res[2]), (float)(res[1] / res[2])));
        }
Exemplo n.º 4
0
            public Ert GetRT(GMatrix m)
            {
                var svd = JacobSvd.JacobiSVD(m);
                var W   = new GMatrix(new double[, ]
                {
                    { 0, -1, 0 },
                    { 1, 0, 0 },
                    { 0, 0, 1 }
                });

                var Wt = new GMatrix(new double[, ]
                {
                    { 0, 1, 0 },
                    { -1, 0, 0 },
                    { 0, 0, 1 }
                });

                svd.W[2] = 0;
                var t = svd.U.dot(W).dot(svd.getWMat()).dot(svd.U.tranpose());
                var r = svd.U.dot(Wt).dot(svd.Vt);

                return(new Ert
                {
                    R = r,
                    T = t,
                });
            }
Exemplo n.º 5
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);
        }