예제 #1
0
        static public void PlaneFit(IList <Matrix> X, out Matrix R, out Matrix t, out Matrix d2)
        {
            int n = X.Count;

            var mu = new Matrix(3, 1);

            for (int i = 0; i < n; i++)
            {
                mu.Add(X[i]);
            }
            mu.Scale(1f / (float)n);

            var A  = new Matrix(3, 3);
            var xc = new Matrix(3, 1);
            var M  = new Matrix(3, 3);

            for (int i = 0; i < X.Count; i++)
            {
                var x = X[i];
                xc.Sub(x, mu);
                M.Outer(xc, xc);
                A.Add(M);
            }
            var V = new Matrix(3, 3);
            var d = new Matrix(3, 1);

            A.Eig(V, d); // eigenvalues in ascending order

            // arrange in descending order so that z = 0
            var V2 = new Matrix(3, 3);

            for (int i = 0; i < 3; i++)
            {
                V2[i, 2] = V[i, 0];
                V2[i, 1] = V[i, 1];
                V2[i, 0] = V[i, 2];
            }

            d2    = new Matrix(3, 1);
            d2[2] = d[0];
            d2[1] = d[1];
            d2[0] = d[2];

            R = new Matrix(3, 3);
            R.Transpose(V2);

            if (R.Det3x3() < 0)
            {
                R.Scale(-1);
            }

            t = new Matrix(3, 1);
            t.Mult(R, mu);
            t.Scale(-1);

            // eigenvalues are the sum of squared distances in each direction
            // i.e., min eigenvalue is the sum of squared distances to the plane = d2[2]

            // compute the distance to the plane by transforming to the plane and take z-coordinate:
            // xPlane = R*x + t; distance = xPlane[2]
        }
예제 #2
0
        static public void PlaneFit(IList<Matrix> X, out Matrix R, out Matrix t, out Matrix d2)
        {
            int n = X.Count;

            var mu = new Matrix(3, 1);
            for (int i = 0; i < n; i++)
                mu.Add(X[i]);
            mu.Scale(1f / (float)n);

            var A = new Matrix(3, 3);
            var xc = new Matrix(3, 1);
            var M = new Matrix(3, 3);
            for (int i = 0; i < X.Count; i++)
            {
                var x = X[i];
                xc.Sub(x, mu);
                M.Outer(xc, xc);
                A.Add(M);
            }
            var V = new Matrix(3, 3);
            var d = new Matrix(3, 1);
            A.Eig(V, d); // eigenvalues in ascending order

            // arrange in descending order so that z = 0
            var V2 = new Matrix(3, 3);
            for (int i = 0; i < 3; i++)
            {
                V2[i, 2] = V[i, 0];
                V2[i, 1] = V[i, 1];
                V2[i, 0] = V[i, 2];
            }

            d2 = new Matrix(3, 1);
            d2[2] = d[0];
            d2[1] = d[1];
            d2[0] = d[2];

            R = new Matrix(3, 3);
            R.Transpose(V2);

            if (R.Det3x3() < 0)
                R.Scale(-1);

            t = new Matrix(3, 1);
            t.Mult(R, mu);
            t.Scale(-1);

            // eigenvalues are the sum of squared distances in each direction
            // i.e., min eigenvalue is the sum of squared distances to the plane = d2[2]

            // compute the distance to the plane by transforming to the plane and take z-coordinate:
            // xPlane = R*x + t; distance = xPlane[2]
        }
예제 #3
0
        public static double PlaneFit(IList<Matrix> points, out Matrix X, out double D)
        {
            X = new Matrix(3, 1);

            var mu = new RoomAliveToolkit.Matrix(3, 1);
            for (int i = 0; i < points.Count; i++)
                mu.Add(points[i]);
            mu.Scale(1f / (float)points.Count);

            var A = new RoomAliveToolkit.Matrix(3, 3);
            var pc = new RoomAliveToolkit.Matrix(3, 1);
            var M = new RoomAliveToolkit.Matrix(3, 3);
            for (int i = 0; i < points.Count; i++)
            {
                var p = points[i];
                pc.Sub(p, mu);
                M.Outer(pc, pc);
                A.Add(M);
            }

            var V = new RoomAliveToolkit.Matrix(3, 3);
            var d = new RoomAliveToolkit.Matrix(3, 1);
            A.Eig(V, d); // TODO: replace with 3x3 version?

            //Console.WriteLine("------");
            //Console.WriteLine(A);
            //Console.WriteLine(V);
            //Console.WriteLine(d);

            double minEigenvalue = Double.MaxValue;
            int minEigenvaluei = 0;
            for (int i = 0; i < 3; i++)
                if (d[i] < minEigenvalue)
                {
                    minEigenvalue = d[i];
                    minEigenvaluei = i;
                }

            X.CopyCol(V, minEigenvaluei);

            D = -X.Dot(mu);

            // min eigenvalue is the sum of squared distances to the plane
            // signed distance is: double distance = X.Dot(point) + D;

            return minEigenvalue;
        }