SequentialGaussNewtonOptimizer

/*
 * Licensed to the Hipparchus project under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The Hipparchus project 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.
 */
package org.hipparchus.optim.nonlinear.vector.leastsquares;

import org.hipparchus.exception.LocalizedCoreFormats;
import org.hipparchus.exception.MathIllegalArgumentException;
import org.hipparchus.exception.MathIllegalStateException;
import org.hipparchus.exception.NullArgumentException;
import org.hipparchus.linear.ArrayRealVector;
import org.hipparchus.linear.CholeskyDecomposition;
import org.hipparchus.linear.MatrixDecomposer;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.QRDecomposer;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;
import org.hipparchus.optim.ConvergenceChecker;
import org.hipparchus.optim.LocalizedOptimFormats;
import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem.Evaluation;
import org.hipparchus.util.Incrementor;
import org.hipparchus.util.Pair;

/**
 * Sequential Gauss-Newton least-squares solver.
 * <p>
 * This class solve a least-square problem by solving the normal equations of
 * the linearized problem at each iteration.
 * </p>
 *
 */
public class SequentialGaussNewtonOptimizer implements LeastSquaresOptimizer {

    /**
     * The singularity threshold for matrix decompositions. Determines when a
     * {@link MathIllegalStateException} is thrown. The current value was the
     * default value for {@link org.hipparchus.linear.LUDecomposition}.
     */
    private static final double SINGULARITY_THRESHOLD = 1e-11;

    /** Decomposer. */
    private final MatrixDecomposer decomposer;

    /** Indicates if normal equations should be formed explicitly. */
    private final boolean formNormalEquations;

    /** Old evaluation previously computed. */
    private final Evaluation oldEvaluation;

    /** Old jacobian previously computed. */
    private final RealMatrix oldLhs;

    /** Old residuals previously computed. */
    private final RealVector oldRhs;

    /**
     * Create a sequential Gauss Newton optimizer.
     * <p>
     * The default for the algorithm is to use QR decomposition, not
     * form normal equations and have no previous evaluation
     * </p>
     *
     */
    public SequentialGaussNewtonOptimizer() {
        this(new QRDecomposer(SINGULARITY_THRESHOLD), false, null);
    }

    /**
     * Create a sequential Gauss Newton optimizer that uses the given matrix
     * decomposition algorithm to solve the normal equations.
     * <p>
     * The {@code decomposer} is used to solve J<sup>T</sup>Jx=J<sup>T</sup>r.
     * </p>
     *
     * @param decomposer the decomposition algorithm to use.
     * @param formNormalEquations whether the normal equations should be explicitly
     *                            formed. If {@code true} then {@code decomposer} is used
     *                            to solve J<sup>T</sup>Jx=J<sup>T</sup>r, otherwise
     *                            {@code decomposer} is used to solve Jx=r. If {@code
     *                            decomposer} can only solve square systems then this
     *                            parameter should be {@code true}.
     * @param evaluation old evaluation previously computed, null if there are no previous evaluations.
     */
    public SequentialGaussNewtonOptimizer(final MatrixDecomposer decomposer,
                                          final boolean formNormalEquations,
                                          final Evaluation evaluation) {
        this.decomposer          = decomposer;
        this.formNormalEquations = formNormalEquations;
        this.oldEvaluation       = evaluation;
        if (evaluation == null) {
            this.oldLhs = null;
            this.oldRhs = null;
        } else {
            if (formNormalEquations) {
                final Pair<RealMatrix, RealVector> normalEquation =
                                computeNormalMatrix(evaluation.getJacobian(), evaluation.getResiduals());
                // solve the linearized least squares problem
                this.oldLhs = normalEquation.getFirst();
                this.oldRhs = normalEquation.getSecond();
            } else {
                this.oldLhs = evaluation.getJacobian();
                this.oldRhs = evaluation.getResiduals();
            }
        }
    }

    /**
     * Get the matrix decomposition algorithm.
     *
     * @return the decomposition algorithm.
     */
    public MatrixDecomposer getDecomposer() {
        return decomposer;
    }

    /**
     * Configure the matrix decomposition algorithm.
     *
     * @param newDecomposer the decomposition algorithm to use.
     * @return a new instance.
     */
    public SequentialGaussNewtonOptimizer withDecomposer(final MatrixDecomposer newDecomposer) {
        return new SequentialGaussNewtonOptimizer(newDecomposer,
                                                  this.isFormNormalEquations(),
                                                  this.getOldEvaluation());
    }

    /**
     * Get if the normal equations are explicitly formed.
     *
     * @return if the normal equations should be explicitly formed. If {@code true} then
     * {@code decomposer} is used to solve J<sup>T</sup>Jx=J<sup>T</sup>r, otherwise
     * {@code decomposer} is used to solve Jx=r.
     */
    public boolean isFormNormalEquations() {
        return formNormalEquations;
    }

    /**
     * Configure if the normal equations should be explicitly formed.
     *
     * @param newFormNormalEquations whether the normal equations should be explicitly
     *                               formed. If {@code true} then {@code decomposer} is used
     *                               to solve J<sup>T</sup>Jx=J<sup>T</sup>r, otherwise
     *                               {@code decomposer} is used to solve Jx=r. If {@code
     *                               decomposer} can only solve square systems then this
     *                               parameter should be {@code true}.
     * @return a new instance.
     */
    public SequentialGaussNewtonOptimizer withFormNormalEquations(final boolean newFormNormalEquations) {
        return new SequentialGaussNewtonOptimizer(this.getDecomposer(),
                                                  newFormNormalEquations,
                                                  this.getOldEvaluation());
    }

    /**
     * Get the previous evaluation used by the optimizer.
     *
     * @return the previous evaluation.
     */
    public Evaluation getOldEvaluation() {
        return oldEvaluation;
    }

    /**
     * Configure the previous evaluation used by the optimizer.
     * <p>
     * This building method uses a complete evaluation to retrieve
     * a priori data. Note that as {@link #withAPrioriData(RealVector, RealMatrix)}
     * generates a fake evaluation and calls this method, either
     * {@link #withAPrioriData(RealVector, RealMatrix)} or {@link #withEvaluation(LeastSquaresProblem.Evaluation)}
     * should be called, but not both as the last one called will override the previous one.
     * </p>
     * @param previousEvaluation the previous evaluation used by the optimizer.
     * @return a new instance.
     */
    public SequentialGaussNewtonOptimizer withEvaluation(final Evaluation previousEvaluation) {
        return new SequentialGaussNewtonOptimizer(this.getDecomposer(),
                                                  this.isFormNormalEquations(),
                                                  previousEvaluation);
    }

    /**
     * Configure from a priori state and covariance.
     * <p>
     * This building method generates a fake evaluation and calls
     * {@link #withEvaluation(LeastSquaresProblem.Evaluation)}, so either
     * {@link #withAPrioriData(RealVector, RealMatrix)} or {@link #withEvaluation(LeastSquaresProblem.Evaluation)}
     * should be called, but not both as the last one called will override the previous one.
     * <p>
     * A Cholesky decomposition is used to compute the weighted jacobian from the
     * a priori covariance. This method uses the default thresholds of the decomposition.
     * </p>
     * @param aPrioriState a priori state to use
     * @param aPrioriCovariance a priori covariance to use
     * @return a new instance.
     * @see #withAPrioriData(RealVector, RealMatrix, double, double)
     */
    public SequentialGaussNewtonOptimizer withAPrioriData(final RealVector aPrioriState,
                                                          final RealMatrix aPrioriCovariance) {
        return withAPrioriData(aPrioriState, aPrioriCovariance,
                               CholeskyDecomposition.DEFAULT_RELATIVE_SYMMETRY_THRESHOLD,
                               CholeskyDecomposition.DEFAULT_ABSOLUTE_POSITIVITY_THRESHOLD);
    }

    /**
     * Configure from a priori state and covariance.
     * <p>
     * This building method generates a fake evaluation and calls
     * {@link #withEvaluation(LeastSquaresProblem.Evaluation)}, so either
     * {@link #withAPrioriData(RealVector, RealMatrix)} or {@link #withEvaluation(LeastSquaresProblem.Evaluation)}
     * should be called, but not both as the last one called will override the previous one.
     * <p>
     * A Cholesky decomposition is used to compute the weighted jacobian from the
     * a priori covariance.
     * </p>
     * @param aPrioriState a priori state to use
     * @param aPrioriCovariance a priori covariance to use
     * @param relativeSymmetryThreshold Cholesky decomposition threshold above which off-diagonal
     * elements are considered too different and matrix not symmetric
     * @param absolutePositivityThreshold Cholesky decomposition threshold below which diagonal
     * elements are considered null and matrix not positive definite
     * @return a new instance.
     * @since 2.3
     */
    public SequentialGaussNewtonOptimizer withAPrioriData(final RealVector aPrioriState,
                                                          final RealMatrix aPrioriCovariance,
                                                          final double relativeSymmetryThreshold,
                                                          final double absolutePositivityThreshold) {

        // we consider the a priori state and covariance come from a
        // previous estimation with exactly one observation of each state
        // component, so partials are the identity matrix, weight is the
        // square root of inverse of covariance, and residuals are zero

        // create a fake weighted Jacobian
        final RealMatrix jTj              = getDecomposer().decompose(aPrioriCovariance).getInverse();
        final RealMatrix weightedJacobian = new CholeskyDecomposition(jTj,
                                                                      relativeSymmetryThreshold,
                                                                      absolutePositivityThreshold).getLT();

        // create fake zero residuals
        final RealVector residuals        = MatrixUtils.createRealVector(aPrioriState.getDimension());

        // combine everything as an evaluation
        final Evaluation fakeEvaluation   = new AbstractEvaluation(aPrioriState.getDimension()) {

            /** {@inheritDoc} */
            @Override
            public RealVector getResiduals() {
                return residuals;
            }

            /** {@inheritDoc} */
            @Override
            public RealVector getPoint() {
                return aPrioriState;
            }

            /** {@inheritDoc} */
            @Override
            public RealMatrix getJacobian() {
                return weightedJacobian;
            }
        };

        return withEvaluation(fakeEvaluation);

    }

    /** {@inheritDoc} */
    @Override
    public Optimum optimize(final LeastSquaresProblem lsp) {
        // create local evaluation and iteration counts
        final Incrementor evaluationCounter = lsp.getEvaluationCounter();
        final Incrementor iterationCounter = lsp.getIterationCounter();
        final ConvergenceChecker<Evaluation> checker =
            lsp.getConvergenceChecker();

        // Computation will be useless without a checker (see "for-loop").
        if (checker == null) {
            throw new NullArgumentException();
        }

        RealVector currentPoint = lsp.getStart();

        if (oldEvaluation != null &&
            currentPoint.getDimension() != oldEvaluation.getPoint().getDimension()) {
            throw new MathIllegalStateException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
                      currentPoint.getDimension(), oldEvaluation.getPoint().getDimension());
        }

        // iterate until convergence is reached
        Evaluation current = null;
        while (true) {
            iterationCounter.increment();

            // evaluate the objective function and its jacobian
            final Evaluation previous = current;

            // Value of the objective function at "currentPoint".
            evaluationCounter.increment();
            current = lsp.evaluate(currentPoint);
            final RealVector currentResiduals = current.getResiduals();
            final RealMatrix weightedJacobian = current.getJacobian();

            currentPoint = current.getPoint();

            // Check convergence.
            if (previous != null &&
                checker.converged(iterationCounter.getCount(), previous,
                                  current)) {
                // combine old and new evaluations
                final Evaluation combinedEvaluation = oldEvaluation == null ?
                                                      current :
                                                      new CombinedEvaluation(oldEvaluation, current);
                return Optimum.of(combinedEvaluation, evaluationCounter.getCount(),
                                  iterationCounter.getCount());
            }

           // solve the linearized least squares problem
            final RealMatrix lhs; // left hand side
            final RealVector rhs; // right hand side
            if (this.formNormalEquations) {
                final Pair<RealMatrix, RealVector> normalEquation =
                                computeNormalMatrix(weightedJacobian, currentResiduals);

                lhs = oldLhs == null ?
                      normalEquation.getFirst() :
                      normalEquation.getFirst().add(oldLhs); // left hand side
                rhs = oldRhs == null ?
                      normalEquation.getSecond() :
                      normalEquation.getSecond().add(oldRhs); // right hand side
            } else {
                lhs = oldLhs == null ?
                      weightedJacobian :
                      combineJacobians(oldLhs, weightedJacobian);
                rhs = oldRhs == null ?
                      currentResiduals :
                      combineResiduals(oldRhs, currentResiduals);
            }

            final RealVector dX;
            try {
                dX = this.decomposer.decompose(lhs).solve(rhs);
            } catch (MathIllegalArgumentException e) {
                // change exception message
                throw new MathIllegalStateException(LocalizedOptimFormats.UNABLE_TO_SOLVE_SINGULAR_PROBLEM,
                                                    e);
            }
            // update the estimated parameters
            currentPoint = currentPoint.add(dX);

        }
    }

    /** {@inheritDoc} */
    @Override
    public String toString() {
        return "SequentialGaussNewtonOptimizer{" +
               "decomposer=" + decomposer + '}';
    }

    /**
     * Compute the normal matrix, J<sup>T</sup>J.
     *
     * @param jacobian  the m by n jacobian matrix, J. Input.
     * @param residuals the m by 1 residual vector, r. Input.
     * @return  the n by n normal matrix and the n by 1 J<sup>Tr</sup> vector.
     */
    private static Pair<RealMatrix, RealVector>
        computeNormalMatrix(final RealMatrix jacobian,
                            final RealVector residuals) {
        // since the normal matrix is symmetric, we only need to compute half of
        // it.
        final int nR = jacobian.getRowDimension();
        final int nC = jacobian.getColumnDimension();
        // allocate space for return values
        final RealMatrix normal = MatrixUtils.createRealMatrix(nC, nC);
        final RealVector jTr = new ArrayRealVector(nC);
        // for each measurement
        for (int i = 0; i < nR; ++i) {
            // compute JTr for measurement i
            for (int j = 0; j < nC; j++) {
                jTr.setEntry(j,
                             jTr.getEntry(j) +
                                residuals.getEntry(i) *
                                               jacobian.getEntry(i, j));
            }

            // add the the contribution to the normal matrix for measurement i
            for (int k = 0; k < nC; ++k) {
                // only compute the upper triangular part
                for (int l = k; l < nC; ++l) {
                    normal
                        .setEntry(k, l,
                                  normal.getEntry(k,
                                                  l) +
                                        jacobian.getEntry(i, k) *
                                                       jacobian.getEntry(i, l));
                }
            }
        }
        // copy the upper triangular part to the lower triangular part.
        for (int i = 0; i < nC; i++) {
            for (int j = 0; j < i; j++) {
                normal.setEntry(i, j, normal.getEntry(j, i));
            }
        }
        return new Pair<>(normal, jTr);
    }

    /** Combine Jacobian matrices
     * @param oldJacobian old Jacobian matrix
     * @param newJacobian new Jacobian matrix
     * @return combined Jacobian matrix
     */
    private static RealMatrix combineJacobians(final RealMatrix oldJacobian,
                                               final RealMatrix newJacobian) {
        final int oldRowDimension    = oldJacobian.getRowDimension();
        final int oldColumnDimension = oldJacobian.getColumnDimension();
        final RealMatrix jacobian =
                        MatrixUtils.createRealMatrix(oldRowDimension + newJacobian.getRowDimension(),
                                                     oldColumnDimension);
        jacobian.setSubMatrix(oldJacobian.getData(), 0,               0);
        jacobian.setSubMatrix(newJacobian.getData(), oldRowDimension, 0);
        return jacobian;
    }

    /** Combine residuals vectors
     * @param oldResiduals old residuals vector
     * @param newResiduals new residuals vector
     * @return combined residuals vector
     */
    private static RealVector combineResiduals(final RealVector oldResiduals,
                                               final RealVector newResiduals) {
        return oldResiduals.append(newResiduals);
    }

    /**
     * Container with an old and a new evaluation and combine both of them
     */
    private static class CombinedEvaluation extends AbstractEvaluation {

        /** Point of evaluation. */
        private final RealVector point;

        /** Derivative at point. */
        private final RealMatrix jacobian;

        /** Computed residuals. */
        private final RealVector residuals;

        /**
         * Create an {@link Evaluation} with no weights.
         *
         * @param oldEvaluation the old evaluation.
         * @param newEvaluation the new evaluation
         */
        private CombinedEvaluation(final Evaluation oldEvaluation,
                                   final Evaluation newEvaluation) {

            super(oldEvaluation.getResiduals().getDimension() +
                  newEvaluation.getResiduals().getDimension());

            this.point    = newEvaluation.getPoint();
            this.jacobian = combineJacobians(oldEvaluation.getJacobian(),
                                             newEvaluation.getJacobian());
            this.residuals = combineResiduals(oldEvaluation.getResiduals(),
                                              newEvaluation.getResiduals());
        }

        /** {@inheritDoc} */
        @Override
        public RealMatrix getJacobian() {
            return jacobian;
        }

        /** {@inheritDoc} */
        @Override
        public RealVector getPoint() {
            return point;
        }

        /** {@inheritDoc} */
        @Override
        public RealVector getResiduals() {
            return residuals;
        }

    }

}