org.apache.commons.math3.linear.MatrixUtils Java Examples

/**
 * Get the covariance matrix.
 * @return covariance matrix
 */
public RealMatrix getResult() {

    int dimension = sums.length;
    RealMatrix result = MatrixUtils.createRealMatrix(dimension, dimension);

    if (n > 1) {
        double c = 1.0 / (n * (isBiasCorrected ? (n - 1) : n));
        int k = 0;
        for (int i = 0; i < dimension; ++i) {
            for (int j = 0; j <= i; ++j) {
                double e = c * (n * productsSums[k++] - sums[i] * sums[j]);
                result.setEntry(i, j, e);
                result.setEntry(j, i, e);
            }
        }
    }

    return result;

}
 

/**
 * Calculate inverse matrix
 *
 * @param a The matrix
 * @return Inverse matrix array
 */
public static Array inv(Array a) {
    double[][] aa = (double[][]) ArrayUtil.copyToNDJavaArray_Double(a);
    RealMatrix matrix = new Array2DRowRealMatrix(aa, false);
    RealMatrix invm = MatrixUtils.inverse(matrix);
    if (invm == null) {
        return null;
    }

    int m = invm.getRowDimension();
    int n = invm.getColumnDimension();
    Array r = Array.factory(DataType.DOUBLE, new int[]{m, n});
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < n; j++) {
            r.setDouble(i * n + j, invm.getEntry(i, j));
        }
    }

    return r;
}
 

public MultivariateTDistribution(RealVector mean, RealMatrix covarianceMatrix, double degreesOfFreedom) {
    this.mean = mean;
    if (mean.getDimension() > 1) {
        this.precisionMatrix = MatrixUtils.blockInverse(covarianceMatrix, (-1 + covarianceMatrix.getColumnDimension()) / 2);
    } else {
        this.precisionMatrix = MatrixUtils.createRealIdentityMatrix(1).scalarMultiply(1. / covarianceMatrix.getEntry(0, 0));
    }
    this.dof = degreesOfFreedom;

    this.D = mean.getDimension();

    double determinant = new LUDecomposition(covarianceMatrix).getDeterminant();

    this.multiplier = Math.exp(Gamma.logGamma(0.5 * (D + dof)) - Gamma.logGamma(0.5 * dof)) /
            Math.pow(Math.PI * dof, 0.5 * D) /
            Math.pow(determinant, 0.5);
}
 

/**
 * Verifies that GLS with identity covariance matrix gives the same results
 * as OLS.
 */
@Test
public void testGLSOLSConsistency() {      
    RealMatrix identityCov = MatrixUtils.createRealIdentityMatrix(16);
    GLSMultipleLinearRegression glsModel = new GLSMultipleLinearRegression();
    OLSMultipleLinearRegression olsModel = new OLSMultipleLinearRegression();
    glsModel.newSampleData(longley, 16, 6);
    olsModel.newSampleData(longley, 16, 6);
    glsModel.newCovarianceData(identityCov.getData());
    double[] olsBeta = olsModel.calculateBeta().toArray();
    double[] glsBeta = glsModel.calculateBeta().toArray();
    // TODO:  Should have assertRelativelyEquals(double[], double[], eps) in TestUtils
    //        Should also add RealVector and RealMatrix versions
    for (int i = 0; i < olsBeta.length; i++) {
        TestUtils.assertRelativelyEquals(olsBeta[i], glsBeta[i], 10E-7);
    }
}
 

/**
 * Computes the Jacobian matrix.
 *
 * @param params Model parameters at which to compute the Jacobian.
 * @return the weighted Jacobian: W<sup>1/2</sup> J.
 * @throws DimensionMismatchException if the Jacobian dimension does not
 * match problem dimension.
 * @since 3.1
 */
protected RealMatrix computeWeightedJacobian(double[] params) {
    ++jacobianEvaluations;

    final DerivativeStructure[] dsPoint = new DerivativeStructure[params.length];
    final int nC = params.length;
    for (int i = 0; i < nC; ++i) {
        dsPoint[i] = new DerivativeStructure(nC, 1, i, params[i]);
    }
    final DerivativeStructure[] dsValue = jF.value(dsPoint);
    final int nR = getTarget().length;
    if (dsValue.length != nR) {
        throw new DimensionMismatchException(dsValue.length, nR);
    }
    final double[][] jacobianData = new double[nR][nC];
    for (int i = 0; i < nR; ++i) {
        int[] orders = new int[nC];
        for (int j = 0; j < nC; ++j) {
            orders[j] = 1;
            jacobianData[i][j] = dsValue[i].getPartialDerivative(orders);
            orders[j] = 0;
        }
    }

    return weightMatrixSqrt.multiply(MatrixUtils.createRealMatrix(jacobianData));
}
 

/**
 * Get the covariance matrix of the optimized parameters.
 * <br/>
 * Note that this operation involves the inversion of the
 * <code>J<sup>T</sup>J</code> matrix, where {@code J} is the
 * Jacobian matrix.
 * The {@code threshold} parameter is a way for the caller to specify
 * that the result of this computation should be considered meaningless,
 * and thus trigger an exception.
 *
 * @param threshold Singularity threshold.
 * @return the covariance matrix.
 * @throws org.apache.commons.math3.linear.SingularMatrixException
 * if the covariance matrix cannot be computed (singular problem).
 */
public double[][] getCovariances(double threshold) {
    // Set up the jacobian.
    updateJacobian();

    // Compute transpose(J)J, without building intermediate matrices.
    double[][] jTj = new double[cols][cols];
    for (int i = 0; i < cols; ++i) {
        for (int j = i; j < cols; ++j) {
            double sum = 0;
            for (int k = 0; k < rows; ++k) {
                sum += weightedResidualJacobian[k][i] * weightedResidualJacobian[k][j];
            }
            jTj[i][j] = sum;
            jTj[j][i] = sum;
        }
    }

    // Compute the covariances matrix.
    final DecompositionSolver solver
        = new QRDecomposition(MatrixUtils.createRealMatrix(jTj), threshold).getSolver();
    return solver.getInverse().getData();
}
 

private void assertSVDValues(final File outputFileV, final File outputFileS, final File outputFileU) {
    try {
        final ReadCountCollection rcc = ReadCountCollectionUtils.parse(CONTROL_PCOV_FULL_FILE);
        final SVD svd = SVDFactory.createSVD(rcc.counts());
        final RealMatrix sDiag = new DiagonalMatrix(svd.getSingularValues());
        assertOutputFileValues(outputFileU, svd.getU());
        assertOutputFileValues(outputFileS, sDiag);
        assertOutputFileValues(outputFileV, svd.getV());
        assertUnitaryMatrix(svd.getV());
        assertUnitaryMatrix(svd.getU());
        Assert.assertTrue(MatrixUtils.isSymmetric(sDiag, 1e-32));

    } catch (final IOException ioe) {
        Assert.fail("Could not open test file: " + CONTROL_PCOV_FULL_FILE, ioe);
    }
}
 

/**
 * Get the covariance matrix.
 * @return covariance matrix
 */
public RealMatrix getResult() {

    int dimension = sums.length;
    RealMatrix result = MatrixUtils.createRealMatrix(dimension, dimension);

    if (n > 1) {
        double c = 1.0 / (n * (isBiasCorrected ? (n - 1) : n));
        int k = 0;
        for (int i = 0; i < dimension; ++i) {
            for (int j = 0; j <= i; ++j) {
                double e = c * (n * productsSums[k++] - sums[i] * sums[j]);
                result.setEntry(i, j, e);
                result.setEntry(j, i, e);
            }
        }
    }

    return result;

}
 

/**
 * Get the covariance matrix.
 * @return covariance matrix
 */
public RealMatrix getResult() {

    int dimension = sums.length;
    RealMatrix result = MatrixUtils.createRealMatrix(dimension, dimension);

    if (n > 1) {
        double c = 1.0 / (n * (isBiasCorrected ? (n - 1) : n));
        int k = 0;
        for (int i = 0; i < dimension; ++i) {
            for (int j = 0; j <= i; ++j) {
                double e = c * (n * productsSums[k++] - sums[i] * sums[j]);
                result.setEntry(i, j, e);
                result.setEntry(j, i, e);
            }
        }
    }

    return result;

}
 

@Test
public void testMath226() {
    double[] mean = { 1, 1, 10, 1 };
    double[][] cov = {
            { 1, 3, 2, 6 },
            { 3, 13, 16, 2 },
            { 2, 16, 38, -1 },
            { 6, 2, -1, 197 }
    };
    RealMatrix covRM = MatrixUtils.createRealMatrix(cov);
    JDKRandomGenerator jg = new JDKRandomGenerator();
    jg.setSeed(5322145245211l);
    NormalizedRandomGenerator rg = new GaussianRandomGenerator(jg);
    CorrelatedRandomVectorGenerator sg =
        new CorrelatedRandomVectorGenerator(mean, covRM, 0.00001, rg);

    double[] min = new double[mean.length];
    Arrays.fill(min, Double.POSITIVE_INFINITY);
    double[] max = new double[mean.length];
    Arrays.fill(max, Double.NEGATIVE_INFINITY);
    for (int i = 0; i < 10; i++) {
        double[] generated = sg.nextVector();
        for (int j = 0; j < generated.length; ++j) {
            min[j] = FastMath.min(min[j], generated[j]);
            max[j] = FastMath.max(max[j], generated[j]);
        }
    }
    for (int j = 0; j < min.length; ++j) {
        Assert.assertTrue(max[j] - min[j] > 2.0);
    }

}
 

/**
 * Calculates the parameters of a bivariate quadratic equation.
 * 
 * @param elevationValues the window of points to use.
 * @return the parameters of the bivariate quadratic equation as [a, b, c, d, e, f]
 */
private static double[] calculateParameters( final double[][] elevationValues ) {
    int rows = elevationValues.length;
    int cols = elevationValues[0].length;
    int pointsNum = rows * cols;

    final double[][] xyMatrix = new double[pointsNum][6];
    final double[] valueArray = new double[pointsNum];

    // TODO check on resolution
    int index = 0;
    for( int y = 0; y < rows; y++ ) {
        for( int x = 0; x < cols; x++ ) {
            xyMatrix[index][0] = x * x; // x^2
            xyMatrix[index][1] = y * y; // y^2
            xyMatrix[index][2] = x * y; // xy
            xyMatrix[index][3] = x; // x
            xyMatrix[index][4] = y; // y
            xyMatrix[index][5] = 1;
            valueArray[index] = elevationValues[y][x];
            index++;
        }
    }

    RealMatrix A = MatrixUtils.createRealMatrix(xyMatrix);
    RealVector z = MatrixUtils.createRealVector(valueArray);

    DecompositionSolver solver = new RRQRDecomposition(A).getSolver();
    RealVector solution = solver.solve(z);

    // start values for a, b, c, d, e, f, all set to 0.0
    final double[] parameters = solution.toArray();
    return parameters;
}
 

public CorrelatedRandomVectorGeneratorTest() {
    mean = new double[] { 0.0, 1.0, -3.0, 2.3 };

    RealMatrix b = MatrixUtils.createRealMatrix(4, 3);
    int counter = 0;
    for (int i = 0; i < b.getRowDimension(); ++i) {
        for (int j = 0; j < b.getColumnDimension(); ++j) {
            b.setEntry(i, j, 1.0 + 0.1 * ++counter);
        }
    }
    RealMatrix bbt = b.multiply(b.transpose());
    covariance = MatrixUtils.createRealMatrix(mean.length, mean.length);
    for (int i = 0; i < covariance.getRowDimension(); ++i) {
        covariance.setEntry(i, i, bbt.getEntry(i, i));
        for (int j = 0; j < covariance.getColumnDimension(); ++j) {
            double s = bbt.getEntry(i, j);
            covariance.setEntry(i, j, s);
            covariance.setEntry(j, i, s);
        }
    }

    RandomGenerator rg = new JDKRandomGenerator();
    rg.setSeed(17399225432l);
    GaussianRandomGenerator rawGenerator = new GaussianRandomGenerator(rg);
    generator = new CorrelatedRandomVectorGenerator(mean,
                                                    covariance,
                                                    1.0e-12 * covariance.getNorm(),
                                                    rawGenerator);
}
 

@Test
public void testMath226() {
    double[] mean = { 1, 1, 10, 1 };
    double[][] cov = {
            { 1, 3, 2, 6 },
            { 3, 13, 16, 2 },
            { 2, 16, 38, -1 },
            { 6, 2, -1, 197 }
    };
    RealMatrix covRM = MatrixUtils.createRealMatrix(cov);
    JDKRandomGenerator jg = new JDKRandomGenerator();
    jg.setSeed(5322145245211l);
    NormalizedRandomGenerator rg = new GaussianRandomGenerator(jg);
    CorrelatedRandomVectorGenerator sg =
        new CorrelatedRandomVectorGenerator(mean, covRM, 0.00001, rg);

    double[] min = new double[mean.length];
    Arrays.fill(min, Double.POSITIVE_INFINITY);
    double[] max = new double[mean.length];
    Arrays.fill(max, Double.NEGATIVE_INFINITY);
    for (int i = 0; i < 10; i++) {
        double[] generated = sg.nextVector();
        for (int j = 0; j < generated.length; ++j) {
            min[j] = FastMath.min(min[j], generated[j]);
            max[j] = FastMath.max(max[j], generated[j]);
        }
    }
    for (int j = 0; j < min.length; ++j) {
        Assert.assertTrue(max[j] - min[j] > 2.0);
    }

}
 

/** Simple constructor.
 * @param nSteps number of steps of the multistep method
 * (excluding the one being computed)
 */
private AdamsNordsieckTransformer(final int nSteps) {

    // compute exact coefficients
    FieldMatrix<BigFraction> bigP = buildP(nSteps);
    FieldDecompositionSolver<BigFraction> pSolver =
        new FieldLUDecomposition<BigFraction>(bigP).getSolver();

    BigFraction[] u = new BigFraction[nSteps];
    Arrays.fill(u, BigFraction.ONE);
    BigFraction[] bigC1 = pSolver
        .solve(new ArrayFieldVector<BigFraction>(u, false)).toArray();

    // update coefficients are computed by combining transform from
    // Nordsieck to multistep, then shifting rows to represent step advance
    // then applying inverse transform
    BigFraction[][] shiftedP = bigP.getData();
    for (int i = shiftedP.length - 1; i > 0; --i) {
        // shift rows
        shiftedP[i] = shiftedP[i - 1];
    }
    shiftedP[0] = new BigFraction[nSteps];
    Arrays.fill(shiftedP[0], BigFraction.ZERO);
    FieldMatrix<BigFraction> bigMSupdate =
        pSolver.solve(new Array2DRowFieldMatrix<BigFraction>(shiftedP, false));

    // convert coefficients to double
    update         = MatrixUtils.bigFractionMatrixToRealMatrix(bigMSupdate);
    c1             = new double[nSteps];
    for (int i = 0; i < nSteps; ++i) {
        c1[i] = bigC1[i].doubleValue();
    }

}
 

@Test
public void testMatricesValues() {
   BiDiagonalTransformer transformer =
        new BiDiagonalTransformer(MatrixUtils.createRealMatrix(testSquare));
   final double s17 = FastMath.sqrt(17.0);
    RealMatrix uRef = MatrixUtils.createRealMatrix(new double[][] {
            {  -8 / (5 * s17), 19 / (5 * s17) },
            { -19 / (5 * s17), -8 / (5 * s17) }
    });
    RealMatrix bRef = MatrixUtils.createRealMatrix(new double[][] {
            { -3 * s17 / 5, 32 * s17 / 85 },
            {      0.0,     -5 * s17 / 17 }
    });
    RealMatrix vRef = MatrixUtils.createRealMatrix(new double[][] {
            { 1.0,  0.0 },
            { 0.0, -1.0 }
    });

    // check values against known references
    RealMatrix u = transformer.getU();
    Assert.assertEquals(0, u.subtract(uRef).getNorm(), 1.0e-14);
    RealMatrix b = transformer.getB();
    Assert.assertEquals(0, b.subtract(bRef).getNorm(), 1.0e-14);
    RealMatrix v = transformer.getV();
    Assert.assertEquals(0, v.subtract(vRef).getNorm(), 1.0e-14);

    // check the same cached instance is returned the second time
    Assert.assertTrue(u == transformer.getU());
    Assert.assertTrue(b == transformer.getB());
    Assert.assertTrue(v == transformer.getV());

}
 

@Test
public void testInverse() {
    RealMatrix matrix = new Array2DRowRealMatrix(new double[][] {{1, 2}, {3, 4}});

    RealMatrix inverse = MatrixUtils.inverse(matrix);
    INDArray arr = InvertMatrix.invert(Nd4j.linspace(1, 4, 4).reshape(2, 2), false);
    for (int i = 0; i < inverse.getRowDimension(); i++) {
        for (int j = 0; j < inverse.getColumnDimension(); j++) {
            assertEquals(arr.getDouble(i, j), inverse.getEntry(i, j), 1e-1);
        }
    }
}
 

public void updateCoefficients(double l2penalty) {
       if (this.X_svd == null) {
       	this.X_svd = new SingularValueDecomposition(X);
       }
    RealMatrix V = this.X_svd.getV();
    double[] s = this.X_svd.getSingularValues();
    RealMatrix U = this.X_svd.getU();
    
    for (int i = 0; i < s.length; i++) {
    	s[i] = s[i] / (s[i]*s[i] + l2penalty);
    }
    RealMatrix S = MatrixUtils.createRealDiagonalMatrix(s);
    
    RealMatrix Z = V.multiply(S).multiply(U.transpose());
    
    this.coefficients = Z.operate(this.Y);
    
    this.fitted = this.X.operate(this.coefficients);
    double errorVariance = 0;
    for (int i = 0; i < residuals.length; i++) {
    	this.residuals[i] = this.Y[i] - this.fitted[i];
    	errorVariance += this.residuals[i] * this.residuals[i];
    }
    errorVariance = errorVariance / (X.getRowDimension() - X.getColumnDimension());
    
    RealMatrix errorVarianceMatrix = MatrixUtils.createRealIdentityMatrix(this.Y.length).scalarMultiply(errorVariance);
    RealMatrix coefficientsCovarianceMatrix = Z.multiply(errorVarianceMatrix).multiply(Z.transpose());
    this.standarderrors = getDiagonal(coefficientsCovarianceMatrix);
}
 

public void fit(List<Location> locations){
	setLocations(locations);
	int n = locations.size();
	double[][] K = new double[n][n];

	for(int i=0; i<n; i++){
		Location loc1 = locations.get(i);
		double[] x1 = ModelAdaptUtils.locationToVec(loc1);
		for(int j=i; j<n; j++){
			Location loc2 = locations.get(j);
			double[] x2 = ModelAdaptUtils.locationToVec(loc2);
			double k =kernel.computeKernel(x1, x2);
			K[i][j] = k;
			K[j][i] = k;
		}
	}
	RealMatrix Kmat = MatrixUtils.createRealMatrix(K);
	RealMatrix lambdamat = MatrixUtils.createRealIdentityMatrix(n).scalarMultiply(sigma_n*sigma_n); //Tentative treatment

	RealMatrix Kymat = Kmat.add(lambdamat);

	CholeskyDecomposition chol = new CholeskyDecomposition(Kymat);
	RealMatrix Lmat = chol.getL();

	double[] normalRands = new double[n];
	for(int i=0; i<n; i++){
		normalRands[i] = rand.nextGaussian();
	}
	this.y = Lmat.operate(normalRands);

	RealMatrix invKymat = (new LUDecomposition(Kymat)).getSolver().getInverse();
	this.alphas = invKymat.operate(y);
}
 

private RealVector getStateValue(ObjectNode state) {
    RealVector vec = MatrixUtils.createRealVector(new double[actionAttrs.size() + 1]);
    double cnt = state.get(modelName + "-state-cnt").asDouble();
    vec.setEntry(0, cnt);
    for (int i=0; i< actionAttrs.size(); i++) {
        vec.setEntry(i + 1, state.get("state-" + actionAttrs.get(i)).asDouble() * cnt);
    }
    return vec;
}
 

final public void requestVectorVar(String name, int size, int dim, double initial,
                                   boolean randomize, boolean normalize) {
    List<RealVector> var = new ArrayList<>(size);
    for (int i=0; i<size; i++) {
        RealVector vec = MatrixUtils.createRealVector(new double[dim]);
        initializeVector(vec, initial, randomize, normalize);
        var.add(vec);
    }
    writeLock.lock();
    try {
        vectorVars.put(name, var);
    } finally {
        writeLock.unlock();
    }
}
 

/**
 * Correct the current state estimate with an actual measurement.
 *
 * @param z
 *            the measurement vector
 * @throws NullArgumentException
 *             if the measurement vector is {@code null}
 * @throws DimensionMismatchException
 *             if the dimension of the measurement vector does not fit
 * @throws SingularMatrixException
 *             if the covariance matrix could not be inverted
 */
public void correct(final RealVector z)
        throws NullArgumentException, DimensionMismatchException, SingularMatrixException {

    // sanity checks
    MathUtils.checkNotNull(z);
    if (z.getDimension() != measurementMatrix.getRowDimension()) {
        throw new DimensionMismatchException(z.getDimension(),
                                             measurementMatrix.getRowDimension());
    }

    // S = H * P(k) - * H' + R
    RealMatrix s = measurementMatrix.multiply(errorCovariance)
        .multiply(measurementMatrixT)
        .add(measurementModel.getMeasurementNoise());

    // invert S
    // as the error covariance matrix is a symmetric positive
    // semi-definite matrix, we can use the cholesky decomposition
    DecompositionSolver solver = new CholeskyDecomposition(s).getSolver();
    RealMatrix invertedS = solver.getInverse();

    // Inn = z(k) - H * xHat(k)-
    RealVector innovation = z.subtract(measurementMatrix.operate(stateEstimation));

    // calculate gain matrix
    // K(k) = P(k)- * H' * (H * P(k)- * H' + R)^-1
    // K(k) = P(k)- * H' * S^-1
    RealMatrix kalmanGain = errorCovariance.multiply(measurementMatrixT).multiply(invertedS);

    // update estimate with measurement z(k)
    // xHat(k) = xHat(k)- + K * Inn
    stateEstimation = stateEstimation.add(kalmanGain.operate(innovation));

    // update covariance of prediction error
    // P(k) = (I - K * H) * P(k)-
    RealMatrix identity = MatrixUtils.createRealIdentityMatrix(kalmanGain.getRowDimension());
    errorCovariance = identity.subtract(kalmanGain.multiply(measurementMatrix)).multiply(errorCovariance);
}
 

@Test
public void testComputeValueAndJacobian() {
    //setup
    final RealVector point = new ArrayRealVector(new double[]{1, 2});
    Evaluation evaluation = new LeastSquaresBuilder()
            .weight(new DiagonalMatrix(new double[]{16, 4}))
            .model(new MultivariateJacobianFunction() {
                public Pair<RealVector, RealMatrix> value(RealVector actualPoint) {
                    //verify correct values passed in
                    Assert.assertArrayEquals(
                            point.toArray(), actualPoint.toArray(), Precision.EPSILON);
                    //return values
                    return new Pair<RealVector, RealMatrix>(
                            new ArrayRealVector(new double[]{3, 4}),
                            MatrixUtils.createRealMatrix(new double[][]{{5, 6}, {7, 8}})
                    );
                }
            })
            .target(new double[2])
            .build()
            .evaluate(point);

    //action
    RealVector residuals = evaluation.getResiduals();
    RealMatrix jacobian = evaluation.getJacobian();

    //verify
    Assert.assertArrayEquals(evaluation.getPoint().toArray(), point.toArray(), 0);
    Assert.assertArrayEquals(new double[]{-12, -8}, residuals.toArray(), Precision.EPSILON);
    TestUtils.assertEquals(
            "jacobian",
            jacobian,
            MatrixUtils.createRealMatrix(new double[][]{{20, 24},{14, 16}}),
            Precision.EPSILON);
}
 

@Test
public void testMatricesValues() {
   BiDiagonalTransformer transformer =
        new BiDiagonalTransformer(MatrixUtils.createRealMatrix(testSquare));
   final double s17 = FastMath.sqrt(17.0);
    RealMatrix uRef = MatrixUtils.createRealMatrix(new double[][] {
            {  -8 / (5 * s17), 19 / (5 * s17) },
            { -19 / (5 * s17), -8 / (5 * s17) }
    });
    RealMatrix bRef = MatrixUtils.createRealMatrix(new double[][] {
            { -3 * s17 / 5, 32 * s17 / 85 },
            {      0.0,     -5 * s17 / 17 }
    });
    RealMatrix vRef = MatrixUtils.createRealMatrix(new double[][] {
            { 1.0,  0.0 },
            { 0.0, -1.0 }
    });

    // check values against known references
    RealMatrix u = transformer.getU();
    Assert.assertEquals(0, u.subtract(uRef).getNorm(), 1.0e-14);
    RealMatrix b = transformer.getB();
    Assert.assertEquals(0, b.subtract(bRef).getNorm(), 1.0e-14);
    RealMatrix v = transformer.getV();
    Assert.assertEquals(0, v.subtract(vRef).getNorm(), 1.0e-14);

    // check the same cached instance is returned the second time
    Assert.assertTrue(u == transformer.getU());
    Assert.assertTrue(b == transformer.getB());
    Assert.assertTrue(v == transformer.getV());

}
 

@Test
public void testMatricesValues() {
   BiDiagonalTransformer transformer =
        new BiDiagonalTransformer(MatrixUtils.createRealMatrix(testSquare));
   final double s17 = FastMath.sqrt(17.0);
    RealMatrix uRef = MatrixUtils.createRealMatrix(new double[][] {
            {  -8 / (5 * s17), 19 / (5 * s17) },
            { -19 / (5 * s17), -8 / (5 * s17) }
    });
    RealMatrix bRef = MatrixUtils.createRealMatrix(new double[][] {
            { -3 * s17 / 5, 32 * s17 / 85 },
            {      0.0,     -5 * s17 / 17 }
    });
    RealMatrix vRef = MatrixUtils.createRealMatrix(new double[][] {
            { 1.0,  0.0 },
            { 0.0, -1.0 }
    });

    // check values against known references
    RealMatrix u = transformer.getU();
    Assert.assertEquals(0, u.subtract(uRef).getNorm(), 1.0e-14);
    RealMatrix b = transformer.getB();
    Assert.assertEquals(0, b.subtract(bRef).getNorm(), 1.0e-14);
    RealMatrix v = transformer.getV();
    Assert.assertEquals(0, v.subtract(vRef).getNorm(), 1.0e-14);

    // check the same cached instance is returned the second time
    Assert.assertTrue(u == transformer.getU());
    Assert.assertTrue(b == transformer.getB());
    Assert.assertTrue(v == transformer.getV());

}
 

@Test
public void testSumWalks() {
    RealMatrix m = new Array2DRowRealMatrix(new double[][] {
        { 0.3, 0.0, 0.3 },
        { 0.1, 0.6, 0.7 },
        { 0.0, 0.4, 0.2 },
    });
    RealMatrix sum = MatrixUtils.createRealIdentityMatrix(3);
    for (int i = 0; i < 2000; i++) {
        sum = sum.add(m.power(i + 1));
    }
    RealVector ones = new ArrayRealVector(3, 1.0);
    // this is how I'm computing the reference (different from the methods implementation)
    double expectedSumWalks = sum.preMultiply(ones).dotProduct(ones);
    RealVector s = new ArrayRealVector(new double[] {1, 2, 3});
    RealVector t = new ArrayRealVector(new double[] {5, 7, 4});
    assertEquals(127.49999999999903, expectedSumWalks, tol);
    assertEquals(127.49999999999903, WeightedIntDiGraph.sumWalks(m, null, null), tol);
    assertEquals(127.49999999999903, WeightedIntDiGraph.sumWalks(m, null, ones), tol);
    assertEquals(127.49999999999903, WeightedIntDiGraph.sumWalks(m, ones, null), tol);

    assertTrue(TestUtils.checkThrows(() -> {
        WeightedIntDiGraph.sumWalks(
            new Array2DRowRealMatrix(
                    new double[][] {
                        { 0.3, 0.0, 0.3, 0 },
                        { 0.1, 0.6, 0.7, 0 },
                        { 0.0, 0.4, 0.2, 0 }}),
            null,
            null);
        }, InputMismatchException.class));                
    assertEquals(699.9999999999948, WeightedIntDiGraph.sumWalks(m, null, t), tol);
    assertEquals(274.99999999999795, WeightedIntDiGraph.sumWalks(m, s, null), tol);
    assertEquals(1509.9999999999886, WeightedIntDiGraph.sumWalks(m, s, t), tol);

}
 

@Test
public void testTransforms() {
	ms.translate(Vector3DUtil.ONE);
	ms.scale(Vector3DUtil.ONE.scalarMultiply(2));
	ms.pushMatrix();
	ms.rotate(Vector3D.PLUS_J, Math.PI / 2);
	assertThat(ms.apply(Vector3D.PLUS_K)).isAlmostEqualTo(new Vector3D(-1, 1, 1));

	ms.popMatrix();
	ms.transform(MatrixUtils.createRealMatrix(new Rotation(Vector3D.PLUS_J, Math.PI / 2).getMatrix()));
	assertThat(ms.apply(Vector3D.PLUS_K)).isAlmostEqualTo(new Vector3D(-1, 1, 1));

	assertThat(ms.apply(Vector3DUtil.ONE)).isAlmostEqualTo(ms.apply(Vector3DUtil.ONE));

}
 

/**
 * Rotates the current matrix
 *
 * @param rotation The rotation to aply
 * @return The rorated matrix
 */
public MatrixStack rotate(Rotation rotation) {
	RealMatrix rotMat = MatrixUtils.createRealMatrix(4, 4);
	rotMat.setSubMatrix(rotation.getMatrix(), 0, 0);
	rotMat.setEntry(3, 3, 1);
	current = current.preMultiply(rotMat);
	return this;
}
 

/**
 * 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 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<RealMatrix, RealVector>(normal, jTr);
}
 

/**
 * Initialization of the dynamic search parameters
 *
 * @param guess Initial guess for the arguments of the fitness function.
 */
private void initializeCMA(double[] guess) {
    if (lambda <= 0) {
        lambda = 4 + (int) (3. * Math.log(dimension));
    }
    // initialize sigma
    double[][] sigmaArray = new double[guess.length][1];
    for (int i = 0; i < guess.length; i++) {
        final double range =  (boundaries == null) ? 1.0 : boundaries[1][i] - boundaries[0][i];
        sigmaArray[i][0]   = ((inputSigma == null) ? 0.3 : inputSigma[i]) / range;
    }
    RealMatrix insigma = new Array2DRowRealMatrix(sigmaArray, false);
    sigma = max(insigma); // overall standard deviation

    // initialize termination criteria
    stopTolUpX = 1e3 * max(insigma);
    stopTolX = 1e-11 * max(insigma);
    stopTolFun = 1e-12;
    stopTolHistFun = 1e-13;

    // initialize selection strategy parameters
    mu = lambda / 2; // number of parents/points for recombination
    logMu2 = Math.log(mu + 0.5);
    weights = log(sequence(1, mu, 1)).scalarMultiply(-1.).scalarAdd(logMu2);
    double sumw = 0;
    double sumwq = 0;
    for (int i = 0; i < mu; i++) {
        double w = weights.getEntry(i, 0);
        sumw += w;
        sumwq += w * w;
    }
    weights = weights.scalarMultiply(1. / sumw);
    mueff = sumw * sumw / sumwq; // variance-effectiveness of sum w_i x_i

    // initialize dynamic strategy parameters and constants
    cc = (4. + mueff / dimension) /
            (dimension + 4. + 2. * mueff / dimension);
    cs = (mueff + 2.) / (dimension + mueff + 3.);
    damps = (1. + 2. * Math.max(0, Math.sqrt((mueff - 1.) /
            (dimension + 1.)) - 1.)) *
            Math.max(0.3, 1. - dimension /
                    (1e-6 + Math.min(maxIterations, getMaxEvaluations() /
                            lambda))) + cs; // minor increment
    ccov1 = 2. / ((dimension + 1.3) * (dimension + 1.3) + mueff);
    ccovmu = Math.min(1 - ccov1, 2. * (mueff - 2. + 1. / mueff) /
            ((dimension + 2.) * (dimension + 2.) + mueff));
    ccov1Sep = Math.min(1, ccov1 * (dimension + 1.5) / 3.);
    ccovmuSep = Math.min(1 - ccov1, ccovmu * (dimension + 1.5) / 3.);
    chiN = Math.sqrt(dimension) *
            (1. - 1. / (4. * dimension) + 1 / (21. * dimension * dimension));
    // intialize CMA internal values - updated each generation
    xmean = MatrixUtils.createColumnRealMatrix(guess); // objective
                                                       // variables
    diagD = insigma.scalarMultiply(1. / sigma);
    diagC = square(diagD);
    pc = zeros(dimension, 1); // evolution paths for C and sigma
    ps = zeros(dimension, 1); // B defines the coordinate system
    normps = ps.getFrobeniusNorm();

    B = eye(dimension, dimension);
    D = ones(dimension, 1); // diagonal D defines the scaling
    BD = times(B, repmat(diagD.transpose(), dimension, 1));
    C = B.multiply(diag(square(D)).multiply(B.transpose())); // covariance
    historySize = 10 + (int) (3. * 10. * dimension / lambda);
    fitnessHistory = new double[historySize]; // history of fitness values
    for (int i = 0; i < historySize; i++) {
        fitnessHistory[i] = Double.MAX_VALUE;
    }
}
 

/**
 * Initialization of the dynamic search parameters
 *
 * @param guess Initial guess for the arguments of the fitness function.
 */
private void initializeCMA(double[] guess) {
    if (lambda <= 0) {
        lambda = 4 + (int) (3. * Math.log(dimension));
    }
    // initialize sigma
    double[][] sigmaArray = new double[guess.length][1];
    for (int i = 0; i < guess.length; i++) {
        final double range =  (boundaries == null) ? 1.0 : boundaries[1][i] - boundaries[0][i];
        sigmaArray[i][0]   = ((inputSigma == null) ? 0.3 : inputSigma[i]) / range;
    }
    RealMatrix insigma = new Array2DRowRealMatrix(sigmaArray, false);
    sigma = max(insigma); // overall standard deviation

    // initialize termination criteria
    stopTolUpX = 1e3 * max(insigma);
    stopTolX = 1e-11 * max(insigma);
    stopTolFun = 1e-12;
    stopTolHistFun = 1e-13;

    // initialize selection strategy parameters
    mu = lambda / 2; // number of parents/points for recombination
    logMu2 = Math.log(mu + 0.5);
    weights = log(sequence(1, mu, 1)).scalarMultiply(-1.).scalarAdd(logMu2);
    double sumw = 0;
    double sumwq = 0;
    for (int i = 0; i < mu; i++) {
        double w = weights.getEntry(i, 0);
        sumw += w;
        sumwq += w * w;
    }
    weights = weights.scalarMultiply(1. / sumw);
    mueff = sumw * sumw / sumwq; // variance-effectiveness of sum w_i x_i

    // initialize dynamic strategy parameters and constants
    cc = (4. + mueff / dimension) /
            (dimension + 4. + 2. * mueff / dimension);
    cs = (mueff + 2.) / (dimension + mueff + 3.);
    damps = (1. + 2. * Math.max(0, Math.sqrt((mueff - 1.) /
            (dimension + 1.)) - 1.)) *
            Math.max(0.3, 1. - dimension /
                    (1e-6 + Math.min(maxIterations, getMaxEvaluations() /
                            lambda))) + cs; // minor increment
    ccov1 = 2. / ((dimension + 1.3) * (dimension + 1.3) + mueff);
    ccovmu = Math.min(1 - ccov1, 2. * (mueff - 2. + 1. / mueff) /
            ((dimension + 2.) * (dimension + 2.) + mueff));
    ccov1Sep = Math.min(1, ccov1 * (dimension + 1.5) / 3.);
    ccovmuSep = Math.min(1 - ccov1, ccovmu * (dimension + 1.5) / 3.);
    chiN = Math.sqrt(dimension) *
            (1. - 1. / (4. * dimension) + 1 / (21. * dimension * dimension));
    // intialize CMA internal values - updated each generation
    xmean = MatrixUtils.createColumnRealMatrix(guess); // objective
                                                       // variables
    diagD = insigma.scalarMultiply(1. / sigma);
    diagC = square(diagD);
    pc = zeros(dimension, 1); // evolution paths for C and sigma
    ps = zeros(dimension, 1); // B defines the coordinate system
    normps = ps.getFrobeniusNorm();

    B = eye(dimension, dimension);
    D = ones(dimension, 1); // diagonal D defines the scaling
    BD = times(B, repmat(diagD.transpose(), dimension, 1));
    C = B.multiply(diag(square(D)).multiply(B.transpose())); // covariance
    historySize = 10 + (int) (3. * 10. * dimension / lambda);
    fitnessHistory = new double[historySize]; // history of fitness values
    for (int i = 0; i < historySize; i++) {
        fitnessHistory[i] = Double.MAX_VALUE;
    }
}