Пример #1
0
        /// <summary>
        /// Initializes a new instance of the <see cref="UserQR"/> class. This object will compute the
        /// QR factorization when the constructor is called and cache it's factorization.
        /// </summary>
        /// <param name="matrix">The matrix to factor.</param>
        /// <param name="method">The QR factorization method to use.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="matrix"/> is <c>null</c>.</exception>
        public static UserQR Create(Matrix<Complex> matrix, QRMethod method = QRMethod.Full)
        {
            if (matrix.RowCount < matrix.ColumnCount)
            {
                throw Matrix.DimensionsDontMatch<ArgumentException>(matrix);
            }

            Matrix<Complex> q;
            Matrix<Complex> r;

            var minmn = Math.Min(matrix.RowCount, matrix.ColumnCount);
            var u = new Complex[minmn][];

            if (method == QRMethod.Full)
            {
                r = matrix.Clone();
                q = Matrix<Complex>.Build.SameAs(matrix, matrix.RowCount, matrix.RowCount);

                for (var i = 0; i < matrix.RowCount; i++)
                {
                    q.At(i, i, 1.0f);
                }

                for (var i = 0; i < minmn; i++)
                {
                    u[i] = GenerateColumn(r, i, i);
                    ComputeQR(u[i], r, i, matrix.RowCount, i + 1, matrix.ColumnCount, Control.MaxDegreeOfParallelism);
                }

                for (var i = minmn - 1; i >= 0; i--)
                {
                    ComputeQR(u[i], q, i, matrix.RowCount, i, matrix.RowCount, Control.MaxDegreeOfParallelism);
                }
            }
            else
            {
                q = matrix.Clone();

                for (var i = 0; i < minmn; i++)
                {
                    u[i] = GenerateColumn(q, i, i);
                    ComputeQR(u[i], q, i, matrix.RowCount, i + 1, matrix.ColumnCount, Control.MaxDegreeOfParallelism);
                }

                r = q.SubMatrix(0, matrix.ColumnCount, 0, matrix.ColumnCount);
                q.Clear();

                for (var i = 0; i < matrix.ColumnCount; i++)
                {
                    q.At(i, i, 1.0f);
                }

                for (var i = minmn - 1; i >= 0; i--)
                {
                    ComputeQR(u[i], q, i, matrix.RowCount, i, matrix.ColumnCount, Control.MaxDegreeOfParallelism);
                }
            }

            return new UserQR(q, r, method);
        }
Пример #2
0
        /// <summary>
        /// Initializes a new instance of the <see cref="UserQR"/> class. This object will compute the
        /// QR factorization when the constructor is called and cache it's factorization.
        /// </summary>
        /// <param name="matrix">The matrix to factor.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="matrix"/> is <c>null</c>.</exception>
        public UserQR(Matrix matrix)
        {
            if (matrix == null)
            {
                throw new ArgumentNullException("matrix");
            }

            if (matrix.RowCount < matrix.ColumnCount)
            {
                throw new ArgumentException(Resources.ArgumentMatrixDimensions);
            }

            MatrixR = matrix.Clone();
            MatrixQ = matrix.CreateMatrix(matrix.RowCount, matrix.RowCount);

            for (var i = 0; i < matrix.RowCount; i++)
            {
                MatrixQ.At(i, i, 1.0);
            }

            var minmn = Math.Min(matrix.RowCount, matrix.ColumnCount);
            var u = new double[minmn][];
            for (var i = 0; i < minmn; i++)
            {
                u[i] = GenerateColumn(MatrixR, i, matrix.RowCount - 1, i);
                ComputeQR(u[i], MatrixR, i, matrix.RowCount - 1, i + 1, matrix.ColumnCount - 1);
            }

            for (var i = minmn - 1; i >= 0; i--)
            {
                ComputeQR(u[i], MatrixQ, i, matrix.RowCount - 1, i, matrix.RowCount - 1);
            }
        }
Пример #3
0
        /// <summary>
        /// Initializes a new instance of the <see cref="UserQR"/> class. This object will compute the
        /// QR factorization when the constructor is called and cache it's factorization.
        /// </summary>
        /// <param name="matrix">The matrix to factor.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="matrix"/> is <c>null</c>.</exception>
        public UserQR(Matrix<Complex32> matrix)
        {
            if (matrix == null)
            {
                throw new ArgumentNullException("matrix");
            }

            if (matrix.RowCount < matrix.ColumnCount)
            {
                throw new ArgumentException(Resources.ArgumentMatrixDimensions);
            }

            MatrixR = matrix.Clone();
            MatrixQ = matrix.CreateMatrix(matrix.RowCount, matrix.RowCount);

            for (var i = 0; i < matrix.RowCount; i++)
            {
                MatrixQ.At(i, i, 1.0f);
            }

            var minmn = Math.Min(matrix.RowCount, matrix.ColumnCount);
            var u = new Complex32[minmn][];
            for (var i = 0; i < minmn; i++)
            {
                u[i] = GenerateColumn(MatrixR, i, i);
                ComputeQR(u[i], MatrixR, i, matrix.RowCount, i + 1, matrix.ColumnCount, Control.NumberOfParallelWorkerThreads);
            }

            for (var i = minmn - 1; i >= 0; i--)
            {
                ComputeQR(u[i], MatrixQ, i, matrix.RowCount, i, matrix.RowCount, Control.NumberOfParallelWorkerThreads);
            }
        }
Пример #4
0
        /// <summary>Construct a QR decomposition.</summary>	
        public QrDecomposition(Matrix value)
        {
            if (value == null)
            {
                throw new ArgumentNullException("value");
            }

            this.QR = (Matrix) value.Clone();
            double[][] qr = this.QR.baseArray;
            int m = value.Rows;
            int n = value.Columns;
            this.Rdiag = new double[n];

            for (int k = 0; k < n; k++)
            {
                // Compute 2-norm of k-th column without under/overflow.
                double nrm = 0;
                for (int i = k; i < m; i++)
                {
                    nrm = AForge.Mathematics.Tools.Hypotenuse(nrm,qr[i][k]);
                }

                if (nrm != 0.0)
                {
                    // Form k-th Householder vector.
                    if (qr[k][k] < 0)
                    {
                        nrm = -nrm;
                    }

                    for (int i = k; i < m; i++)
                    {
                        qr[i][k] /= nrm;
                    }

                    qr[k][k] += 1.0;

                    // Apply transformation to remaining columns.
                    for (int j = k+1; j < n; j++)
                    {
                        double s = 0.0;

                        for (int i = k; i < m; i++)
                        {
                            s += qr[i][k]*qr[i][j];
                        }

                        s = -s/qr[k][k];

                        for (int i = k; i < m; i++)
                        {
                            qr[i][j] += s*qr[i][k];
                        }
                    }
                }

                this.Rdiag[k] = -nrm;
            }
        }
        /// <summary>
        /// QR Decomposition, computed by Householder reflections.
        /// </summary>
        /// <remarks>Provides access to R, the Householder vectors and computes Q.</remarks>
        /// <param name="A">Rectangular matrix</param>
        public QRDecomposition(
            Matrix A
            )
        {
            // TODO: it is usually considered as a poor practice to execute algorithms within a constructor.

            // Initialize.
            QR = A.Clone();
            Rdiag = new double[n];

            // Main loop.
            for(int k = 0; k < n; k++)
            {
                // Compute 2-norm of k-th column without under/overflow.
                double norm = 0;
                for(int i = k; i < m; i++)
                {
                    norm = Fn.Hypot(norm, QR[i][k]);
                }

                if(norm != 0.0)
                {
                    // Form k-th Householder vector.
                    if(QR[k][k] < 0)
                    {
                        norm = -norm;
                    }

                    for(int i = k; i < m; i++)
                    {
                        QR[i][k] /= norm;
                    }

                    QR[k][k] += 1.0;

                    // Apply transformation to remaining columns.
                    for(int j = k + 1; j < n; j++)
                    {
                        double s = 0.0;
                        for(int i = k; i < m; i++)
                        {
                            s += QR[i][k] * QR[i][j];
                        }

                        s = (-s) / QR[k][k];
                        for(int i = k; i < m; i++)
                        {
                            QR[i][j] += s * QR[i][k];
                        }
                    }
                }

                Rdiag[k] = -norm;
            }

            InitOnDemandComputations();
        }
Пример #6
0
 /// <summary>
 /// Create a Kalman Filter using the specific values
 /// </summary>
 /// <param name="initialState">The m x 1 matrix</param>
 /// <param name="transitionMatrix">The m x m matrix (A) </param>
 /// <param name="controlMatrix">The m x n matrix (B)</param>
 /// <param name="measurementMatrix">The n x m matrix (H)</param>
 /// <param name="processNoiseCovarianceMatrix">The n x n matrix (Q)</param>
 /// <param name="measurementNoiseCovarianceMatrix">The m x m matrix (R)</param>
 public Kalman(
   Matrix<float> initialState,
   Matrix<float> transitionMatrix,
   Matrix<float> controlMatrix,
   Matrix<float> measurementMatrix,
   Matrix<float> processNoiseCovarianceMatrix,
   Matrix<float> measurementNoiseCovarianceMatrix
   )
     : this(initialState.Rows,
  measurementMatrix.Rows,
  controlMatrix == null ? 0 : controlMatrix.Rows)
 {
     PredictedState = initialState.Clone();
      CorrectedState = initialState.Clone();
      TransitionMatrix = transitionMatrix;
      if (controlMatrix != null) ControlMatrix = controlMatrix;
      MeasurementMatrix = measurementMatrix;
      ProcessNoiseCovariance = processNoiseCovarianceMatrix;
      MeasurementNoiseCovariance = measurementNoiseCovarianceMatrix;
 }
        /// <summary>
        /// Initializes a new instance of the <see cref="DenseGramSchmidt"/> class. This object creates an unitary matrix 
        /// using the modified Gram-Schmidt method.
        /// </summary>
        /// <param name="matrix">The matrix to factor.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="matrix"/> is <c>null</c>.</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> row count is less then column count</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> is rank deficient</exception>
        public static DenseGramSchmidt Create(Matrix<Complex32> matrix)
        {
            if (matrix.RowCount < matrix.ColumnCount)
            {
                throw Matrix.DimensionsDontMatch<ArgumentException>(matrix);
            }

            var q = (DenseMatrix)matrix.Clone();
            var r = new DenseMatrix(matrix.ColumnCount, matrix.ColumnCount);
            Factorize(q.Values, q.RowCount, q.ColumnCount, r.Values);

            return new DenseGramSchmidt(q, r);
        }
Пример #8
0
        /// <summary>
        /// Initializes a new instance of the <see cref="UserCholesky"/> class. This object will compute the
        /// Cholesky factorization when the constructor is called and cache it's factorization.
        /// </summary>
        /// <param name="matrix">The matrix to factor.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="matrix"/> is <c>null</c>.</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> is not a square matrix.</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> is not positive definite.</exception>
        public UserCholesky(Matrix<Complex32> matrix)
        {
            if (matrix == null)
            {
                throw new ArgumentNullException("matrix");
            }

            if (matrix.RowCount != matrix.ColumnCount)
            {
                throw new ArgumentException(Resources.ArgumentMatrixSquare);
            }

            // Create a new matrix for the Cholesky factor, then perform factorization (while overwriting).
            CholeskyFactor = matrix.Clone();
            var tmpColumn = new Complex32[CholeskyFactor.RowCount];

            // Main loop - along the diagonal
            for (var ij = 0; ij < CholeskyFactor.RowCount; ij++)
            {
                // "Pivot" element
                var tmpVal = CholeskyFactor.At(ij, ij);

                if (tmpVal.Real > 0.0)
                {
                    tmpVal = tmpVal.SquareRoot();
                    CholeskyFactor.At(ij, ij, tmpVal);
                    tmpColumn[ij] = tmpVal;

                    // Calculate multipliers and copy to local column
                    // Current column, below the diagonal
                    for (var i = ij + 1; i < CholeskyFactor.RowCount; i++)
                    {
                        CholeskyFactor.At(i, ij, CholeskyFactor.At(i, ij) / tmpVal);
                        tmpColumn[i] = CholeskyFactor.At(i, ij);
                    }

                    // Remaining columns, below the diagonal
                    DoCholeskyStep(CholeskyFactor, CholeskyFactor.RowCount, ij + 1, CholeskyFactor.RowCount, tmpColumn, Control.NumberOfParallelWorkerThreads);
                }
                else
                {
                    throw new ArgumentException(Resources.ArgumentMatrixPositiveDefinite);
                }

                for (var i = ij + 1; i < CholeskyFactor.RowCount; i++)
                {
                    CholeskyFactor.At(ij, i, Complex32.Zero);
                }
            }
        }
Пример #9
0
        private static void Main(string[] args)
        {
            var A = new Matrix(new double[,] { { 1, 2, 3 }, { 0, 1, 4 }, { 5, 6, 0 } });
            var clone = A.Clone();

            A[0, 0] = -99;

            Console.WriteLine(A);
            Console.WriteLine(clone);

            A[2, 2] = -88;
            Console.WriteLine(A);
            Console.WriteLine(clone);
            Console.Read();
        }
Пример #10
0
        /// <summary>
        /// Initializes a new instance of the <see cref="UserCholesky"/> class. This object will compute the
        /// Cholesky factorization when the constructor is called and cache it's factorization.
        /// </summary>
        /// <param name="matrix">The matrix to factor.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="matrix"/> is <c>null</c>.</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> is not a square matrix.</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> is not positive definite.</exception>
        public static UserCholesky Create(Matrix<float> matrix)
        {
            if (matrix.RowCount != matrix.ColumnCount)
            {
                throw new ArgumentException(Resources.ArgumentMatrixSquare);
            }

            // Create a new matrix for the Cholesky factor, then perform factorization (while overwriting).
            var factor = matrix.Clone();
            var tmpColumn = new float[factor.RowCount];

            // Main loop - along the diagonal
            for (var ij = 0; ij < factor.RowCount; ij++)
            {
                // "Pivot" element
                var tmpVal = factor.At(ij, ij);

                if (tmpVal > 0.0)
                {
                    tmpVal = (float) Math.Sqrt(tmpVal);
                    factor.At(ij, ij, tmpVal);
                    tmpColumn[ij] = tmpVal;

                    // Calculate multipliers and copy to local column
                    // Current column, below the diagonal
                    for (var i = ij + 1; i < factor.RowCount; i++)
                    {
                        factor.At(i, ij, factor.At(i, ij)/tmpVal);
                        tmpColumn[i] = factor.At(i, ij);
                    }

                    // Remaining columns, below the diagonal
                    DoCholeskyStep(factor, factor.RowCount, ij + 1, factor.RowCount, tmpColumn, Control.MaxDegreeOfParallelism);
                }
                else
                {
                    throw new ArgumentException(Resources.ArgumentMatrixPositiveDefinite);
                }

                for (var i = ij + 1; i < factor.RowCount; i++)
                {
                    factor.At(ij, i, 0.0f);
                }
            }

            return new UserCholesky(factor);
        }
Пример #11
0
        /// <summary>
        /// Initializes a new instance of the <see cref="UserGramSchmidt"/> class. This object creates an unitary matrix 
        /// using the modified Gram-Schmidt method.
        /// </summary>
        /// <param name="matrix">The matrix to factor.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="matrix"/> is <c>null</c>.</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> row count is less then column count</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> is rank deficient</exception>
        public UserGramSchmidt(Matrix<Complex32> matrix)
        {
            if (matrix == null)
            {
                throw new ArgumentNullException("matrix");
            }

            if (matrix.RowCount < matrix.ColumnCount)
            {
                throw new ArgumentException(Resources.ArgumentMatrixDimensions);
            }

            MatrixQ = matrix.Clone();
            MatrixR = matrix.CreateMatrix(matrix.ColumnCount, matrix.ColumnCount);

            for (var k = 0; k < MatrixQ.ColumnCount; k++)
            {
                var norm = MatrixQ.Column(k).Norm(2).Real;
                if (norm == 0.0f)
                {
                    throw new ArgumentException(Resources.ArgumentMatrixNotRankDeficient);
                }

                MatrixR.At(k, k, norm);
                for (var i = 0; i < MatrixQ.RowCount; i++)
                {
                    MatrixQ.At(i, k, MatrixQ.At(i, k) / norm);
                }

                for (var j = k + 1; j < MatrixQ.ColumnCount; j++)
                {
                    var dot = Complex32.Zero;
                    for (int i = 0; i < MatrixQ.RowCount; i++)
                    {
                        dot += MatrixQ.Column(k)[i].Conjugate() * MatrixQ.Column(j)[i];
                    }
                    
                    MatrixR.At(k, j, dot);
                    for (var i = 0; i < MatrixQ.RowCount; i++)
                    {
                        var value = MatrixQ.At(i, j) - (MatrixQ.At(i, k) * dot);
                        MatrixQ.At(i, j, value);
                    }
                }
            }
        }
        public void CloneTest()
        {
            var original = new Matrix<double>(3, 2);
            original[1, 1] = 1;
            original[1, 2] = 2;
            original[2, 1] = 3;
            original[2, 2] = 4;
            original[3, 1] = 5;
            original[3, 2] = 6;

            var clone = original.Clone();

            Assert.AreEqual(1, clone[1, 1]);
            Assert.AreEqual(2, clone[1, 2]);
            Assert.AreEqual(3, clone[2, 1]);
            Assert.AreEqual(4, clone[2, 2]);
            Assert.AreEqual(5, clone[3, 1]);
            Assert.AreEqual(6, clone[3, 2]);
        }
Пример #13
0
        /// <summary>
        /// Initializes a new instance of the <see cref="UserCholesky"/> class. This object will compute the
        /// Cholesky factorization when the constructor is called and cache it's factorization.
        /// </summary>
        /// <param name="matrix">The matrix to factor.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="matrix"/> is <c>null</c>.</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> is not a square matrix.</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> is not positive definite.</exception>
        public UserCholesky(Matrix matrix)
        {
            if (matrix == null)
            {
                throw new ArgumentNullException("matrix");
            }

            if (matrix.RowCount != matrix.ColumnCount)
            {
                throw new ArgumentException(Resources.ArgumentMatrixSquare);
            }

            // Create a new matrix for the Cholesky factor, then perform factorization (while overwriting).
            CholeskyFactor = matrix.Clone();
            for (var j = 0; j < CholeskyFactor.RowCount; j++)
            {
                var d = 0.0;
                for (var k = 0; k < j; k++)
                {
                    var s = 0.0;
                    for (var i = 0; i < k; i++)
                    {
                        s += CholeskyFactor.At(k, i) * CholeskyFactor.At(j, i);
                    }

                    s = (matrix.At(j, k) - s) / CholeskyFactor.At(k, k);
                    CholeskyFactor.At(j, k, s);
                    d += s * s;
                }

                d = matrix.At(j, j) - d;
                if (d <= 0.0)
                {
                    throw new ArgumentException(Resources.ArgumentMatrixPositiveDefinite);
                }

                CholeskyFactor.At(j, j, Math.Sqrt(d));
                for (var k = j + 1; k < CholeskyFactor.RowCount; k++)
                {
                    CholeskyFactor.At(j, k, 0.0);
                }
            }
        }
Пример #14
0
        /// <summary>
        /// Initializes a new instance of the <see cref="UserGramSchmidt"/> class. This object creates an unitary matrix 
        /// using the modified Gram-Schmidt method.
        /// </summary>
        /// <param name="matrix">The matrix to factor.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="matrix"/> is <c>null</c>.</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> row count is less then column count</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> is rank deficient</exception>
        public static UserGramSchmidt Create(Matrix<Complex32> matrix)
        {
            if (matrix.RowCount < matrix.ColumnCount)
            {
                throw Matrix.DimensionsDontMatch<ArgumentException>(matrix);
            }

            var q = matrix.Clone();
            var r = Matrix<Complex32>.Build.SameAs(matrix, matrix.ColumnCount, matrix.ColumnCount);

            for (var k = 0; k < q.ColumnCount; k++)
            {
                var norm = (float) q.Column(k).L2Norm();
                if (norm == 0f)
                {
                    throw new ArgumentException(Resources.ArgumentMatrixNotRankDeficient);
                }

                r.At(k, k, norm);
                for (var i = 0; i < q.RowCount; i++)
                {
                    q.At(i, k, (q.At(i, k) / norm));
                }

                for (var j = k + 1; j < q.ColumnCount; j++)
                {
                    var dot = Complex32.Zero;
                    for (int i = 0; i < q.RowCount; i++)
                    {
                        dot += q.Column(k)[i].Conjugate() * q.Column(j)[i];
                    }
                    
                    r.At(k, j, dot);
                    for (var i = 0; i < q.RowCount; i++)
                    {
                        var value = q.At(i, j) - (q.At(i, k) * dot);
                        q.At(i, j, value);
                    }
                }
            }

            return new UserGramSchmidt(q, r);
        }
Пример #15
0
        /// <summary>
        /// Initializes a new instance of the <see cref="UserGramSchmidt"/> class. This object creates an orthogonal matrix 
        /// using the modified Gram-Schmidt method.
        /// </summary>
        /// <param name="matrix">The matrix to factor.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="matrix"/> is <c>null</c>.</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> row count is less then column count</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> is rank deficient</exception>
        public UserGramSchmidt(Matrix<double> matrix)
        {
            if (matrix == null)
            {
                throw new ArgumentNullException("matrix");
            }

            if (matrix.RowCount < matrix.ColumnCount)
            {
                throw new ArgumentException(Resources.ArgumentMatrixDimensions);
            }

            MatrixQ = matrix.Clone();
            MatrixR = matrix.CreateMatrix(matrix.ColumnCount, matrix.ColumnCount);

            for (var k = 0; k < MatrixQ.ColumnCount; k++)
            {
                var norm = MatrixQ.Column(k).Norm(2);
                if (norm == 0.0)
                {
                    throw new ArgumentException(Resources.ArgumentMatrixNotRankDeficient);
                }

                MatrixR.At(k, k, norm);
                for (var i = 0; i < MatrixQ.RowCount; i++)
                {
                    MatrixQ.At(i, k, MatrixQ.At(i, k) / norm);
                }

                for (var j = k + 1; j < MatrixQ.ColumnCount; j++)
                {
                    var dot = MatrixQ.Column(k).DotProduct(MatrixQ.Column(j));
                    MatrixR.At(k, j, dot);
                    for (var i = 0; i < MatrixQ.RowCount; i++)
                    {
                        var value = MatrixQ.At(i, j) - (MatrixQ.At(i, k) * dot);
                        MatrixQ.At(i, j, value);
                    }
                }
            }
        }
Пример #16
0
        /// <summary>
        /// Initializes a new instance of the <see cref="UserGramSchmidt"/> class. This object creates an orthogonal matrix 
        /// using the modified Gram-Schmidt method.
        /// </summary>
        /// <param name="matrix">The matrix to factor.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="matrix"/> is <c>null</c>.</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> row count is less then column count</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> is rank deficient</exception>
        public static UserGramSchmidt Create(Matrix<float> matrix)
        {
            if (matrix.RowCount < matrix.ColumnCount)
            {
                throw Matrix.DimensionsDontMatch<ArgumentException>(matrix);
            }

            var q = matrix.Clone();
            var r = matrix.CreateMatrix(matrix.ColumnCount, matrix.ColumnCount);

            for (var k = 0; k < q.ColumnCount; k++)
            {
                var norm = q.Column(k).L2Norm();
                if (norm == 0.0)
                {
                    throw new ArgumentException(Resources.ArgumentMatrixNotRankDeficient);
                }

                r.At(k, k, norm);
                for (var i = 0; i < q.RowCount; i++)
                {
                    q.At(i, k, q.At(i, k) / norm);
                }

                for (var j = k + 1; j < q.ColumnCount; j++)
                {
                    var dot = q.Column(k).DotProduct(q.Column(j));
                    r.At(k, j, dot);
                    for (var i = 0; i < q.RowCount; i++)
                    {
                        var value = q.At(i, j) - (q.At(i, k) * dot);
                        q.At(i, j, value);
                    }
                }
            }

            return new UserGramSchmidt(q, r);
        }
Пример #17
0
        /// <summary>
        /// Initializes a new instance of the <see cref="UserLU"/> class. This object will compute the
        /// LU factorization when the constructor is called and cache it's factorization.
        /// </summary>
        /// <param name="matrix">The matrix to factor.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="matrix"/> is <c>null</c>.</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> is not a square matrix.</exception>
        public UserLU(Matrix<double> matrix)
        {
            if (matrix == null)
            {
                throw new ArgumentNullException("matrix");
            }

            if (matrix.RowCount != matrix.ColumnCount)
            {
                throw new ArgumentException(Resources.ArgumentMatrixSquare);
            }

            // Create an array for the pivot indices.
            var order = matrix.RowCount;
            Factors = matrix.Clone();
            Pivots = new int[order];
            
            // Initialize the pivot matrix to the identity permutation.
            for (var i = 0; i < order; i++)
            {
                Pivots[i] = i;
            }

            var vectorLUcolj = new double[order];
            for (var j = 0; j < order; j++)
            {
                // Make a copy of the j-th column to localize references.
                for (var i = 0; i < order; i++)
                {
                    vectorLUcolj[i] = Factors.At(i, j);
                }

                // Apply previous transformations.
                for (var i = 0; i < order; i++)
                {
                    var kmax = Math.Min(i, j);
                    var s = 0.0;
                    for (var k = 0; k < kmax; k++)
                    {
                        s += Factors.At(i, k) * vectorLUcolj[k];
                    }

                    vectorLUcolj[i] -= s;
                    Factors.At(i, j, vectorLUcolj[i]);
                }

                // Find pivot and exchange if necessary.
                var p = j;
                for (var i = j + 1; i < order; i++)
                {
                    if (Math.Abs(vectorLUcolj[i]) > Math.Abs(vectorLUcolj[p]))
                    {
                        p = i;
                    }
                }

                if (p != j)
                {
                    for (var k = 0; k < order; k++)
                    {
                        var temp = Factors.At(p, k);
                        Factors.At(p, k, Factors.At(j, k));
                        Factors.At(j, k, temp);
                    }

                    Pivots[j] = p;
                }

                // Compute multipliers.
                if (j < order & Factors.At(j, j) != 0.0)
                {
                    for (var i = j + 1; i < order; i++)
                    {
                        Factors.At(i, j, (Factors.At(i, j) / Factors.At(j, j)));
                    }
                }
            }
        }
Пример #18
0
      public void TestEigenVV()
      {
         int size = 3;
         Matrix<float> tmp = new Matrix<float>(size, size);
         tmp.SetRandNormal(new MCvScalar(0), new MCvScalar(1));
         Matrix<float> symMat = new Matrix<float>(tmp.Size);
         CvInvoke.MulTransposed(tmp, symMat, false, null, 1.0, CvEnum.DepthType.Cv32S);
         Matrix<float> clone = symMat.Clone();

         Matrix<float> evects = new Matrix<float>(symMat.Size);
         Matrix<float> evals = new Matrix<float>(symMat.Rows, 1);
         CvInvoke.Eigen(symMat, evals, evects);
      }
Пример #19
0
 /// <summary>
 /// Create a spill tree from the specific feature descriptors
 /// </summary>
 /// <param name="descriptors">The array of feature descriptors</param>
 /// <param name="naive">A good value is 50</param>
 /// <param name="rho">A good value is .7</param>
 /// <param name="tau">A good value is .1</param>
 public FeatureTree(Matrix<float> descriptors, int naive, double rho, double tau)
 {
     _descriptorMatrix = descriptors.Clone();
      _ptr = CvInvoke.cvCreateSpillTree(_descriptorMatrix.Ptr, naive, rho, tau);
 }
Пример #20
0
 /// <summary>
 /// Create a k-d tree from the specific feature descriptors
 /// </summary>
 /// <param name="descriptorMatrix">The array of feature descriptors</param>
 public FeatureTree(Matrix<float> descriptorMatrix)
 {
     _descriptorMatrix = descriptorMatrix.Clone();
      _ptr = CvInvoke.cvCreateKDTree(_descriptorMatrix.Ptr);
 }
Пример #21
0
        public SingularValueDecomposition(Matrix value, bool computeLeftSingularVectors, bool computeRightSingularVectors)
        {
            if (value == null)
            {
                throw new ArgumentNullException("value");
            }

            Matrix copy = (Matrix)value.Clone();
            double[][] a = copy.baseArray;
            m = value.Rows;
            n = value.Columns;
            int nu = System.Math.Min(m, n);
            s = new double[System.Math.Min(m + 1, n)];
            U = new Matrix(m, nu);
            V = new Matrix(n, n);
            double[][] u = U.baseArray;
            double[][] v = V.baseArray;
            double[] e = new double[n];
            double[] work = new double[m];
            bool wantu = computeLeftSingularVectors;
            bool wantv = computeRightSingularVectors;

            // Reduce A to bidiagonal form, storing the diagonal elements in s and the super-diagonal elements in e.
            int nct = System.Math.Min(m - 1, n);
            int nrt = System.Math.Max(0, System.Math.Min(n - 2, m));
            for (int k = 0; k < System.Math.Max(nct, nrt); k++)
            {
                if (k < nct)
                {
                    // Compute the transformation for the k-th column and place the k-th diagonal in s[k].
                    // Compute 2-norm of k-th column without under/overflow.
                    s[k] = 0;
                    for (int i = k; i < m; i++)
                    {
                        s[k] = Tools.Hypotenuse(s[k], a[i][k]);
                    }

                    if (s[k] != 0.0)
                    {
                        if (a[k][k] < 0.0)
                        {
                            s[k] = -s[k];
                        }

                        for (int i = k; i < m; i++)
                        {
                            a[i][k] /= s[k];
                        }

                        a[k][k] += 1.0;
                    }

                    s[k] = -s[k];
                }

                for (int j = k + 1; j < n; j++)
                {
                    if ((k < nct) & (s[k] != 0.0))
                    {
                        // Apply the transformation.
                        double t = 0;
                        for (int i = k; i < m; i++)
                            t += a[i][k] * a[i][j];
                        t = -t / a[k][k];
                        for (int i = k; i < m; i++)
                            a[i][j] += t * a[i][k];
                    }

                    // Place the k-th row of A into e for the subsequent calculation of the row transformation.
                    e[j] = a[k][j];
                }

                if (wantu & (k < nct))
                {
                    // Place the transformation in U for subsequent back
                    // multiplication.
                    for (int i = k; i < m; i++)
                        u[i][k] = a[i][k];
                }

                if (k < nrt)
                {
                    // Compute the k-th row transformation and place the k-th super-diagonal in e[k].
                    // Compute 2-norm without under/overflow.
                    e[k] = 0;
                    for (int i = k + 1; i < n; i++)
                    {
                        e[k] = Tools.Hypotenuse(e[k], e[i]);
                    }

                    if (e[k] != 0.0)
                    {
                        if (e[k + 1] < 0.0)
                            e[k] = -e[k];

                        for (int i = k + 1; i < n; i++)
                            e[i] /= e[k];

                        e[k + 1] += 1.0;
                    }

                    e[k] = -e[k];
                    if ((k + 1 < m) & (e[k] != 0.0))
                    {
                        // Apply the transformation.
                        for (int i = k + 1; i < m; i++)
                            work[i] = 0.0;

                        for (int j = k + 1; j < n; j++)
                            for (int i = k + 1; i < m; i++)
                                work[i] += e[j] * a[i][j];

                        for (int j = k + 1; j < n; j++)
                        {
                            double t = -e[j] / e[k + 1];
                            for (int i = k + 1; i < m; i++)
                                a[i][j] += t * work[i];
                        }
                    }

                    if (wantv)
                    {
                        // Place the transformation in V for subsequent back multiplication.
                        for (int i = k + 1; i < n; i++)
                            v[i][k] = e[i];
                    }
                }
            }

            // Set up the final bidiagonal matrix or order p.
            int p = System.Math.Min(n, m + 1);
            if (nct < n) s[nct] = a[nct][nct];
            if (m < p) s[p - 1] = 0.0;
            if (nrt + 1 < p) e[nrt] = a[nrt][p - 1];
            e[p - 1] = 0.0;

            // If required, generate U.
            if (wantu)
            {
                for (int j = nct; j < nu; j++)
                {
                    for (int i = 0; i < m; i++)
                        u[i][j] = 0.0;
                    u[j][j] = 1.0;
                }

                for (int k = nct - 1; k >= 0; k--)
                {
                    if (s[k] != 0.0)
                    {
                        for (int j = k + 1; j < nu; j++)
                        {
                            double t = 0;
                            for (int i = k; i < m; i++)
                                t += u[i][k] * u[i][j];

                            t = -t / u[k][k];
                            for (int i = k; i < m; i++)
                                u[i][j] += t * u[i][k];
                        }

                        for (int i = k; i < m; i++)
                            u[i][k] = -u[i][k];

                        u[k][k] = 1.0 + u[k][k];
                        for (int i = 0; i < k - 1; i++)
                            u[i][k] = 0.0;
                    }
                    else
                    {
                        for (int i = 0; i < m; i++)
                            u[i][k] = 0.0;
                        u[k][k] = 1.0;
                    }
                }
            }

            // If required, generate V.
            if (wantv)
            {
                for (int k = n - 1; k >= 0; k--)
                {
                    if ((k < nrt) & (e[k] != 0.0))
                    {
                        for (int j = k + 1; j < nu; j++)
                        {
                            double t = 0;
                            for (int i = k + 1; i < n; i++)
                                t += v[i][k] * v[i][j];

                            t = -t / v[k + 1][k];
                            for (int i = k + 1; i < n; i++)
                                v[i][j] += t * v[i][k];
                        }
                    }

                    for (int i = 0; i < n; i++)
                        v[i][k] = 0.0;
                    v[k][k] = 1.0;
                }
            }

            // Main iteration loop for the singular values.
            int pp = p - 1;
            int iter = 0;
            double eps = System.Math.Pow(2.0, -52.0);
            while (p > 0)
            {
                int k, kase;

                // Here is where a test for too many iterations would go.
                // This section of the program inspects for
                // negligible elements in the s and e arrays.  On
                // completion the variables kase and k are set as follows.
                // kase = 1     if s(p) and e[k-1] are negligible and k<p
                // kase = 2     if s(k) is negligible and k<p
                // kase = 3     if e[k-1] is negligible, k<p, and s(k), ..., s(p) are not negligible (qr step).
                // kase = 4     if e(p-1) is negligible (convergence).
                for (k = p - 2; k >= -1; k--)
                {
                    if (k == -1)
                        break;

                    if (System.Math.Abs(e[k]) <= eps * (System.Math.Abs(s[k]) + System.Math.Abs(s[k + 1])))
                    {
                        e[k] = 0.0;
                        break;
                    }
                }

                if (k == p - 2)
                {
                    kase = 4;
                }
                else
                {
                    int ks;
                    for (ks = p - 1; ks >= k; ks--)
                    {
                        if (ks == k)
                            break;

                        double t = (ks != p ? System.Math.Abs(e[ks]) : 0.0) + (ks != k + 1 ? System.Math.Abs(e[ks - 1]) : 0.0);
                        if (System.Math.Abs(s[ks]) <= eps * t)
                        {
                            s[ks] = 0.0;
                            break;
                        }
                    }

                    if (ks == k)
                        kase = 3;
                    else if (ks == p - 1)
                        kase = 1;
                    else
                    {
                        kase = 2;
                        k = ks;
                    }
                }

                k++;

                // Perform the task indicated by kase.
                switch (kase)
                {
                    // Deflate negligible s(p).
                    case 1:
                        {
                            double f = e[p - 2];
                            e[p - 2] = 0.0;
                            for (int j = p - 2; j >= k; j--)
                            {
                                double t = Tools.Hypotenuse(s[j], f);
                                double cs = s[j] / t;
                                double sn = f / t;
                                s[j] = t;
                                if (j != k)
                                {
                                    f = -sn * e[j - 1];
                                    e[j - 1] = cs * e[j - 1];
                                }

                                if (wantv)
                                {
                                    for (int i = 0; i < n; i++)
                                    {
                                        t = cs * v[i][j] + sn * v[i][p - 1];
                                        v[i][p - 1] = -sn * v[i][j] + cs * v[i][p - 1];
                                        v[i][j] = t;
                                    }
                                }
                            }
                        }
                        break;

                    // Split at negligible s(k).
                    case 2:
                        {
                            double f = e[k - 1];
                            e[k - 1] = 0.0;
                            for (int j = k; j < p; j++)
                            {
                                double t = Tools.Hypotenuse(s[j], f);
                                double cs = s[j] / t;
                                double sn = f / t;
                                s[j] = t;
                                f = -sn * e[j];
                                e[j] = cs * e[j];
                                if (wantu)
                                {
                                    for (int i = 0; i < m; i++)
                                    {
                                        t = cs * u[i][j] + sn * u[i][k - 1];
                                        u[i][k - 1] = -sn * u[i][j] + cs * u[i][k - 1];
                                        u[i][j] = t;
                                    }
                                }
                            }
                        }
                        break;

                    // Perform one qr step.
                    case 3:
                        {
                            // Calculate the shift.
                            double scale = System.Math.Max(System.Math.Max(System.Math.Max(System.Math.Max(System.Math.Abs(s[p - 1]), System.Math.Abs(s[p - 2])), System.Math.Abs(e[p - 2])), System.Math.Abs(s[k])), System.Math.Abs(e[k]));
                            double sp = s[p - 1] / scale;
                            double spm1 = s[p - 2] / scale;
                            double epm1 = e[p - 2] / scale;
                            double sk = s[k] / scale;
                            double ek = e[k] / scale;
                            double b = ((spm1 + sp) * (spm1 - sp) + epm1 * epm1) / 2.0;
                            double c = (sp * epm1) * (sp * epm1);
                            double shift = 0.0;
                            if ((b != 0.0) | (c != 0.0))
                            {
                                shift = System.Math.Sqrt(b * b + c);
                                if (b < 0.0)
                                    shift = -shift;
                                shift = c / (b + shift);
                            }

                            double f = (sk + sp) * (sk - sp) + shift;
                            double g = sk * ek;

                            // Chase zeros.
                            for (int j = k; j < p - 1; j++)
                            {
                                double t = Tools.Hypotenuse(f, g);
                                double cs = f / t;
                                double sn = g / t;
                                if (j != k)
                                    e[j - 1] = t;
                                f = cs * s[j] + sn * e[j];
                                e[j] = cs * e[j] - sn * s[j];
                                g = sn * s[j + 1];
                                s[j + 1] = cs * s[j + 1];
                                if (wantv)
                                {
                                    for (int i = 0; i < n; i++)
                                    {
                                        t = cs * v[i][j] + sn * v[i][j + 1];
                                        v[i][j + 1] = -sn * v[i][j] + cs * v[i][j + 1];
                                        v[i][j] = t;
                                    }
                                }

                                t = Tools.Hypotenuse(f, g);
                                cs = f / t;
                                sn = g / t;
                                s[j] = t;
                                f = cs * e[j] + sn * s[j + 1];
                                s[j + 1] = -sn * e[j] + cs * s[j + 1];
                                g = sn * e[j + 1];
                                e[j + 1] = cs * e[j + 1];
                                if (wantu && (j < m - 1))
                                {
                                    for (int i = 0; i < m; i++)
                                    {
                                        t = cs * u[i][j] + sn * u[i][j + 1];
                                        u[i][j + 1] = -sn * u[i][j] + cs * u[i][j + 1];
                                        u[i][j] = t;
                                    }
                                }
                            }

                            e[p - 2] = f;
                            iter = iter + 1;
                        }
                        break;

                    // Convergence.
                    case 4:
                        {
                            // Make the singular values positive.
                            if (s[k] <= 0.0)
                            {
                                s[k] = (s[k] < 0.0 ? -s[k] : 0.0);
                                if (wantv)
                                    for (int i = 0; i <= pp; i++)
                                        v[i][k] = -v[i][k];
                            }

                            // Order the singular values.
                            while (k < pp)
                            {
                                if (s[k] >= s[k + 1])
                                    break;

                                double t = s[k];
                                s[k] = s[k + 1];
                                s[k + 1] = t;
                                if (wantv && (k < n - 1))
                                    for (int i = 0; i < n; i++)
                                    {
                                        t = v[i][k + 1];
                                        v[i][k + 1] = v[i][k];
                                        v[i][k] = t;
                                    }

                                if (wantu && (k < m - 1))
                                    for (int i = 0; i < m; i++)
                                    {
                                        t = u[i][k + 1];
                                        u[i][k + 1] = u[i][k];
                                        u[i][k] = t;
                                    }

                                k++;
                            }

                            iter = 0;
                            p--;
                        }
                        break;
                }
            }
        }
Пример #22
0
        /// <summary>
        /// Solves a system of linear equations, <b>AX = B</b>, with A QR factorized.
        /// </summary>
        /// <param name="input">The right hand side <see cref="Matrix{T}"/>, <b>B</b>.</param>
        /// <param name="result">The left hand side <see cref="Matrix{T}"/>, <b>X</b>.</param>
        public override void Solve(Matrix<float> input, Matrix<float> result)
        {
            // The solution X should have the same number of columns as B
            if (input.ColumnCount != result.ColumnCount)
            {
                throw new ArgumentException(Resources.ArgumentMatrixSameColumnDimension);
            }

            // The dimension compatibility conditions for X = A\B require the two matrices A and B to have the same number of rows
            if (Q.RowCount != input.RowCount)
            {
                throw new ArgumentException(Resources.ArgumentMatrixSameRowDimension);
            }

            // The solution X row dimension is equal to the column dimension of A
            if (Q.ColumnCount != result.RowCount)
            {
                throw new ArgumentException(Resources.ArgumentMatrixSameColumnDimension);
            }

            var inputCopy = input.Clone();

            // Compute Y = transpose(Q)*B
            var column = new float[Q.RowCount];
            for (var j = 0; j < input.ColumnCount; j++)
            {
                for (var k = 0; k < Q.RowCount; k++)
                {
                    column[k] = inputCopy.At(k, j);
                }

                for (var i = 0; i < Q.ColumnCount; i++)
                {
                    float s = 0;
                    for (var k = 0; k < Q.RowCount; k++)
                    {
                        s += Q.At(k, i) * column[k];
                    }

                    inputCopy.At(i, j, s);
                }
            }

            // Solve R*X = Y;
            for (var k = Q.ColumnCount - 1; k >= 0; k--)
            {
                for (var j = 0; j < input.ColumnCount; j++)
                {
                    inputCopy.At(k, j, inputCopy.At(k, j) / FullR.At(k, k));
                }

                for (var i = 0; i < k; i++)
                {
                    for (var j = 0; j < input.ColumnCount; j++)
                    {
                        inputCopy.At(i, j, inputCopy.At(i, j) - (inputCopy.At(k, j) * FullR.At(i, k)));
                    }
                }
            }

            for (var i = 0; i < FullR.ColumnCount; i++)
            {
                for (var j = 0; j < input.ColumnCount; j++)
                {
                    result.At(i, j, inputCopy.At(i, j));
                }
            }
        }
Пример #23
0
        /// <summary>
        /// Initializes a new instance of the <see cref="UserQR"/> class. This object will compute the
        /// QR factorization when the constructor is called and cache it's factorization.
        /// </summary>
        /// <param name="matrix">The matrix to factor.</param>
        /// <param name="method">The QR factorization method to use.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="matrix"/> is <c>null</c>.</exception>
        public UserQR(Matrix<double> matrix, QRMethod method = QRMethod.Full)
        {
            if (matrix == null)
            {
                throw new ArgumentNullException("matrix");
            }

            if (matrix.RowCount < matrix.ColumnCount)
            {
                throw Matrix.DimensionsDontMatch<ArgumentException>(matrix);
            }

            QrMethod = method;
            var minmn = Math.Min(matrix.RowCount, matrix.ColumnCount);
            var u = new double[minmn][];

            if (method == QRMethod.Full)
            {
                MatrixR = matrix.Clone();
                MatrixQ = matrix.CreateMatrix(matrix.RowCount, matrix.RowCount);

                for (var i = 0; i < matrix.RowCount; i++)
                {
                    MatrixQ.At(i, i, 1.0);
                }

                for (var i = 0; i < minmn; i++)
                {
                    u[i] = GenerateColumn(MatrixR, i, i);
                    ComputeQR(u[i], MatrixR, i, matrix.RowCount, i + 1, matrix.ColumnCount,
                              Control.NumberOfParallelWorkerThreads);
                }

                for (var i = minmn - 1; i >= 0; i--)
                {
                    ComputeQR(u[i], MatrixQ, i, matrix.RowCount, i, matrix.RowCount,
                              Control.NumberOfParallelWorkerThreads);
                }
            }
            else
            {
                MatrixR = matrix.CreateMatrix(matrix.ColumnCount, matrix.ColumnCount);
                MatrixQ = matrix.Clone();

                for (var i = 0; i < minmn; i++)
                {
                    u[i] = GenerateColumn(MatrixQ, i, i);
                    ComputeQR(u[i], MatrixQ, i, matrix.RowCount, i + 1, matrix.ColumnCount,
                              Control.NumberOfParallelWorkerThreads);
                }

                MatrixR = MatrixQ.SubMatrix(0, matrix.ColumnCount, 0, matrix.ColumnCount);
                MatrixQ.Clear();
                
                for (var i = 0; i < matrix.ColumnCount; i++)
                {
                    MatrixQ.At(i, i, 1.0);
                }
                
                for (var i = minmn - 1; i >= 0; i--)
                {
                    ComputeQR(u[i], MatrixQ, i, matrix.RowCount, i, matrix.ColumnCount,
                              Control.NumberOfParallelWorkerThreads);
                }

            }
        }
Пример #24
0
        /// <summary>
        /// Initializes a new instance of the <see cref="UserLU"/> class. This object will compute the
        /// LU factorization when the constructor is called and cache it's factorization.
        /// </summary>
        /// <param name="matrix">The matrix to factor.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="matrix"/> is <c>null</c>.</exception>
        /// <exception cref="ArgumentException">If <paramref name="matrix"/> is not a square matrix.</exception>
        public static UserLU Create(Matrix<Complex> matrix)
        {
            if (matrix == null)
            {
                throw new ArgumentNullException("matrix");
            }

            if (matrix.RowCount != matrix.ColumnCount)
            {
                throw new ArgumentException(Resources.ArgumentMatrixSquare);
            }

            // Create an array for the pivot indices.
            var order = matrix.RowCount;
            var factors = matrix.Clone();
            var pivots = new int[order];

            // Initialize the pivot matrix to the identity permutation.
            for (var i = 0; i < order; i++)
            {
                pivots[i] = i;
            }

            var vectorLUcolj = new Complex[order];
            for (var j = 0; j < order; j++)
            {
                // Make a copy of the j-th column to localize references.
                for (var i = 0; i < order; i++)
                {
                    vectorLUcolj[i] = factors.At(i, j);
                }

                // Apply previous transformations.
                for (var i = 0; i < order; i++)
                {
                    var kmax = Math.Min(i, j);
                    var s = Complex.Zero;
                    for (var k = 0; k < kmax; k++)
                    {
                        s += factors.At(i, k)*vectorLUcolj[k];
                    }

                    vectorLUcolj[i] -= s;
                    factors.At(i, j, vectorLUcolj[i]);
                }

                // Find pivot and exchange if necessary.
                var p = j;
                for (var i = j + 1; i < order; i++)
                {
                    if (vectorLUcolj[i].Magnitude > vectorLUcolj[p].Magnitude)
                    {
                        p = i;
                    }
                }

                if (p != j)
                {
                    for (var k = 0; k < order; k++)
                    {
                        var temp = factors.At(p, k);
                        factors.At(p, k, factors.At(j, k));
                        factors.At(j, k, temp);
                    }

                    pivots[j] = p;
                }

                // Compute multipliers.
                if (j < order & factors.At(j, j) != 0.0)
                {
                    for (var i = j + 1; i < order; i++)
                    {
                        factors.At(i, j, (factors.At(i, j)/factors.At(j, j)));
                    }
                }
            }

            return new UserLU(factors, pivots);
        }
Пример #25
0
        /// <summary>
        /// Initializes a new instance of the <see cref="UserSvd"/> class. This object will compute the
        /// the singular value decomposition when the constructor is called and cache it's decomposition.
        /// </summary>
        /// <param name="matrix">The matrix to factor.</param>
        /// <param name="computeVectors">Compute the singular U and VT vectors or not.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="matrix"/> is <c>null</c>.</exception>
        /// <exception cref="NonConvergenceException"></exception>
        public static UserSvd Create(Matrix<Complex> matrix, bool computeVectors)
        {
            var nm = Math.Min(matrix.RowCount + 1, matrix.ColumnCount);
            var matrixCopy = matrix.Clone();

            var s = Vector<Complex>.Build.SameAs(matrixCopy, nm);
            var u = Matrix<Complex>.Build.SameAs(matrixCopy, matrixCopy.RowCount, matrixCopy.RowCount);
            var vt = Matrix<Complex>.Build.SameAs(matrixCopy, matrixCopy.ColumnCount, matrixCopy.ColumnCount);

            const int maxiter = 1000;
            var e = new Complex[matrixCopy.ColumnCount];
            var work = new Complex[matrixCopy.RowCount];

            int i, j;
            int l, lp1;
            Complex t;

            var ncu = matrixCopy.RowCount;

            // Reduce matrixCopy to bidiagonal form, storing the diagonal elements
            // In s and the super-diagonal elements in e.
            var nct = Math.Min(matrixCopy.RowCount - 1, matrixCopy.ColumnCount);
            var nrt = Math.Max(0, Math.Min(matrixCopy.ColumnCount - 2, matrixCopy.RowCount));
            var lu = Math.Max(nct, nrt);
            for (l = 0; l < lu; l++)
            {
                lp1 = l + 1;
                if (l < nct)
                {
                    // Compute the transformation for the l-th column and place the l-th diagonal in VectorS[l].
                    s[l] = Cnrm2Column(matrixCopy, matrixCopy.RowCount, l, l);
                    if (s[l].Magnitude != 0.0)
                    {
                        if (matrixCopy.At(l, l).Magnitude != 0.0)
                        {
                            s[l] = Csign(s[l], matrixCopy.At(l, l));
                        }

                        CscalColumn(matrixCopy, matrixCopy.RowCount, l, l, 1.0/s[l]);
                        matrixCopy.At(l, l, (Complex.One + matrixCopy.At(l, l)));
                    }

                    s[l] = -s[l];
                }

                for (j = lp1; j < matrixCopy.ColumnCount; j++)
                {
                    if (l < nct)
                    {
                        if (s[l].Magnitude != 0.0)
                        {
                            // Apply the transformation.
                            t = -Cdotc(matrixCopy, matrixCopy.RowCount, l, j, l)/matrixCopy.At(l, l);
                            if (t != Complex.Zero)
                            {
                                for (var ii = l; ii < matrixCopy.RowCount; ii++)
                                {
                                    matrixCopy.At(ii, j, matrixCopy.At(ii, j) + (t*matrixCopy.At(ii, l)));
                                }
                            }
                        }
                    }

                    // Place the l-th row of matrixCopy into  e for the
                    // Subsequent calculation of the row transformation.
                    e[j] = matrixCopy.At(l, j).Conjugate();
                }

                if (computeVectors && l < nct)
                {
                    // Place the transformation in u for subsequent back multiplication.
                    for (i = l; i < matrixCopy.RowCount; i++)
                    {
                        u.At(i, l, matrixCopy.At(i, l));
                    }
                }

                if (l >= nrt)
                {
                    continue;
                }

                // Compute the l-th row transformation and place the l-th super-diagonal in e(l).
                var enorm = Cnrm2Vector(e, lp1);
                e[l] = enorm;
                if (e[l].Magnitude != 0.0)
                {
                    if (e[lp1].Magnitude != 0.0)
                    {
                        e[l] = Csign(e[l], e[lp1]);
                    }

                    CscalVector(e, lp1, 1.0/e[l]);
                    e[lp1] = Complex.One + e[lp1];
                }

                e[l] = -e[l].Conjugate();
                if (lp1 < matrixCopy.RowCount && e[l].Magnitude != 0.0)
                {
                    // Apply the transformation.
                    for (i = lp1; i < matrixCopy.RowCount; i++)
                    {
                        work[i] = Complex.Zero;
                    }

                    for (j = lp1; j < matrixCopy.ColumnCount; j++)
                    {
                        if (e[j] != Complex.Zero)
                        {
                            for (var ii = lp1; ii < matrixCopy.RowCount; ii++)
                            {
                                work[ii] += e[j]*matrixCopy.At(ii, j);
                            }
                        }
                    }

                    for (j = lp1; j < matrixCopy.ColumnCount; j++)
                    {
                        var ww = (-e[j]/e[lp1]).Conjugate();
                        if (ww != Complex.Zero)
                        {
                            for (var ii = lp1; ii < matrixCopy.RowCount; ii++)
                            {
                                matrixCopy.At(ii, j, matrixCopy.At(ii, j) + (ww*work[ii]));
                            }
                        }
                    }
                }

                if (computeVectors)
                {
                    // Place the transformation in v for subsequent back multiplication.
                    for (i = lp1; i < matrixCopy.ColumnCount; i++)
                    {
                        vt.At(i, l, e[i]);
                    }
                }
            }

            // Set up the final bidiagonal matrixCopy or order m.
            var m = Math.Min(matrixCopy.ColumnCount, matrixCopy.RowCount + 1);
            var nctp1 = nct + 1;
            var nrtp1 = nrt + 1;
            if (nct < matrixCopy.ColumnCount)
            {
                s[nctp1 - 1] = matrixCopy.At((nctp1 - 1), (nctp1 - 1));
            }

            if (matrixCopy.RowCount < m)
            {
                s[m - 1] = Complex.Zero;
            }

            if (nrtp1 < m)
            {
                e[nrtp1 - 1] = matrixCopy.At((nrtp1 - 1), (m - 1));
            }

            e[m - 1] = Complex.Zero;

            // If required, generate u.
            if (computeVectors)
            {
                for (j = nctp1 - 1; j < ncu; j++)
                {
                    for (i = 0; i < matrixCopy.RowCount; i++)
                    {
                        u.At(i, j, Complex.Zero);
                    }

                    u.At(j, j, Complex.One);
                }

                for (l = nct - 1; l >= 0; l--)
                {
                    if (s[l].Magnitude != 0.0)
                    {
                        for (j = l + 1; j < ncu; j++)
                        {
                            t = -Cdotc(u, matrixCopy.RowCount, l, j, l)/u.At(l, l);
                            if (t != Complex.Zero)
                            {
                                for (var ii = l; ii < matrixCopy.RowCount; ii++)
                                {
                                    u.At(ii, j, u.At(ii, j) + (t*u.At(ii, l)));
                                }
                            }
                        }

                        CscalColumn(u, matrixCopy.RowCount, l, l, -1.0);
                        u.At(l, l, Complex.One + u.At(l, l));
                        for (i = 0; i < l; i++)
                        {
                            u.At(i, l, Complex.Zero);
                        }
                    }
                    else
                    {
                        for (i = 0; i < matrixCopy.RowCount; i++)
                        {
                            u.At(i, l, Complex.Zero);
                        }

                        u.At(l, l, Complex.One);
                    }
                }
            }

            // If it is required, generate v.
            if (computeVectors)
            {
                for (l = matrixCopy.ColumnCount - 1; l >= 0; l--)
                {
                    lp1 = l + 1;
                    if (l < nrt)
                    {
                        if (e[l].Magnitude != 0.0)
                        {
                            for (j = lp1; j < matrixCopy.ColumnCount; j++)
                            {
                                t = -Cdotc(vt, matrixCopy.ColumnCount, l, j, lp1)/vt.At(lp1, l);
                                if (t != Complex.Zero)
                                {
                                    for (var ii = l; ii < matrixCopy.ColumnCount; ii++)
                                    {
                                        vt.At(ii, j, vt.At(ii, j) + (t*vt.At(ii, l)));
                                    }
                                }
                            }
                        }
                    }

                    for (i = 0; i < matrixCopy.ColumnCount; i++)
                    {
                        vt.At(i, l, Complex.Zero);
                    }

                    vt.At(l, l, Complex.One);
                }
            }

            // Transform s and e so that they are real .
            for (i = 0; i < m; i++)
            {
                Complex r;
                if (s[i].Magnitude != 0.0)
                {
                    t = s[i].Magnitude;
                    r = s[i]/t;
                    s[i] = t;
                    if (i < m - 1)
                    {
                        e[i] = e[i]/r;
                    }

                    if (computeVectors)
                    {
                        CscalColumn(u, matrixCopy.RowCount, i, 0, r);
                    }
                }

                // Exit
                if (i == m - 1)
                {
                    break;
                }

                if (e[i].Magnitude != 0.0)
                {
                    t = e[i].Magnitude;
                    r = t/e[i];
                    e[i] = t;
                    s[i + 1] = s[i + 1]*r;
                    if (computeVectors)
                    {
                        CscalColumn(vt, matrixCopy.ColumnCount, i + 1, 0, r);
                    }
                }
            }

            // Main iteration loop for the singular values.
            var mn = m;
            var iter = 0;

            while (m > 0)
            {
                // Quit if all the singular values have been found. If too many iterations have been performed, 
                // throw exception that Convergence Failed
                if (iter >= maxiter)
                {
                    throw new NonConvergenceException();
                }

                // This section of the program inspects for negligible elements in the s and e arrays. On
                // completion the variables kase and l are set as follows.
                // Kase = 1     if VectorS[m] and e[l-1] are negligible and l < m
                // Kase = 2     if VectorS[l] is negligible and l < m
                // Kase = 3     if e[l-1] is negligible, l < m, and VectorS[l, ..., VectorS[m] are not negligible (qr step).
                // Лase = 4     if e[m-1] is negligible (convergence).
                double ztest;
                double test;
                for (l = m - 2; l >= 0; l--)
                {
                    test = s[l].Magnitude + s[l + 1].Magnitude;
                    ztest = test + e[l].Magnitude;
                    if (ztest.AlmostEqualRelative(test, 15))
                    {
                        e[l] = Complex.Zero;
                        break;
                    }
                }

                int kase;
                if (l == m - 2)
                {
                    kase = 4;
                }
                else
                {
                    int ls;
                    for (ls = m - 1; ls > l; ls--)
                    {
                        test = 0.0;
                        if (ls != m - 1)
                        {
                            test = test + e[ls].Magnitude;
                        }

                        if (ls != l + 1)
                        {
                            test = test + e[ls - 1].Magnitude;
                        }

                        ztest = test + s[ls].Magnitude;
                        if (ztest.AlmostEqualRelative(test, 15))
                        {
                            s[ls] = Complex.Zero;
                            break;
                        }
                    }

                    if (ls == l)
                    {
                        kase = 3;
                    }
                    else if (ls == m - 1)
                    {
                        kase = 1;
                    }
                    else
                    {
                        kase = 2;
                        l = ls;
                    }
                }

                l = l + 1;

                // Perform the task indicated by kase.
                int k;
                double f;
                double cs;
                double sn;
                switch (kase)
                {
                        // Deflate negligible VectorS[m].
                    case 1:
                        f = e[m - 2].Real;
                        e[m - 2] = Complex.Zero;
                        double t1;
                        for (var kk = l; kk < m - 1; kk++)
                        {
                            k = m - 2 - kk + l;
                            t1 = s[k].Real;
                            Srotg(ref t1, ref f, out cs, out sn);
                            s[k] = t1;
                            if (k != l)
                            {
                                f = -sn*e[k - 1].Real;
                                e[k - 1] = cs*e[k - 1];
                            }

                            if (computeVectors)
                            {
                                Csrot(vt, matrixCopy.ColumnCount, k, m - 1, cs, sn);
                            }
                        }

                        break;

                        // Split at negligible VectorS[l].
                    case 2:
                        f = e[l - 1].Real;
                        e[l - 1] = Complex.Zero;
                        for (k = l; k < m; k++)
                        {
                            t1 = s[k].Real;
                            Srotg(ref t1, ref f, out cs, out sn);
                            s[k] = t1;
                            f = -sn*e[k].Real;
                            e[k] = cs*e[k];
                            if (computeVectors)
                            {
                                Csrot(u, matrixCopy.RowCount, k, l - 1, cs, sn);
                            }
                        }

                        break;

                        // Perform one qr step.
                    case 3:
                        // Calculate the shift.
                        var scale = 0.0;
                        scale = Math.Max(scale, s[m - 1].Magnitude);
                        scale = Math.Max(scale, s[m - 2].Magnitude);
                        scale = Math.Max(scale, e[m - 2].Magnitude);
                        scale = Math.Max(scale, s[l].Magnitude);
                        scale = Math.Max(scale, e[l].Magnitude);
                        var sm = s[m - 1].Real/scale;
                        var smm1 = s[m - 2].Real/scale;
                        var emm1 = e[m - 2].Real/scale;
                        var sl = s[l].Real/scale;
                        var el = e[l].Real/scale;
                        var b = (((smm1 + sm)*(smm1 - sm)) + (emm1*emm1))/2.0;
                        var c = (sm*emm1)*(sm*emm1);
                        var shift = 0.0;

                        if (b != 0.0 || c != 0.0)
                        {
                            shift = Math.Sqrt((b*b) + c);
                            if (b < 0.0)
                            {
                                shift = -shift;
                            }

                            shift = c/(b + shift);
                        }

                        f = ((sl + sm)*(sl - sm)) + shift;
                        var g = sl*el;

                        // Chase zeros.
                        for (k = l; k < m - 1; k++)
                        {
                            Srotg(ref f, ref g, out cs, out sn);
                            if (k != l)
                            {
                                e[k - 1] = f;
                            }

                            f = (cs*s[k].Real) + (sn*e[k].Real);
                            e[k] = (cs*e[k]) - (sn*s[k]);
                            g = sn*s[k + 1].Real;
                            s[k + 1] = cs*s[k + 1];
                            if (computeVectors)
                            {
                                Csrot(vt, matrixCopy.ColumnCount, k, k + 1, cs, sn);
                            }

                            Srotg(ref f, ref g, out cs, out sn);
                            s[k] = f;
                            f = (cs*e[k].Real) + (sn*s[k + 1].Real);
                            s[k + 1] = (-sn*e[k]) + (cs*s[k + 1]);
                            g = sn*e[k + 1].Real;
                            e[k + 1] = cs*e[k + 1];
                            if (computeVectors && k < matrixCopy.RowCount)
                            {
                                Csrot(u, matrixCopy.RowCount, k, k + 1, cs, sn);
                            }
                        }

                        e[m - 2] = f;
                        iter = iter + 1;
                        break;

                        // Convergence.
                    case 4:
                        // Make the singular value  positive
                        if (s[l].Real < 0.0)
                        {
                            s[l] = -s[l];
                            if (computeVectors)
                            {
                                CscalColumn(vt, matrixCopy.ColumnCount, l, 0, -1.0);
                            }
                        }

                        // Order the singular value.
                        while (l != mn - 1)
                        {
                            if (s[l].Real >= s[l + 1].Real)
                            {
                                break;
                            }

                            t = s[l];
                            s[l] = s[l + 1];
                            s[l + 1] = t;
                            if (computeVectors && l < matrixCopy.ColumnCount)
                            {
                                Swap(vt, matrixCopy.ColumnCount, l, l + 1);
                            }

                            if (computeVectors && l < matrixCopy.RowCount)
                            {
                                Swap(u, matrixCopy.RowCount, l, l + 1);
                            }

                            l = l + 1;
                        }

                        iter = 0;
                        m = m - 1;
                        break;
                }
            }

            if (computeVectors)
            {
                vt = vt.ConjugateTranspose();
            }

            // Adjust the size of s if rows < columns. We are using ported copy of linpack's svd code and it uses
            // a singular vector of length mRows+1 when mRows < mColumns. The last element is not used and needs to be removed.
            // we should port lapack's svd routine to remove this problem.
            if (matrixCopy.RowCount < matrixCopy.ColumnCount)
            {
                nm--;
                var tmp = Vector<Complex>.Build.SameAs(matrixCopy, nm);
                for (i = 0; i < nm; i++)
                {
                    tmp[i] = s[i];
                }

                s = tmp;
            }

            return new UserSvd(s, u, vt, computeVectors);
        }
        public static void CalculateNormalVectorInPointUsingPCA(double p_x, double p_y, double p_z, List<Cl3DModel.Cl3DModelPointIterator> p_pNeighborhood, out Vector p_NormalVector)
        {
            p_NormalVector = new Vector(new double[] { 0, 0, 0 });

            float MeanX = 0;
            float MeanY = 0;
            float MeanZ = 0;
            Matrix xy = new Matrix(3, p_pNeighborhood.Count);
            for (int i = 0; i < p_pNeighborhood.Count; i++)
            {
                xy[0, i] = p_pNeighborhood[i].X;
                xy[1, i] = p_pNeighborhood[i].Y;
                xy[2, i] = p_pNeighborhood[i].Z;

                MeanX += p_pNeighborhood[i].X;
                MeanY += p_pNeighborhood[i].Y;
                MeanZ += p_pNeighborhood[i].Z;

            }
            MeanX /= p_pNeighborhood.Count;
            MeanY /= p_pNeighborhood.Count;
            MeanZ /= p_pNeighborhood.Count;

            for (int i = 0; i < p_pNeighborhood.Count; i++)
            {
                xy[0, i] -= MeanX;
                xy[1, i] -= MeanY;
                xy[2, i] -= MeanZ;
            }

            Matrix Transpose = xy.Clone();
            Transpose.Transpose();
            Matrix Corelation = xy * Transpose;

            SingularValueDecomposition SVD = new SingularValueDecomposition(Corelation);

            p_NormalVector[0] = SVD.RightSingularVectors[0, 2];
            p_NormalVector[1] = SVD.RightSingularVectors[1, 2];
            p_NormalVector[2] = SVD.RightSingularVectors[2, 2];
        }
Пример #27
0
        /// <summary>
        /// Initializes a new instance of the <see cref="UserSvd"/> class. This object will compute the
        /// the singular value decomposition when the constructor is called and cache it's decomposition.
        /// </summary>
        /// <param name="matrix">The matrix to factor.</param>
        /// <param name="computeVectors">Compute the singular U and VT vectors or not.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="matrix"/> is <c>null</c>.</exception>
        /// <exception cref="NonConvergenceException"></exception>
        public UserSvd(Matrix<float> matrix, bool computeVectors)
        {
            if (matrix == null)
            {
                throw new ArgumentNullException("matrix");
            }

            ComputeVectors = computeVectors;
            var nm = Math.Min(matrix.RowCount + 1, matrix.ColumnCount);
            var matrixCopy = matrix.Clone();

            VectorS = matrixCopy.CreateVector(nm);
            MatrixU = matrixCopy.CreateMatrix(matrixCopy.RowCount, matrixCopy.RowCount);
            MatrixVT = matrixCopy.CreateMatrix(matrixCopy.ColumnCount, matrixCopy.ColumnCount);

            const int Maxiter = 1000;
            var e = new float[matrixCopy.ColumnCount];
            var work = new float[matrixCopy.RowCount];

            int i, j;
            int l, lp1;
            var cs = 0.0f;
            var sn = 0.0f;
            float t;

            var ncu = matrixCopy.RowCount;

            // Reduce matrixCopy to bidiagonal form, storing the diagonal elements
            // In s and the super-diagonal elements in e.
            var nct = Math.Min(matrixCopy.RowCount - 1, matrixCopy.ColumnCount);
            var nrt = Math.Max(0, Math.Min(matrixCopy.ColumnCount - 2, matrixCopy.RowCount));
            var lu = Math.Max(nct, nrt);
            for (l = 0; l < lu; l++)
            {
                lp1 = l + 1;
                if (l < nct)
                {
                    // Compute the transformation for the l-th column and place the l-th diagonal in VectorS[l].
                    var xnorm = Dnrm2Column(matrixCopy, matrixCopy.RowCount, l, l);
                    VectorS[l] = xnorm;
                    if (VectorS[l] != 0.0)
                    {
                        if (matrixCopy.At(l, l) != 0.0)
                        {
                            VectorS[l] = Dsign(VectorS[l], matrixCopy.At(l, l));
                        }

                        DscalColumn(matrixCopy, matrixCopy.RowCount, l, l, 1.0f / VectorS[l]);
                        matrixCopy.At(l, l, (1.0f + matrixCopy.At(l, l)));
                    }

                    VectorS[l] = -VectorS[l];
                }

                for (j = lp1; j < matrixCopy.ColumnCount; j++)
                {
                    if (l < nct)
                    {
                        if (VectorS[l] != 0.0)
                        {
                            // Apply the transformation.
                            t = -Ddot(matrixCopy, matrixCopy.RowCount, l, j, l) / matrixCopy.At(l, l);
                            for (var ii = l; ii < matrixCopy.RowCount; ii++)
                            {
                                matrixCopy.At(ii, j, matrixCopy.At(ii, j) + (t * matrixCopy.At(ii, l)));
                            }
                        }
                    }

                    // Place the l-th row of matrixCopy into  e for the
                    // Subsequent calculation of the row transformation.
                    e[j] = matrixCopy.At(l, j);
                }

                if (ComputeVectors && l < nct)
                {
                    // Place the transformation in u for subsequent back multiplication.
                    for (i = l; i < matrixCopy.RowCount; i++)
                    {
                        MatrixU.At(i, l, matrixCopy.At(i, l));
                    }
                }

                if (l >= nrt)
                {
                    continue;
                }

                // Compute the l-th row transformation and place the l-th super-diagonal in e(l).
                var enorm = Dnrm2Vector(e, lp1);
                e[l] = enorm;
                if (e[l] != 0.0)
                {
                    if (e[lp1] != 0.0)
                    {
                        e[l] = Dsign(e[l], e[lp1]);
                    }

                    DscalVector(e, lp1, 1.0f / e[l]);
                    e[lp1] = 1.0f + e[lp1];
                }

                e[l] = -e[l];
                if (lp1 < matrixCopy.RowCount && e[l] != 0.0)
                {
                    // Apply the transformation.
                    for (i = lp1; i < matrixCopy.RowCount; i++)
                    {
                        work[i] = 0.0f;
                    }

                    for (j = lp1; j < matrixCopy.ColumnCount; j++)
                    {
                        for (var ii = lp1; ii < matrixCopy.RowCount; ii++)
                        {
                            work[ii] += e[j] * matrixCopy.At(ii, j);
                        }
                    }

                    for (j = lp1; j < matrixCopy.ColumnCount; j++)
                    {
                        var ww = -e[j] / e[lp1];
                        for (var ii = lp1; ii < matrixCopy.RowCount; ii++)
                        {
                            matrixCopy.At(ii, j, matrixCopy.At(ii, j) + (ww * work[ii]));
                        }
                    }
                }

                if (ComputeVectors)
                {
                    // Place the transformation in v for subsequent back multiplication.
                    for (i = lp1; i < matrixCopy.ColumnCount; i++)
                    {
                        MatrixVT.At(i, l, e[i]);
                    }
                }
            }

            // Set up the final bidiagonal matrixCopy or order m.
            var m = Math.Min(matrixCopy.ColumnCount, matrixCopy.RowCount + 1);
            var nctp1 = nct + 1;
            var nrtp1 = nrt + 1;
            if (nct < matrixCopy.ColumnCount)
            {
                VectorS[nctp1 - 1] = matrixCopy.At((nctp1 - 1), (nctp1 - 1));
            }

            if (matrixCopy.RowCount < m)
            {
                VectorS[m - 1] = 0.0f;
            }

            if (nrtp1 < m)
            {
                e[nrtp1 - 1] = matrixCopy.At((nrtp1 - 1), (m - 1));
            }

            e[m - 1] = 0.0f;

            // If required, generate u.
            if (ComputeVectors)
            {
                for (j = nctp1 - 1; j < ncu; j++)
                {
                    for (i = 0; i < matrixCopy.RowCount; i++)
                    {
                        MatrixU.At(i, j, 0.0f);
                    }

                    MatrixU.At(j, j, 1.0f);
                }

                for (l = nct - 1; l >= 0; l--)
                {
                    if (VectorS[l] != 0.0)
                    {
                        for (j = l + 1; j < ncu; j++)
                        {
                            t = -Ddot(MatrixU, matrixCopy.RowCount, l, j, l) / MatrixU.At(l, l);
                            for (var ii = l; ii < matrixCopy.RowCount; ii++)
                            {
                                MatrixU.At(ii, j, MatrixU.At(ii, j) + (t * MatrixU.At(ii, l)));
                            }
                        }

                        DscalColumn(MatrixU, matrixCopy.RowCount, l, l, -1.0f);
                        MatrixU.At(l, l, 1.0f + MatrixU.At(l, l));
                        for (i = 0; i < l; i++)
                        {
                            MatrixU.At(i, l, 0.0f);
                        }
                    }
                    else
                    {
                        for (i = 0; i < matrixCopy.RowCount; i++)
                        {
                            MatrixU.At(i, l, 0.0f);
                        }

                        MatrixU.At(l, l, 1.0f);
                    }
                }
            }

            // If it is required, generate v.
            if (ComputeVectors)
            {
                for (l = matrixCopy.ColumnCount - 1; l >= 0; l--)
                {
                    lp1 = l + 1;
                    if (l < nrt)
                    {
                        if (e[l] != 0.0)
                        {
                            for (j = lp1; j < matrixCopy.ColumnCount; j++)
                            {
                                t = -Ddot(MatrixVT, matrixCopy.ColumnCount, l, j, lp1) / MatrixVT.At(lp1, l);
                                for (var ii = l; ii < matrixCopy.ColumnCount; ii++)
                                {
                                    MatrixVT.At(ii, j, MatrixVT.At(ii, j) + (t * MatrixVT.At(ii, l)));
                                }
                            }
                        }
                    }

                    for (i = 0; i < matrixCopy.ColumnCount; i++)
                    {
                        MatrixVT.At(i, l, 0.0f);
                    }

                    MatrixVT.At(l, l, 1.0f);
                }
            }

            // Transform s and e so that they are  float .
            for (i = 0; i < m; i++)
            {
                float r;
                if (VectorS[i] != 0.0)
                {
                    t = VectorS[i];
                    r = VectorS[i] / t;
                    VectorS[i] = t;
                    if (i < m - 1)
                    {
                        e[i] = e[i] / r;
                    }

                    if (ComputeVectors)
                    {
                        DscalColumn(MatrixU, matrixCopy.RowCount, i, 0, r);
                    }
                }

                // Exit
                if (i == m - 1)
                {
                    break;
                }

                if (e[i] != 0.0)
                {
                    t = e[i];
                    r = t / e[i];
                    e[i] = t;
                    VectorS[i + 1] = VectorS[i + 1] * r;
                    if (ComputeVectors)
                    {
                        DscalColumn(MatrixVT, matrixCopy.ColumnCount, i + 1, 0, r);
                    }
                }
            }

            // Main iteration loop for the singular values.
            var mn = m;
            var iter = 0;

            while (m > 0)
            {
                // Quit if all the singular values have been found. If too many iterations have been performed, 
                // throw exception that Convergence Failed
                if (iter >= Maxiter)
                {
                    throw new NonConvergenceException();
                }

                // This section of the program inspects for negligible elements in the s and e arrays. On
                // completion the variables kase and l are set as follows.
                // Kase = 1     if VectorS[m] and e[l-1] are negligible and l < m
                // Kase = 2     if VectorS[l] is negligible and l < m
                // Kase = 3     if e[l-1] is negligible, l < m, and VectorS[l, ..., VectorS[m] are not negligible (qr step).
                // Лase = 4     if e[m-1] is negligible (convergence).
                float ztest;
                float test;
                for (l = m - 2; l >= 0; l--)
                {
                    test = Math.Abs(VectorS[l]) + Math.Abs(VectorS[l + 1]);
                    ztest = test + Math.Abs(e[l]);
                    if (ztest.AlmostEqualInDecimalPlaces(test, 7))
                    {
                        e[l] = 0.0f;
                        break;
                    }
                }

                int kase;
                if (l == m - 2)
                {
                    kase = 4;
                }
                else
                {
                    int ls;
                    for (ls = m - 1; ls > l; ls--)
                    {
                        test = 0.0f;
                        if (ls != m - 1)
                        {
                            test = test + Math.Abs(e[ls]);
                        }

                        if (ls != l + 1)
                        {
                            test = test + Math.Abs(e[ls - 1]);
                        }

                        ztest = test + Math.Abs(VectorS[ls]);
                        if (ztest.AlmostEqualInDecimalPlaces(test, 7))
                        {
                            VectorS[ls] = 0.0f;
                            break;
                        }
                    }

                    if (ls == l)
                    {
                        kase = 3;
                    }
                    else if (ls == m - 1)
                    {
                        kase = 1;
                    }
                    else
                    {
                        kase = 2;
                        l = ls;
                    }
                }

                l = l + 1;

                // Perform the task indicated by kase.
                int k;
                float f;
                switch (kase)
                {
                    // Deflate negligible VectorS[m].
                    case 1:
                        f = e[m - 2];
                        e[m - 2] = 0.0f;
                        float t1;
                        for (var kk = l; kk < m - 1; kk++)
                        {
                            k = m - 2 - kk + l;
                            t1 = VectorS[k];
                            Drotg(ref t1, ref f, ref cs, ref sn);
                            VectorS[k] = t1;
                            if (k != l)
                            {
                                f = -sn * e[k - 1];
                                e[k - 1] = cs * e[k - 1];
                            }

                            if (ComputeVectors)
                            {
                                Drot(MatrixVT, matrixCopy.ColumnCount, k, m - 1, cs, sn);
                            }
                        }

                        break;

                    // Split at negligible VectorS[l].
                    case 2:
                        f = e[l - 1];
                        e[l - 1] = 0.0f;
                        for (k = l; k < m; k++)
                        {
                            t1 = VectorS[k];
                            Drotg(ref t1, ref f, ref cs, ref sn);
                            VectorS[k] = t1;
                            f = -sn * e[k];
                            e[k] = cs * e[k];
                            if (ComputeVectors)
                            {
                                Drot(MatrixU, matrixCopy.RowCount, k, l - 1, cs, sn);
                            }
                        }

                        break;

                    // Perform one qr step.
                    case 3:
                        // Calculate the shift.
                        var scale = 0.0f;
                        scale = Math.Max(scale, Math.Abs(VectorS[m - 1]));
                        scale = Math.Max(scale, Math.Abs(VectorS[m - 2]));
                        scale = Math.Max(scale, Math.Abs(e[m - 2]));
                        scale = Math.Max(scale, Math.Abs(VectorS[l]));
                        scale = Math.Max(scale, Math.Abs(e[l]));
                        var sm = VectorS[m - 1] / scale;
                        var smm1 = VectorS[m - 2] / scale;
                        var emm1 = e[m - 2] / scale;
                        var sl = VectorS[l] / scale;
                        var el = e[l] / scale;
                        var b = (((smm1 + sm) * (smm1 - sm)) + (emm1 * emm1)) / 2.0f;
                        var c = (sm * emm1) * (sm * emm1);
                        var shift = 0.0f;
                        if (b != 0.0 || c != 0.0)
                        {
                            shift = (float)Math.Sqrt((b * b) + c);
                            if (b < 0.0)
                            {
                                shift = -shift;
                            }

                            shift = c / (b + shift);
                        }

                        f = ((sl + sm) * (sl - sm)) + shift;
                        var g = sl * el;

                        // Chase zeros.
                        for (k = l; k < m - 1; k++)
                        {
                            Drotg(ref f, ref g, ref cs, ref sn);
                            if (k != l)
                            {
                                e[k - 1] = f;
                            }

                            f = (cs * VectorS[k]) + (sn * e[k]);
                            e[k] = (cs * e[k]) - (sn * VectorS[k]);
                            g = sn * VectorS[k + 1];
                            VectorS[k + 1] = cs * VectorS[k + 1];
                            if (ComputeVectors)
                            {
                                Drot(MatrixVT, matrixCopy.ColumnCount, k, k + 1, cs, sn);
                            }

                            Drotg(ref f, ref g, ref cs, ref sn);
                            VectorS[k] = f;
                            f = (cs * e[k]) + (sn * VectorS[k + 1]);
                            VectorS[k + 1] = (-sn * e[k]) + (cs * VectorS[k + 1]);
                            g = sn * e[k + 1];
                            e[k + 1] = cs * e[k + 1];
                            if (ComputeVectors && k < matrixCopy.RowCount)
                            {
                                Drot(MatrixU, matrixCopy.RowCount, k, k + 1, cs, sn);
                            }
                        }

                        e[m - 2] = f;
                        iter = iter + 1;
                        break;

                    // Convergence.
                    case 4:
                        // Make the singular value  positive
                        if (VectorS[l] < 0.0)
                        {
                            VectorS[l] = -VectorS[l];
                            if (ComputeVectors)
                            {
                                DscalColumn(MatrixVT, matrixCopy.ColumnCount, l, 0, -1.0f);
                            }
                        }

                        // Order the singular value.
                        while (l != mn - 1)
                        {
                            if (VectorS[l] >= VectorS[l + 1])
                            {
                                break;
                            }

                            t = VectorS[l];
                            VectorS[l] = VectorS[l + 1];
                            VectorS[l + 1] = t;
                            if (ComputeVectors && l < matrixCopy.ColumnCount)
                            {
                                Dswap(MatrixVT, matrixCopy.ColumnCount, l, l + 1);
                            }

                            if (ComputeVectors && l < matrixCopy.RowCount)
                            {
                                Dswap(MatrixU, matrixCopy.RowCount, l, l + 1);
                            }

                            l = l + 1;
                        }

                        iter = 0;
                        m = m - 1;
                        break;
                }
            }

            if (ComputeVectors)
            {
                MatrixVT = MatrixVT.Transpose();
            }

            // Adjust the size of s if rows < columns. We are using ported copy of linpack's svd code and it uses
            // a singular vector of length mRows+1 when mRows < mColumns. The last element is not used and needs to be removed.
            // we should port lapack's svd routine to remove this problem.
            if (matrixCopy.RowCount < matrixCopy.ColumnCount)
            {
                nm--;
                var tmp = matrixCopy.CreateVector(nm);
                for (i = 0; i < nm; i++)
                {
                    tmp[i] = VectorS[i];
                }

                VectorS = tmp;
            }
        }
Пример #28
0
        /// <summary>
        /// Solves a system of linear equations, <b>AX = B</b>, with A QR factorized.
        /// </summary>
        /// <param name="input">The right hand side <see cref="Matrix{T}"/>, <b>B</b>.</param>
        /// <param name="result">The left hand side <see cref="Matrix{T}"/>, <b>X</b>.</param>
        public override void Solve(Matrix<Complex32> input, Matrix<Complex32> result)
        {
            // Check for proper arguments.
            if (input == null)
            {
                throw new ArgumentNullException("input");
            }

            if (result == null)
            {
                throw new ArgumentNullException("result");
            }

            // The solution X should have the same number of columns as B
            if (input.ColumnCount != result.ColumnCount)
            {
                throw new ArgumentException(Resources.ArgumentMatrixSameColumnDimension);
            }

            // The dimension compatibility conditions for X = A\B require the two matrices A and B to have the same number of rows
            if (MatrixQ.RowCount != input.RowCount)
            {
                throw new ArgumentException(Resources.ArgumentMatrixSameRowDimension);
            }

            // The solution X row dimension is equal to the column dimension of A
            if (MatrixQ.ColumnCount != result.RowCount)
            {
                throw new ArgumentException(Resources.ArgumentMatrixSameColumnDimension);
            }

            var inputCopy = input.Clone();
            
            // Compute Y = transpose(Q)*B
            var column = new Complex32[MatrixQ.RowCount];
            for (var j = 0; j < input.ColumnCount; j++)
            {
                for (var k = 0; k < MatrixQ.RowCount; k++)
                {
                    column[k] = inputCopy.At(k, j);
                }

                for (var i = 0; i < MatrixQ.ColumnCount; i++)
                {
                    var s = Complex32.Zero;
                    for (var k = 0; k < MatrixQ.RowCount; k++)
                    {
                        s += MatrixQ.At(k, i).Conjugate() * column[k];
                    }

                    inputCopy.At(i, j, s);
                }
            }

            // Solve R*X = Y;
            for (var k = MatrixQ.ColumnCount - 1; k >= 0; k--)
            {
                for (var j = 0; j < input.ColumnCount; j++)
                {
                    inputCopy.At(k, j, inputCopy.At(k, j) / MatrixR.At(k, k));
                }

                for (var i = 0; i < k; i++)
                {
                    for (var j = 0; j < input.ColumnCount; j++)
                    {
                        inputCopy.At(i, j, inputCopy.At(i, j) - (inputCopy.At(k, j) * MatrixR.At(i, k)));
                    }
                }
            }

            for (var i = 0; i < MatrixR.ColumnCount; i++)
            {
                for (var j = 0; j < input.ColumnCount; j++)
                {
                    result.At(i, j, inputCopy.At(i, j));
                }
            }
        }
Пример #29
0
        /// <summary>
        /// Samples a matrix normal distributed random variable.
        /// </summary>
        /// <param name="rnd">The random number generator to use.</param>
        /// <param name="m">The mean of the matrix normal.</param>
        /// <param name="v">The covariance matrix for the rows.</param>
        /// <param name="k">The covariance matrix for the columns.</param>
        /// <exception cref="ArgumentOutOfRangeException">If the dimensions of the mean and two covariance matrices don't match.</exception>
        /// <returns>a sequence of samples from the distribution.</returns>
        public static Matrix<double> Sample(Random rnd, Matrix<double> m, Matrix<double> v, Matrix<double> k)
        {
            if (Control.CheckDistributionParameters && !IsValidParameterSet(m, v, k))
            {
                throw new ArgumentOutOfRangeException(Resources.InvalidDistributionParameters);
            }

            var n = m.RowCount;
            var p = m.ColumnCount;

            // Compute the Kronecker product of V and K, this is the covariance matrix for the stacked matrix.
            var vki = v.KroneckerProduct(k.Inverse());

            // Sample a vector valued random variable with VKi as the covariance.
            var vector = SampleVectorNormal(rnd, new DenseVector(n * p, 0.0), vki);

            // Unstack the vector v and add the mean.
            var r = m.Clone();
            for (var i = 0; i < n; i++)
            {
                for (var j = 0; j < p; j++)
                {
                    r[i, j] += vector[(j * n) + i];
                }
            }

            return r;
        }
Пример #30
0
        /// <summary>
        /// Decomposes the specified matrix using a LU decomposition.
        /// </summary>
        /// <param name="matrix">The matrix to decompose.</param>
        public void Decompose(Matrix matrix)
        {
            LU = matrix.Clone();

             pivots = new int[LU.Rows];

             for (var i = 0; i < LU.Rows; i++)
             {
            pivots[i] = i;
             }

             pivotSign = 1;

             var column = new double[LU.Rows];

             for (var j = 0; j < LU.Columns; j++)
             {
            for (var i = 0; i < LU.Rows; i++)
            {
               column[i] = LU.GetValue(i, j);
            }

            // Apply previous transformations.
            for (var i = 0; i < LU.Rows; i++)
            {
               // Most of the time is spent in the following dot product.
               var kmax = Math.Min(i, j);
               var s = 0.0;

               for (var k = 0; k < kmax; k++)
               {
                  s += LU.GetValue(i, k) * column[k];
               }

               LU.SetValue(i, j, column[i] - s);
               column[i] -= s;
            }

            // Find pivot and exchange if necessary.
            var p = j;

            for (var i = j + 1; i < LU.Rows; i++)
            {
               if (Math.Abs(column[i]) > Math.Abs(column[p]))
               {
                  p = i;
               }
            }

            if (p != j)
            {
               for (var k = 0; k < LU.Columns; k++)
               {
                  var t = LU[p, k];
                  LU.SetValue(p, k, LU[j, k]);
                  LU.SetValue(j, k, t);
               }

               Swapper.Swap(pivots, p, j);

               pivotSign = -pivotSign;
            }

            // Compute multipliers.
            if ((j < LU.Rows) && (LU.GetValue(j, j) != 0.0))
            {
               for (var i = j + 1; i < LU.Rows; i++)
               {
                  LU.SetValue(i, j, LU.GetValue(i, j) / LU.GetValue(j, j));
               }
            }
             }
        }