FieldQRDecomposition.java

/*
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The ASF licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *      https://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/*
 * This is not the original file distributed by the Apache Software Foundation
 * It has been modified by the Hipparchus project
 */

package org.hipparchus.linear;

import java.util.Arrays;
import java.util.function.Predicate;

import org.hipparchus.CalculusFieldElement;
import org.hipparchus.FieldElement;
import org.hipparchus.exception.LocalizedCoreFormats;
import org.hipparchus.exception.MathIllegalArgumentException;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.MathArrays;


/**
 * Calculates the QR-decomposition of a field matrix.
 * <p>The QR-decomposition of a matrix A consists of two matrices Q and R
 * that satisfy: A = QR, Q is orthogonal (Q<sup>T</sup>Q = I), and R is
 * upper triangular. If A is m&times;n, Q is m&times;m and R m&times;n.</p>
 * <p>This class compute the decomposition using Householder reflectors.</p>
 * <p>For efficiency purposes, the decomposition in packed form is transposed.
 * This allows inner loop to iterate inside rows, which is much more cache-efficient
 * in Java.</p>
 * <p>This class is based on the class {@link QRDecomposition}.</p>
 *
 * @param <T> type of the underlying field elements
 * @see <a href="http://mathworld.wolfram.com/QRDecomposition.html">MathWorld</a>
 * @see <a href="http://en.wikipedia.org/wiki/QR_decomposition">Wikipedia</a>
 *
 */
public class FieldQRDecomposition<T extends CalculusFieldElement<T>> {
    /**
     * A packed TRANSPOSED representation of the QR decomposition.
     * <p>The elements BELOW the diagonal are the elements of the UPPER triangular
     * matrix R, and the rows ABOVE the diagonal are the Householder reflector vectors
     * from which an explicit form of Q can be recomputed if desired.</p>
     */
    private T[][] qrt;
    /** The diagonal elements of R. */
    private T[] rDiag;
    /** Cached value of Q. */
    private FieldMatrix<T> cachedQ;
    /** Cached value of QT. */
    private FieldMatrix<T> cachedQT;
    /** Cached value of R. */
    private FieldMatrix<T> cachedR;
    /** Cached value of H. */
    private FieldMatrix<T> cachedH;
    /** Singularity threshold. */
    private final T threshold;
    /** checker for zero. */
    private final Predicate<T> zeroChecker;

    /**
     * Calculates the QR-decomposition of the given matrix.
     * The singularity threshold defaults to zero.
     *
     * @param matrix The matrix to decompose.
     *
     * @see #FieldQRDecomposition(FieldMatrix, CalculusFieldElement)
     */
    public FieldQRDecomposition(FieldMatrix<T> matrix) {
        this(matrix, matrix.getField().getZero());
    }

    /**
     * Calculates the QR-decomposition of the given matrix.
     *
     * @param matrix The matrix to decompose.
     * @param threshold Singularity threshold.
     */
    public FieldQRDecomposition(FieldMatrix<T> matrix, T threshold) {
        this(matrix, threshold, FieldElement::isZero);
    }

    /**
     * Calculates the QR-decomposition of the given matrix.
     *
     * @param matrix The matrix to decompose.
     * @param threshold Singularity threshold.
     * @param zeroChecker checker for zero
     */
    public FieldQRDecomposition(FieldMatrix<T> matrix, T threshold, Predicate<T> zeroChecker) {
        this.threshold   = threshold;
        this.zeroChecker = zeroChecker;

        final int m = matrix.getRowDimension();
        final int n = matrix.getColumnDimension();
        qrt = matrix.transpose().getData();
        rDiag = MathArrays.buildArray(threshold.getField(),FastMath.min(m, n));
        cachedQ  = null;
        cachedQT = null;
        cachedR  = null;
        cachedH  = null;

        decompose(qrt);

    }

    /** Decompose matrix.
     * @param matrix transposed matrix
     */
    protected void decompose(T[][] matrix) {
        for (int minor = 0; minor < FastMath.min(matrix.length, matrix[0].length); minor++) {
            performHouseholderReflection(minor, matrix);
        }
    }

    /** Perform Householder reflection for a minor A(minor, minor) of A.
     * @param minor minor index
     * @param matrix transposed matrix
     */
    protected void performHouseholderReflection(int minor, T[][] matrix) {

        final T[] qrtMinor = matrix[minor];
        final T zero = threshold.getField().getZero();
        /*
         * Let x be the first column of the minor, and a^2 = |x|^2.
         * x will be in the positions qr[minor][minor] through qr[m][minor].
         * The first column of the transformed minor will be (a,0,0,..)'
         * The sign of a is chosen to be opposite to the sign of the first
         * component of x. Let's find a:
         */
        T xNormSqr = zero;
        for (int row = minor; row < qrtMinor.length; row++) {
            final T c = qrtMinor[row];
            xNormSqr = xNormSqr.add(c.square());
        }
        final T a = (qrtMinor[minor].getReal() > 0) ? xNormSqr.sqrt().negate() : xNormSqr.sqrt();
        rDiag[minor] = a;

        if (!zeroChecker.test(a)) {

            /*
             * Calculate the normalized reflection vector v and transform
             * the first column. We know the norm of v beforehand: v = x-ae
             * so |v|^2 = <x-ae,x-ae> = <x,x>-2a<x,e>+a^2<e,e> =
             * a^2+a^2-2a<x,e> = 2a*(a - <x,e>).
             * Here <x, e> is now qr[minor][minor].
             * v = x-ae is stored in the column at qr:
             */
            qrtMinor[minor] = qrtMinor[minor].subtract(a); // now |v|^2 = -2a*(qr[minor][minor])

            /*
             * Transform the rest of the columns of the minor:
             * They will be transformed by the matrix H = I-2vv'/|v|^2.
             * If x is a column vector of the minor, then
             * Hx = (I-2vv'/|v|^2)x = x-2vv'x/|v|^2 = x - 2<x,v>/|v|^2 v.
             * Therefore the transformation is easily calculated by
             * subtracting the column vector (2<x,v>/|v|^2)v from x.
             *
             * Let 2<x,v>/|v|^2 = alpha. From above we have
             * |v|^2 = -2a*(qr[minor][minor]), so
             * alpha = -<x,v>/(a*qr[minor][minor])
             */
            for (int col = minor+1; col < matrix.length; col++) {
                final T[] qrtCol = matrix[col];
                T alpha = zero;
                for (int row = minor; row < qrtCol.length; row++) {
                    alpha = alpha.subtract(qrtCol[row].multiply(qrtMinor[row]));
                }
                alpha = alpha.divide(a.multiply(qrtMinor[minor]));

                // Subtract the column vector alpha*v from x.
                for (int row = minor; row < qrtCol.length; row++) {
                    qrtCol[row] = qrtCol[row].subtract(alpha.multiply(qrtMinor[row]));
                }
            }
        }
    }


    /**
     * Returns the matrix R of the decomposition.
     * <p>R is an upper-triangular matrix</p>
     * @return the R matrix
     */
    public FieldMatrix<T> getR() {

        if (cachedR == null) {

            // R is supposed to be m x n
            final int n = qrt.length;
            final int m = qrt[0].length;
            T[][] ra = MathArrays.buildArray(threshold.getField(), m, n);
            // copy the diagonal from rDiag and the upper triangle of qr
            for (int row = FastMath.min(m, n) - 1; row >= 0; row--) {
                ra[row][row] = rDiag[row];
                for (int col = row + 1; col < n; col++) {
                    ra[row][col] = qrt[col][row];
                }
            }
            cachedR = MatrixUtils.createFieldMatrix(ra);
        }

        // return the cached matrix
        return cachedR;
    }

    /**
     * Returns the matrix Q of the decomposition.
     * <p>Q is an orthogonal matrix</p>
     * @return the Q matrix
     */
    public FieldMatrix<T> getQ() {
        if (cachedQ == null) {
            cachedQ = getQT().transpose();
        }
        return cachedQ;
    }

    /**
     * Returns the transpose of the matrix Q of the decomposition.
     * <p>Q is an orthogonal matrix</p>
     * @return the transpose of the Q matrix, Q<sup>T</sup>
     */
    public FieldMatrix<T> getQT() {
        if (cachedQT == null) {

            // QT is supposed to be m x m
            final int n = qrt.length;
            final int m = qrt[0].length;
            T[][] qta = MathArrays.buildArray(threshold.getField(), m, m);

            /*
             * Q = Q1 Q2 ... Q_m, so Q is formed by first constructing Q_m and then
             * applying the Householder transformations Q_(m-1),Q_(m-2),...,Q1 in
             * succession to the result
             */
            for (int minor = m - 1; minor >= FastMath.min(m, n); minor--) {
                qta[minor][minor] = threshold.getField().getOne();
            }

            for (int minor = FastMath.min(m, n)-1; minor >= 0; minor--){
                final T[] qrtMinor = qrt[minor];
                qta[minor][minor] = threshold.getField().getOne();
                if (!qrtMinor[minor].isZero()) {
                    for (int col = minor; col < m; col++) {
                        T alpha = threshold.getField().getZero();
                        for (int row = minor; row < m; row++) {
                            alpha = alpha.subtract(qta[col][row].multiply(qrtMinor[row]));
                        }
                        alpha = alpha.divide(rDiag[minor].multiply(qrtMinor[minor]));

                        for (int row = minor; row < m; row++) {
                            qta[col][row] = qta[col][row].add(alpha.negate().multiply(qrtMinor[row]));
                        }
                    }
                }
            }
            cachedQT = MatrixUtils.createFieldMatrix(qta);
        }

        // return the cached matrix
        return cachedQT;
    }

    /**
     * Returns the Householder reflector vectors.
     * <p>H is a lower trapezoidal matrix whose columns represent
     * each successive Householder reflector vector. This matrix is used
     * to compute Q.</p>
     * @return a matrix containing the Householder reflector vectors
     */
    public FieldMatrix<T> getH() {
        if (cachedH == null) {

            final int n = qrt.length;
            final int m = qrt[0].length;
            T[][] ha = MathArrays.buildArray(threshold.getField(), m, n);
            for (int i = 0; i < m; ++i) {
                for (int j = 0; j < FastMath.min(i + 1, n); ++j) {
                    ha[i][j] = qrt[j][i].divide(rDiag[j].negate());
                }
            }
            cachedH = MatrixUtils.createFieldMatrix(ha);
        }

        // return the cached matrix
        return cachedH;
    }

    /**
     * Get a solver for finding the A &times; X = B solution in least square sense.
     * <p>
     * Least Square sense means a solver can be computed for an overdetermined system,
     * (i.e. a system with more equations than unknowns, which corresponds to a tall A
     * matrix with more rows than columns). In any case, if the matrix is singular
     * within the tolerance set at {@link #FieldQRDecomposition(FieldMatrix,
     * CalculusFieldElement) construction}, an error will be triggered when
     * the {@link DecompositionSolver#solve(RealVector) solve} method will be called.
     * </p>
     * @return a solver
     */
    public FieldDecompositionSolver<T> getSolver() {
        return new FieldSolver();
    }

    /**
     * Specialized solver.
     */
    private class FieldSolver implements FieldDecompositionSolver<T>{

        /** {@inheritDoc} */
        @Override
        public boolean isNonSingular() {
            return !checkSingular(rDiag, threshold, false);
        }

        /** {@inheritDoc} */
        @Override
        public FieldVector<T> solve(FieldVector<T> b) {
            final int n = qrt.length;
            final int m = qrt[0].length;
            if (b.getDimension() != m) {
                throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
                                                       b.getDimension(), m);
            }
            checkSingular(rDiag, threshold, true);

            final T[] x =MathArrays.buildArray(threshold.getField(),n);
            final T[] y = b.toArray();

            // apply Householder transforms to solve Q.y = b
            for (int minor = 0; minor < FastMath.min(m, n); minor++) {

                final T[] qrtMinor = qrt[minor];
                T dotProduct = threshold.getField().getZero();
                for (int row = minor; row < m; row++) {
                    dotProduct = dotProduct.add(y[row].multiply(qrtMinor[row]));
                }
                dotProduct =  dotProduct.divide(rDiag[minor].multiply(qrtMinor[minor]));

                for (int row = minor; row < m; row++) {
                    y[row] = y[row].add(dotProduct.multiply(qrtMinor[row]));
                }
            }

            // solve triangular system R.x = y
            for (int row = rDiag.length - 1; row >= 0; --row) {
                y[row] = y[row].divide(rDiag[row]);
                final T yRow = y[row];
                final T[] qrtRow = qrt[row];
                x[row] = yRow;
                for (int i = 0; i < row; i++) {
                    y[i] = y[i].subtract(yRow.multiply(qrtRow[i]));
                }
            }

            return new ArrayFieldVector<>(x, false);
        }

        /** {@inheritDoc} */
        @Override
        public FieldMatrix<T> solve(FieldMatrix<T> b) {
            final int n = qrt.length;
            final int m = qrt[0].length;
            if (b.getRowDimension() != m) {
                throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
                                                       b.getRowDimension(), m);
            }
            checkSingular(rDiag, threshold, true);

            final int columns        = b.getColumnDimension();
            final int blockSize      = BlockFieldMatrix.BLOCK_SIZE;
            final int cBlocks        = (columns + blockSize - 1) / blockSize;
            final T[][] xBlocks = BlockFieldMatrix.createBlocksLayout(threshold.getField(),n, columns);
            final T[][] y       = MathArrays.buildArray(threshold.getField(), b.getRowDimension(), blockSize);
            final T[]   alpha   = MathArrays.buildArray(threshold.getField(), blockSize);

            for (int kBlock = 0; kBlock < cBlocks; ++kBlock) {
                final int kStart = kBlock * blockSize;
                final int kEnd   = FastMath.min(kStart + blockSize, columns);
                final int kWidth = kEnd - kStart;

                // get the right hand side vector
                b.copySubMatrix(0, m - 1, kStart, kEnd - 1, y);

                // apply Householder transforms to solve Q.y = b
                for (int minor = 0; minor < FastMath.min(m, n); minor++) {
                    final T[] qrtMinor = qrt[minor];
                    final T factor     = rDiag[minor].multiply(qrtMinor[minor]).reciprocal();

                    Arrays.fill(alpha, 0, kWidth, threshold.getField().getZero());
                    for (int row = minor; row < m; ++row) {
                        final T   d    = qrtMinor[row];
                        final T[] yRow = y[row];
                        for (int k = 0; k < kWidth; ++k) {
                            alpha[k] = alpha[k].add(d.multiply(yRow[k]));
                        }
                    }

                    for (int k = 0; k < kWidth; ++k) {
                        alpha[k] = alpha[k].multiply(factor);
                    }

                    for (int row = minor; row < m; ++row) {
                        final T   d    = qrtMinor[row];
                        final T[] yRow = y[row];
                        for (int k = 0; k < kWidth; ++k) {
                            yRow[k] = yRow[k].add(alpha[k].multiply(d));
                        }
                    }
                }

                // solve triangular system R.x = y
                for (int j = rDiag.length - 1; j >= 0; --j) {
                    final int      jBlock = j / blockSize;
                    final int      jStart = jBlock * blockSize;
                    final T   factor = rDiag[j].reciprocal();
                    final T[] yJ     = y[j];
                    final T[] xBlock = xBlocks[jBlock * cBlocks + kBlock];
                    int index = (j - jStart) * kWidth;
                    for (int k = 0; k < kWidth; ++k) {
                        yJ[k]           =yJ[k].multiply(factor);
                        xBlock[index++] = yJ[k];
                    }

                    final T[] qrtJ = qrt[j];
                    for (int i = 0; i < j; ++i) {
                        final T rIJ  = qrtJ[i];
                        final T[] yI = y[i];
                        for (int k = 0; k < kWidth; ++k) {
                            yI[k] = yI[k].subtract(yJ[k].multiply(rIJ));
                        }
                    }
                }
            }

            return new BlockFieldMatrix<>(n, columns, xBlocks, false);
        }

        /**
         * {@inheritDoc}
         * @throws MathIllegalArgumentException if the decomposed matrix is singular.
         */
        @Override
        public FieldMatrix<T> getInverse() {
            return solve(MatrixUtils.createFieldIdentityMatrix(threshold.getField(), qrt[0].length));
        }

        /**
         * Check singularity.
         *
         * @param diag Diagonal elements of the R matrix.
         * @param min Singularity threshold.
         * @param raise Whether to raise a {@link MathIllegalArgumentException}
         * if any element of the diagonal fails the check.
         * @return {@code true} if any element of the diagonal is smaller
         * or equal to {@code min}.
         * @throws MathIllegalArgumentException if the matrix is singular and
         * {@code raise} is {@code true}.
         */
        private boolean checkSingular(T[] diag,
                                             T min,
                                             boolean raise) {
            for (final T d : diag) {
                if (FastMath.abs(d.getReal()) <= min.getReal()) {
                    if (raise) {
                        throw new MathIllegalArgumentException(LocalizedCoreFormats.SINGULAR_MATRIX);
                    } else {
                        return true;
                    }
                }
            }
            return false;
        }

        /** {@inheritDoc} */
        @Override
        public int getRowDimension() {
            return qrt[0].length;
        }

        /** {@inheritDoc} */
        @Override
        public int getColumnDimension() {
            return qrt.length;
        }

    }
}