コード例 #1
0
        private static Matrix <double> PseudoInverse(Svd <double> svd)
        {
            Matrix <double> W = svd.W();
            Vector <double> s = svd.S();

            // The first element of W has the maximum value.
            double tolerance = Precision.EpsilonOf(2) * Math.Max(svd.U().RowCount, svd.VT().ColumnCount) * W[0, 0];

            for (int i = 0; i < s.Count; i++)
            {
                if (s[i] < tolerance)
                {
                    s[i] = 0;
                }
                else
                {
                    s[i] = 1 / s[i];
                }
            }

            W.SetDiagonal(s);

            // (U * W * VT)T is equivalent with V * WT * UT
            return((svd.U() * W * svd.VT()).Transpose());
        }
コード例 #2
0
ファイル: Matlab.cs プロジェクト: Volador17/OilCute
        /// <summary>
        /// Moore–Penrose pseudoinverse
        /// If A = U • Σ • VT is the singular value decomposition of A, then A† = V • Σ† • UT.
        /// For a diagonal matrix such as Σ, we get the pseudoinverse by taking the reciprocal of each non-zero element
        /// on the diagonal, leaving the zeros in place, and transposing the resulting matrix.
        /// In numerical computation, only elements larger than some small tolerance are taken to be nonzero,
        /// and the others are replaced by zeros. For example, in the MATLAB or NumPy function pinv,
        /// the tolerance is taken to be t = ε • max(m,n) • max(Σ), where ε is the machine epsilon. (Wikipedia)
        /// </summary>
        /// <param name="M">The matrix to pseudoinverse</param>
        /// <returns>The pseudoinverse of this Matrix</returns>
        public static Matrix PseudoInverse(this Matrix M)
        {
            Svd    D = M.Svd(true);
            Matrix W = (Matrix)D.W();
            Vector s = (Vector)D.S();

            // The first element of W has the maximum value.
            double tolerance = Precision.EpsilonOf(2) * Math.Max(M.RowCount, M.ColumnCount) * W[0, 0];


            for (int i = 0; i < s.Count; i++)
            {
                if (s[i] < tolerance)
                {
                    s[i] = 0;
                }
                else
                {
                    s[i] = 1 / s[i];
                }
            }
            W.SetDiagonal(s);

            // (U * W * VT)T is equivalent with V * WT * UT
            return((Matrix)(D.U() * W * D.VT()).Transpose());
        }
コード例 #3
0
        public static Transform Kabsch(List <Point3d> P, List <Point3d> Q)
        {
            Transform xf = Transform.Identity;

            if (P.Count != Q.Count)
            {
                return(xf);
            }
            Point3d CenP = new Point3d(), CenQ = new Point3d();

            for (int i = 0; i < P.Count; i++)
            {
                CenP += P[i];
                CenQ += Q[i];
            }
            CenP /= P.Count; CenQ /= Q.Count;
            DenseMatrix MX = new DenseMatrix(P.Count, 3);
            DenseMatrix MY = new DenseMatrix(P.Count, 3);

            for (int i = 0; i < P.Count; i++)
            {
                DenseVector v1 = new DenseVector(3);
                DenseVector v2 = new DenseVector(3);
                v1[0] = P[i].X - CenP.X; v2[0] = Q[i].X - CenQ.X;
                v1[1] = P[i].Y - CenP.Y; v2[1] = Q[i].Y - CenQ.Y;
                v1[2] = P[i].Z - CenP.Z; v2[2] = Q[i].Z - CenQ.Z;
                MX.SetRow(i, v1); MY.SetRow(i, v2);
            }
            DenseMatrix     H   = DenseMatrix.OfMatrix(MX.TransposeThisAndMultiply(MY));
            Svd             svd = H.Svd(true);
            Matrix <double> UT  = svd.U().Transpose();
            Matrix <double> V   = svd.VT().Transpose();
            Matrix <double> R   = V.Multiply(UT);
            double          d   = R.Determinant();

            if (d > 0)
            {
                d = 1;
            }
            else
            {
                d = -1;
            }
            DenseMatrix I = new DenseMatrix(3, 3, new double[] { 1, 0, 0, 0, 1, 0, 0, 0, d });

            R      = V.Multiply(I).Multiply(UT);
            xf.M00 = R[0, 0]; xf.M01 = R[0, 1]; xf.M02 = R[0, 2];
            xf.M10 = R[1, 0]; xf.M11 = R[1, 1]; xf.M12 = R[1, 2];
            xf.M20 = R[2, 0]; xf.M21 = R[2, 1]; xf.M22 = R[2, 2];
            CenP.Transform(xf);
            Transform tf = Transform.Translation(CenQ - CenP);

            return(Transform.Multiply(tf, xf));
        }
コード例 #4
0
        public override void Estimate(List <Point3D> datas)
        {
            double sum_x = 0;
            double sum_y = 0;
            double sum_z = 0;

            foreach (Point3D temp in datas)
            {
                sum_x += temp.x;
                sum_y += temp.y;
                sum_z += temp.z;
            }
            sum_x /= datas.Count;
            sum_y /= datas.Count;
            sum_z /= datas.Count;

            DenseMatrix jacobian = new DenseMatrix(datas.Count, 3);

            foreach (Point3D temp in datas)
            {
                Vector <double> gradient = new DenseVector(3);
                gradient[0] = temp.x - sum_x;
                gradient[1] = temp.y - sum_y;
                gradient[2] = temp.z - sum_z;
                jacobian.SetRow(datas.IndexOf(temp), gradient);
            }
            Svd svd = jacobian.Svd(true);
            // get matrix of left singular vectors with first n columns of U
            Matrix <double> U1 = svd.U().SubMatrix(0, datas.Count, 0, 3);
            // get matrix of singular values
            Matrix <double> S = new DiagonalMatrix(3, 3, svd.S().ToArray());
            // get matrix of right singular vectors
            Matrix <double> V = svd.VT().Transpose();

            Vector <double> parameters = new DenseVector(3);

            parameters = V.Column(0);
            x          = sum_x;
            y          = sum_y;
            z          = sum_z;
            i          = parameters[0];
            j          = parameters[1];
            k          = parameters[2];
        }
コード例 #5
0
        public static void LinePlaneEstimate(List <Point3d> datas, out Line line, out Plane plane)
        {
            Point3d     cen      = Point3d.GetCenter(datas);
            DenseMatrix jacobian = new DenseMatrix(datas.Count, 3);

            foreach (Point3d temp in datas)
            {
                Vector <double> gradient = new DenseVector(3);
                gradient[0] = temp.X - cen.X;
                gradient[1] = temp.Y - cen.Y;
                gradient[2] = temp.Z - cen.Z;
                jacobian.SetRow(datas.IndexOf(temp), gradient);
            }
            Svd             svd         = jacobian.Svd(true);
            Matrix <double> V           = svd.VT().Transpose();
            Vector <double> parameters  = V.Column(2);
            Vector <double> parameters2 = V.Column(0);

            plane = new Plane(cen, new Vector3d(parameters[0], parameters[1], parameters[2]));
            line  = new Line(cen, cen + new Vector3d(parameters2[0], parameters2[1], parameters2[2]));
        }
コード例 #6
0
        public Transform KabschEstimate(List <Point3d> P, List <Point3d> Q)
        {
            /*
             * 一组空间点匹配另一组空间点 kabsch算法
             * 将两组点的中心算出来 ,然后平移中点到原点
             * 协方差矩阵 H = X * Y.transpose();
             * [U,S,VT]=SVD(H)
             * I=(V*UT).Determinant判断方向
             * 旋转R = V*I*UT;
             * 平移T = -R * cenA + cenB;
             * 新点坐标P=P*T*R
             */
            Transform xf = Transform.Identity;

            if (P.Count != Q.Count)
            {
                return(xf);
            }
            Point3d CenP = new Point3d(); Point3d CenQ = new Point3d();

            for (int i = 0; i < P.Count; i++)
            {
                CenP += P[i];
                CenQ += Q[i];
            }
            CenP /= P.Count; CenQ /= Q.Count;
            DenseMatrix MX = new DenseMatrix(P.Count, 3);
            DenseMatrix MY = new DenseMatrix(P.Count, 3);

            for (int i = 0; i < P.Count; i++)
            {
                DenseVector v1 = new DenseVector(3);
                DenseVector v2 = new DenseVector(3);
                v1[0] = P[i].X - CenP.X; v2[0] = Q[i].X - CenQ.X;
                v1[1] = P[i].Y - CenP.Y; v2[1] = Q[i].Y - CenQ.Y;
                v1[2] = P[i].Z - CenP.Z; v2[2] = Q[i].Z - CenQ.Z;
                MX.SetRow(i, v1); MY.SetRow(i, v2);
            }
            DenseMatrix     H   = DenseMatrix.OfMatrix(MX.TransposeThisAndMultiply(MY));
            Svd             svd = H.Svd(true);
            Matrix <double> UT  = svd.U().Transpose();
            Matrix <double> V   = svd.VT().Transpose();
            Matrix <double> R   = V.Multiply(UT);
            double          d   = R.Determinant();

            if (d > 0)
            {
                d = 1;
            }
            else
            {
                d = -1;
            }
            DenseMatrix I = new DenseMatrix(3, 3, new double[] { 1, 0, 0, 0, 1, 0, 0, 0, d });

            R      = V.Multiply(I).Multiply(UT);
            xf.M00 = R[0, 0]; xf.M01 = R[0, 1]; xf.M02 = R[0, 2];
            xf.M10 = R[1, 0]; xf.M11 = R[1, 1]; xf.M12 = R[1, 2];
            xf.M20 = R[2, 0]; xf.M21 = R[2, 1]; xf.M22 = R[2, 2];
            CenP.Transform(xf);
            Transform tf = Transform.Translation(CenQ - CenP);

            return(Transform.Multiply(tf, xf));
        }